update geely code.

src/common/common/math/gnss_coordinate_convert.cpp

@@ -1,5 +1,11 @@
 #include <math/gnss_coordinate_convert.h>
 
+
+#ifdef USE_UTM
+#include <GeographicLib/UTMUPS.hpp>
+#include <iostream>
+#endif
+
 void gps2xy(double J4, double K4, double *x, double *y)
 {
     int L4 = (int)((K4 - 1.5) / 3) + 1;
@@ -25,6 +31,19 @@ void gps2xy(double J4, double K4, double *x, double *y)
 //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
 void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 {
+
+#ifdef USE_UTM
+
+    int zone{};
+    bool northp{};
+    try {
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, *X,*Y);
+      zone = zone;
+    } catch (GeographicLib::GeographicErr& e) {
+  //    throw ForwardProjectionError(e.what());
+        std::cout<<"GeographicLib::UTMUPS::Forward Fail. what: "<<e.what()<<std::endl;
+    }
+#else
     int ProjNo = 0; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double a, f, e2, ee, NN, T, C, A, M, iPI;
@@ -55,6 +74,7 @@ void GaussProjCal(double longitude, double latitude, double *X, double *Y)
     xval = xval + X0; yval = yval + Y0;
     *X = xval;
     *Y = yval;
+#endif
 }
 
 
@@ -115,6 +135,19 @@ void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
 //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
 void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
 {
+
+#ifdef USE_UTM
+    int zone = 50;
+    bool isInNorthernHemisphere_ = true;
+    try {
+      GeographicLib::UTMUPS::Reverse(zone, isInNorthernHemisphere_, X,
+                                     Y, *latitude, *longitude);
+    } catch (GeographicLib::GeographicErr& e) {
+      std::cout<<"GeographicLib::UTMUPS::Reverse what:  "<<e.what()<<std::endl;
+    }
+
+#else
+
     int ProjNo; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
@@ -150,4 +183,6 @@ void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
     //转换为度 DD
     *longitude = longitude1 / iPI;
     *latitude = latitude1 / iPI;
+
+#endif
 }

src/common/common/nvenc/include/GLES3/gl32.h

@@ -295,7 +295,7 @@ double LaneSection::GetLeftRoadWidth(double s_check)
         pLane = &mLaneVector[i];
         if(pLane->GetId()>0)
         {
-            fwidth = fwidth + pLane->GetWidthValue(s_check);
+            fwidth = fwidth + pLane->GetWidthValue(s_check );
         }
     }
     return fwidth;
@@ -317,7 +317,7 @@ double LaneSection::GetRightRoadWidth(double s_check)
         pLane = &mLaneVector[i];
         if(pLane->GetId()<0)
         {
-            fwidth = fwidth + pLane->GetWidthValue(s_check);
+            fwidth = fwidth + pLane->GetWidthValue(s_check );
         }
     }
     return fwidth;
@@ -467,7 +467,7 @@ bool LaneSection::FillLaneSectionSample(double s_check, LaneSectionSample& laneS
 //Examp i= 1 return lane id = 1 lane
 Lane * LaneSection::GetLeftLaneAt(unsigned int index)
 {
-    int nlaneid = index;
+    int nlaneid = static_cast<int>(index) ;
     Lane * pRtn = NULL;
 
     unsigned int i;
@@ -483,10 +483,27 @@ Lane * LaneSection::GetLeftLaneAt(unsigned int index)
 
 }
 
+
+Lane * LaneSection::GetCenterLane()
+{
+    Lane * pRtn = NULL;
+
+    unsigned int i;
+    for(i=0;i<mLaneVector.size();i++)
+    {
+        if(mLaneVector.at(i).GetId() == 0)
+        {
+            pRtn = &mLaneVector.at(i);
+            break;
+        }
+    }
+    return pRtn;
+}
+
 //Examp i= 1 return lane id = -1 lane
 Lane * LaneSection::GetRightLaneAt(unsigned int index)
 {
-    int nlaneid = index;
+    int nlaneid = static_cast<int>(index) ;
     nlaneid = nlaneid *(-1);
     Lane * pRtn = NULL;
 
@@ -502,6 +519,7 @@ Lane * LaneSection::GetRightLaneAt(unsigned int index)
     return pRtn;
 }
 
+
 /**
 * Destructor. Delete all the members of the vectors: mLeft, mCenter, mRight
 */
@@ -528,6 +546,11 @@ string LaneSection::GetSingleSide()
 {
     return msingleSide;
 }
+
+
+
+
+
 /**
 * Lane Section Sample. Holds all the lane information at a certain S value including lane widths, levels, 
 * heights, etc
@@ -1386,6 +1409,17 @@ double  Lane::GetWidthValue(double s_check)
 	return retVal;
 }
 
+double  Lane::GetWidthValueGradient(double s_check)
+{
+    double retVal=0;
+    //find the record where s_check belongs
+    int index=CheckWidthInterval(s_check);
+    //If found, return the type
+    if (index>=0)
+        retVal= mLaneWidth.at(index).GetValueGradient(s_check);
+    return retVal;
+}
+
 /**
 *	Evaluate the record and return the height object
 */
@@ -1538,6 +1572,16 @@ void LaneRoadMark::SetWidth(double value)
 {	mWidth=value;	}
 void LaneRoadMark::SetLaneChange(string laneChange)
 {	mLaneChange=laneChange;	}
+void LaneRoadMark::SetLaneRoadMarkType(LaneRoadMarkType  laneRoadMarkType)
+{
+//    ResetLaneRoadMarkType();
+    std::vector<LaneRoadMarkType> * px = &mvectorLaneRoadMarkType;
+    mvectorLaneRoadMarkType.push_back(laneRoadMarkType);
+}
+void LaneRoadMark::ResetLaneRoadMarkType()
+{
+    mvectorLaneRoadMarkType.clear();
+}
 
 
 /*
@@ -1555,6 +1599,13 @@ double LaneRoadMark::GetWidth()
 {	return mWidth;	}
 string LaneRoadMark::GetLaneChange()
 {	return mLaneChange;	}
+int LaneRoadMark::GetLaneRoadMarkType(LaneRoadMarkType & laneRoadMarkType)
+{
+    if(mvectorLaneRoadMarkType.size() == 0)return 0;
+    laneRoadMarkType = mvectorLaneRoadMarkType[0];
+    return 1;
+}
+
 
 /**
 * Lane material class. Contains all the data that describes a lane material
@@ -1796,3 +1847,254 @@ void LaneOffset::Setc(double value)
 {   mc = value;}
 void LaneOffset::Setd(double value)
 {   md = value;}
+
+
+
+/**
+* Lane RoadMarkLine class. Contains all the data that describes lane roadmark line record
+*
+*
+*
+*
+*
+*/
+
+/*
+* Constructors
+*/
+LaneRoadMarkLine::LaneRoadMarkLine(double length, double space, double tOffset, double sOffset)
+{
+    mlength = length;
+    mspace = space;
+    mtOffset = tOffset;
+    msOffset = sOffset;
+}
+
+
+/*
+* Methods that return the parameters of the lane offset
+*/
+double LaneRoadMarkLine::Getlength()
+{
+    return mlength;
+}
+
+double LaneRoadMarkLine::Getspace()
+{
+    return mspace;
+}
+
+double LaneRoadMarkLine::GettOffset()
+{
+    return  mtOffset;
+}
+
+double LaneRoadMarkLine::GetsOffset()
+{
+    return msOffset;
+}
+
+int LaneRoadMarkLine::Getrule(std::string & rule)  //If return 1, have vaule, return 0 no this value
+{
+    if(mrule.size() == 0)return 0;
+    rule = mrule[0];
+    return 1;
+}
+
+int LaneRoadMarkLine::Getwidth(double & width) //same with rule
+{
+    if(mwidth.size() == 0)return 0;
+    width = mwidth[0];
+    return 1;
+}
+
+int LaneRoadMarkLine::Getcolor(std::string & color)   //same with rule
+{
+    if(mcolor.size() == 0)return 0;
+    color = mcolor[0];
+    return  1;
+}
+
+
+
+/*
+* Methods that set the parameters of the lane roadmark type
+*/
+
+
+LaneRoadMarkType::LaneRoadMarkType(std::string strname,double fwidth)
+{
+    mstrname = strname;
+    mfwidth = fwidth;
+}
+
+/*
+* Methods that return the parameters of the lane roadmark
+*/
+
+std::string LaneRoadMarkType::Getname()
+{
+    return mstrname;
+}
+
+double LaneRoadMarkType::GetWidth()
+{
+    return mfwidth;
+}
+
+/*
+* Methods that set the parameters of the lane roadmark
+*/
+
+void LaneRoadMarkType::Setname(std::string name)
+{
+    mstrname = name;
+}
+
+void LaneRoadMarkType::SetWidth(double width)
+{
+    mfwidth = width;
+}
+
+
+/**
+ * Methods used to add child records to the respective vectors
+ */
+unsigned int LaneRoadMarkType::AddLaneRoadMarkLine(double length, double space, double tOffset, double sOffset)
+{
+    mvetorLaneRoadMarkLine.push_back(LaneRoadMarkLine(length,space,tOffset,sOffset));
+    mLastAddedLaneRoadMarkLine = static_cast<unsigned int >(mvetorLaneRoadMarkLine.size())  -1;
+    return  mLastAddedLaneRoadMarkLine;
+}
+
+/**
+ * Methods used to clone child records in the respective vectors
+ */
+unsigned int LaneRoadMarkType::CloneLaneRoadMarkLine(unsigned int index)
+{
+    if(index<(mvetorLaneRoadMarkLine.size()-1))
+        mvetorLaneRoadMarkLine.insert(mvetorLaneRoadMarkLine.begin()+index+1, mvetorLaneRoadMarkLine[index]);
+    else if(index==mvetorLaneRoadMarkLine.size()-1)
+        mvetorLaneRoadMarkLine.push_back(mvetorLaneRoadMarkLine[index]);
+    mLastAddedLaneRoadMarkLine=index+1;
+    return mLastAddedLaneRoadMarkLine;
+}
+
+/**
+ * Methods used to delete child records from the respective vectors
+ */
+void LaneRoadMarkType::DeleteLaneRoadMarkLine(unsigned int index)
+{
+    mvetorLaneRoadMarkLine.erase(mvetorLaneRoadMarkLine.begin()+index);
+}
+
+
+/**
+*	Get pointers to the records vectors
+*/
+vector <LaneRoadMarkLine> * LaneRoadMarkType::GetLaneRoadMarkLineVector()
+{
+    return &mvetorLaneRoadMarkLine;
+}
+
+
+
+/**
+*	Get the number of elements in a certain vector
+*/
+unsigned int LaneRoadMarkType::GetLaneRoadMarkLineCount()
+{
+    return  static_cast<unsigned int >(mvetorLaneRoadMarkLine.size()) ;
+}
+
+
+/**
+*	Get the elements of a certain vectors at position i
+*/
+LaneRoadMarkLine* LaneRoadMarkType::GetLaneRoadMarkLine(unsigned int i)
+{
+    if ((mvetorLaneRoadMarkLine.size()>0)&&(i<mvetorLaneRoadMarkLine.size()))
+        return &mvetorLaneRoadMarkLine.at(i);
+    else
+        return NULL;
+}
+
+
+/**
+*	Get the last elements of a certain vectors
+*/
+LaneRoadMarkLine* LaneRoadMarkType::GetLastLaneRoadMarkLine()
+{
+    if (mvetorLaneRoadMarkLine.size()>0)
+        return &mvetorLaneRoadMarkLine.at(mvetorLaneRoadMarkLine.size()-1);
+    else
+        return NULL;
+}
+
+/**
+*	Get the last added elements of a certain vectors (their position might not be at the end of the vector)
+*/
+LaneRoadMarkLine* LaneRoadMarkType::GetLastAddedLaneRoadMarkLine()
+{
+    if(mLastAddedLaneRoadMarkLine<mvetorLaneRoadMarkLine.size())
+        return &mvetorLaneRoadMarkLine.at(mLastAddedLaneRoadMarkLine);
+    else
+        return NULL;
+}
+
+
+/*
+* Methods that set the parameters of the lane roadmark line
+*/
+void LaneRoadMarkLine::Setlength(double length)
+{
+    mlength = length;
+}
+
+void LaneRoadMarkLine::Setspace(double space)
+{
+    mspace = space;
+}
+
+void LaneRoadMarkLine::SettOffset(double tOffset)
+{
+    mtOffset = tOffset;
+}
+
+void LaneRoadMarkLine::SetsOffset(double sOffset)
+{
+    msOffset = sOffset;
+}
+
+void LaneRoadMarkLine::Setrule(std::string & rule)
+{
+    if(mrule.size()>0)mrule.clear();
+    mrule.push_back(rule);
+}
+
+void LaneRoadMarkLine::Setwidth(double & width)
+{
+    if(mwidth.size()>0)mwidth.clear();
+    mwidth.push_back(width);
+}
+
+void LaneRoadMarkLine::Setcolor(std::string & color)
+{
+    if(mcolor.size()>0)mcolor.clear();
+    mcolor.push_back(color);
+}
+
+void LaneRoadMarkLine::Resetrule()
+{
+    if(mrule.size()>0)mrule.clear();
+}
+
+void LaneRoadMarkLine::Resetwidth()
+{
+    if(mwidth.size()>0)mwidth.clear();
+}
+
+void LaneRoadMarkLine::Resetcolor()
+{
+    if(mcolor.size()>0)mcolor.clear();
+}

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

@@ -1,7 +1,7 @@
 #ifndef LANE_H
 #define LANE_H
 
-#include "Road.h"
+//#include "Road.h"
 #include "OtherStructures.h"
 #include <vector>
 #include <string>
@@ -12,6 +12,7 @@ class LaneSection;
 class LaneSectionSample;
 class Lane;
 class LaneWidth;
+class LaneRoadMarkLine;
 class LaneRoadMark;
 class LaneMaterial;
 class LaneVisibility;
@@ -19,6 +20,7 @@ class LaneSpeed;
 class LaneAccess;
 class LaneHeight;
 class LaneBorder;
+class LaneRoadMarkType;
 
 using std::vector;
 using std::string;
@@ -198,6 +200,10 @@ public:
 
     Lane * GetLeftLaneAt(unsigned int index);
     Lane * GetRightLaneAt(unsigned int index);
+    Lane * GetCenterLane();
+
+
+
 };
 
 
@@ -520,6 +526,11 @@ public:
 	*/
 	double GetWidthValue(double s_check);
 
+    /**
+    *	Evaluate the record and return the width value
+    */
+    double GetWidthValueGradient(double s_check);
+
 	/**
 	*	Evaluate the record and return the height object
 	*/
@@ -599,6 +610,9 @@ private:
 	string mColor; 
 	double mWidth;
 	string mLaneChange;
+
+    vector<LaneRoadMarkType> mvectorLaneRoadMarkType;
+
 public:
 	/*
 	* Constructors
@@ -617,6 +631,9 @@ public:
 	void SetWidth(double value);
 	void SetLaneChange(string laneChange);
 
+    void SetLaneRoadMarkType(LaneRoadMarkType  laneRoadMarkType);
+    void ResetLaneRoadMarkType();
+
 	/*
 	* Methods that return the parameters of the road mark
 	*/
@@ -626,6 +643,10 @@ public:
 	string GetColor();
 	double GetWidth();
 	string GetLaneChange();
+    int GetLaneRoadMarkType(LaneRoadMarkType & laneRoadMarkType);
+
+
+
 
 };
 
@@ -869,4 +890,136 @@ public:
     void Setd(double value);
 };
 
+//----------------------------------------------------------------------------------
+
+class LaneRoadMarkType
+{
+private:
+    /*
+    * Parameters that describe the lane roadmark type
+    */
+    std::string mstrname;
+    double mfwidth;
+
+    vector<LaneRoadMarkLine> mvetorLaneRoadMarkLine;
+
+    unsigned int mLastAddedLaneRoadMarkLine;
+
+public:
+    /*
+    * Constructors
+    */
+    LaneRoadMarkType(std::string strname,double fwidth);
+
+    /*
+    * Methods that return the parameters of the lane roadmark
+    */
+
+    std::string Getname();
+    double GetWidth();
+
+    /*
+    * Methods that set the parameters of the lane roadmark
+    */
+
+    void Setname(std::string name);
+    void SetWidth(double width);
+
+    /**
+     * Methods used to add child records to the respective vectors
+     */
+    unsigned int AddLaneRoadMarkLine(double length, double space, double tOffset, double sOffset);
+
+    /**
+     * Methods used to clone child records in the respective vectors
+     */
+    unsigned int CloneLaneRoadMarkLine(unsigned int index);
+
+    /**
+     * Methods used to delete child records from the respective vectors
+     */
+    void DeleteLaneRoadMarkLine(unsigned int index);
+
+
+    /**
+    *	Get pointers to the records vectors
+    */
+    vector <LaneRoadMarkLine> *GetLaneRoadMarkLineVector();
+
+
+
+    /**
+    *	Get the number of elements in a certain vector
+    */
+    unsigned int GetLaneRoadMarkLineCount();
+
+
+    /**
+    *	Get the elements of a certain vectors at position i
+    */
+    LaneRoadMarkLine* GetLaneRoadMarkLine(unsigned int i);
+
+
+    /**
+    *	Get the last elements of a certain vectors
+    */
+    LaneRoadMarkLine* GetLastLaneRoadMarkLine();
+
+    /**
+    *	Get the last added elements of a certain vectors (their position might not be at the end of the vector)
+    */
+    LaneRoadMarkLine* GetLastAddedLaneRoadMarkLine();
+
+
+};
+
+//----------------------------------------------------------------------------------
+
+class LaneRoadMarkLine
+{
+private:
+    /*
+    * Parameters that describe the lane roadmark line
+    */
+    double mlength;
+    double mspace;
+    double mtOffset;
+    double msOffset;
+    std::vector<std::string> mrule;
+    std::vector<double> mwidth;
+    std::vector<std::string> mcolor;
+public:
+    /*
+    * Constructors
+    */
+    LaneRoadMarkLine(double length, double space, double tOffset, double sOffset);
+
+
+    /*
+    * Methods that return the parameters of the lane roadmark
+    */
+    double Getlength();
+    double Getspace();
+    double GettOffset();
+    double GetsOffset();
+    int Getrule(std::string & rule);  //If return 1, have vaule, return 0 no this value
+    int Getwidth(double & width); //same with rule
+    int Getcolor(std::string & color);    //same with rule
+
+
+    /*
+    * Methods that set the parameters of the lane roadmark
+    */
+    void Setlength(double length);
+    void Setspace(double space);
+    void SettOffset(double tOffset);
+    void SetsOffset(double sOffset);
+    void Setrule(std::string & rule);
+    void Setwidth(double & width);
+    void Setcolor(std::string & color);
+    void Resetrule();
+    void Resetwidth();
+    void Resetcolor();
+};
+
 #endif

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

@@ -8,6 +8,224 @@ using std::vector;
 using std::string;
 
 
+class Object_markings_marking_cornerReference;
+class Object_laneValidity;
+
+
+class Objects_objectReference
+{
+private:
+    double mS;
+    double mt;
+    std::string mstrid;
+    std::vector<double> mzOffset;
+    std::vector<double> mvalidLength;
+    std::string morientation;
+
+    vector<Object_laneValidity> mObject_laneValidity;
+
+    unsigned int mLastAddedObjectlaneValidity;
+
+public:
+    Objects_objectReference(double s,double t,std::string id,std::string orientation);
+
+    double GetS();
+    double Gett();
+    std::string Getid();
+    std::string Getorientation();
+
+    void SetS(double s);
+    void Sett(double t);
+    void Setid(std::string id);
+    void Setorientation(std::string orientation);
+
+    int GetzOffset(double & zOffset);
+    int GetvalidLength(double & validLength);
+
+    void SetzOffset(double zOffset);
+    void SetvalidLength(double validLength);
+
+    void ResetzOffset();
+    void ResetvalidLength();
+
+    vector<Object_laneValidity> * GetObjectlaneValidityVector();
+    Object_laneValidity* GetObjectlaneValidity(unsigned int i);
+    unsigned int GetObjectlaneValidityCount();
+    Object_laneValidity*			GetLastObjectlaneValidity();
+    Object_laneValidity*			GetLastAddedObjectlaneValidity();
+    unsigned int AddObjectlaneValidity(int fromLane,int toLane);
+    unsigned int CloneObjectlaneValidity(unsigned int index);
+    void DeleteObjectlaneValidity(unsigned int index);
+
+    bool CheckInterval(double s_check);
+
+};
+
+class Objects_tunnel
+{
+private:
+    double mS;
+    double mlength;
+    std::vector<std::string> mname;
+    std::string mstrid;
+    std::string mstrtype;
+
+    std::vector<int> mvectorlighting;
+    std::vector<int> mvectordaylight;
+
+    vector<Object_laneValidity> mObject_laneValidity;
+
+    unsigned int mLastAddedObjectlaneValidity;
+
+public:
+    Objects_tunnel(double s,double length,std::string strid,std::string strtype);
+    double GetS();
+    double Getlength();
+    int GetName(std::string name);
+    std::string Getid();
+    std::string Gettype();
+
+
+    void SetS(double s);
+    void Setlength(double length);
+    void Setname(std::string name);
+    void Resetname();
+    void Setid(std::string id);
+    void Settype(std::string type);
+
+    int Getlighting(int & lighting);
+    int Getdaylight(int & daylight);
+    void Setlighting(int lighting);
+    void Setdaylight(int daylight);
+    void Resetlighting();
+    void Resetdaylight();
+
+    vector<Object_laneValidity> * GetObjectlaneValidityVector();
+    Object_laneValidity* GetObjectlaneValidity(unsigned int i);
+    unsigned int GetObjectlaneValidityCount();
+    Object_laneValidity*			GetLastObjectlaneValidity();
+    Object_laneValidity*			GetLastAddedObjectlaneValidity();
+    unsigned int AddObjectlaneValidity(int fromLane,int toLane);
+    unsigned int CloneObjectlaneValidity(unsigned int index);
+    void DeleteObjectlaneValidity(unsigned int index);
+
+    bool CheckInterval(double s_check);
+
+};
+
+class Objects_bridge
+{
+private:
+    double mS;
+    double mlength;
+    std::vector<std::string> mname;
+    std::string mstrid;
+    std::string mstrtype;
+
+    vector<Object_laneValidity> mObject_laneValidity;
+
+    unsigned int mLastAddedObjectlaneValidity;
+
+public:
+    Objects_bridge(double s,double length,std::string strid,std::string strtype);
+    double GetS();
+    double Getlength();
+    int GetName(std::string name);
+    std::string Getid();
+    std::string Gettype();
+
+    void SetS(double s);
+    void Setlength(double length);
+    void Setname(std::string name);
+    void Resetname();
+    void Setid(std::string id);
+    void Settype(std::string type);
+
+    vector<Object_laneValidity> * GetObjectlaneValidityVector();
+    Object_laneValidity* GetObjectlaneValidity(unsigned int i);
+    unsigned int GetObjectlaneValidityCount();
+    Object_laneValidity*			GetLastObjectlaneValidity();
+    Object_laneValidity*			GetLastAddedObjectlaneValidity();
+    unsigned int AddObjectlaneValidity(int fromLane,int toLane);
+    unsigned int CloneObjectlaneValidity(unsigned int index);
+    void DeleteObjectlaneValidity(unsigned int index);
+
+    bool CheckInterval(double s_check);
+};
+
+class Object_laneValidity
+{
+private:
+    int mfromLane;
+    int mtoLane;
+
+public:
+   Object_laneValidity(int fromLane,int toLane);
+   int GetfromLane();
+   int GettoLane();
+
+   void SetfromLane(int fromLane);
+   void SettoLane(int toLane);
+
+};
+
+class Object_borders_border
+{
+private:
+    double mwidth;
+    std::string mborderType;
+    int moutlineid;
+    std::vector<bool> muserCompleteOutline;
+
+    vector<Object_markings_marking_cornerReference> mcornerReference;
+
+
+private:
+    unsigned int mnLastAddedcornerReference = 0;
+
+public:
+    Object_borders_border(double width,std::string borderType,int outlineid);
+
+    int GetuserCompleteOutline(bool & userCompleteOutline);
+    void SetuserCompleteOutline(bool userCompleteOutline);
+    void ResetuserCompleteOutline();
+
+    void Setwidth(double width);
+    void SetborderType(std::string borderType);
+    void Setoutlineid(int outlineid);
+
+    double Getwidth();
+    std::string GetborderType();
+    int Getoutlineid();
+
+    vector<Object_markings_marking_cornerReference> * GetcornerReferenceVector();
+    Object_markings_marking_cornerReference * GetcornerReference(unsigned int i);
+    unsigned int GetcornerReferenceCount();
+    Object_markings_marking_cornerReference *			GetLastcornerReference();
+    unsigned int AddcornerReference(unsigned int id);
+    unsigned int ClonecornerReference(unsigned int index);
+    void DeletecornerReference(unsigned int index);
+};
+
+class Object_borders
+{
+private:
+    std::vector<Object_borders_border> mvectorborder;
+    unsigned int mnLastAddedBorders;
+public:
+    Object_borders(Object_borders_border xborder);
+    Object_borders();
+
+    vector<Object_borders_border> * GetBorderVector();
+    Object_borders_border* GetBorder(unsigned int i);
+    unsigned int GetBorderCount();
+    Object_borders_border*			GetLastBorder();
+    unsigned int AddBorder(double width,std::string borderType,int outlineid);
+    unsigned int CloneBorder(unsigned int index);
+    void DeleteBorder(unsigned int index);
+
+};
+
 class Object_markings_marking_cornerReference
 {
 private:
@@ -384,10 +602,13 @@ private:
     vector<Object_outlines> mObject_outlines;
     vector<Object_outlines_outline> mObject_outlines_outline;
     vector<Object_markings> mObject_markings;
+    vector<Object_borders> mObject_borders;
+    vector<Object_laneValidity> mObject_laneValidity;
 
     unsigned int mLastAddedObjectrepeat;
     unsigned int mLastAddedObjectmaterial;
     unsigned int mLastAddedObjectoutline;
+    unsigned int mLastAddedObjectlaneValidity;
 
 public:
 
@@ -415,6 +636,7 @@ public:
 
     Object_outlines * Getoutlines();
     Object_markings * Getmarkings();
+    Object_borders * Getborders();
 
     void Sett(double t);
     void SetzOffset(double zOffset);
@@ -443,6 +665,9 @@ public:
     void Setmarkings(Object_markings & xObject_markings);
     void Resetmarkings();
 
+    void Setborders(Object_borders & xObject_borders);
+    void Resetborders();
+
     vector<Object_repeat> * GetObjectrepeatVector();
     Object_repeat* GetObjectrepeat(unsigned int i);
     unsigned int GetObjectrepeatCount();
@@ -472,6 +697,15 @@ public:
     unsigned int CloneObjectoutline(unsigned int index);
     void DeleteObjectoutline(unsigned int index);
 
+    vector<Object_laneValidity> * GetObjectlaneValidityVector();
+    Object_laneValidity* GetObjectlaneValidity(unsigned int i);
+    unsigned int GetObjectlaneValidityCount();
+    Object_laneValidity*			GetLastObjectlaneValidity();
+    Object_laneValidity*			GetLastAddedObjectlaneValidity();
+    unsigned int AddObjectlaneValidity(int fromLane,int toLane);
+    unsigned int CloneObjectlaneValidity(unsigned int index);
+    void DeleteObjectlaneValidity(unsigned int index);
+
 };
 //----------------------------------------------------------------------------------
 
@@ -480,26 +714,31 @@ public:
 class signal_positionRoad
 {
 private:
+    std::string mroadid;
     double ms;
     double mt;
     double mzOffset;
     double mhOffset;
-    double mpitch;
-    double mroll;
+    std::vector<double> mpitch;
+    std::vector<double> mroll;
 public:
-    signal_positionRoad(double s,double t,double zOffset,double hOffset, double pitch,double roll);
+    signal_positionRoad(std::string roadid, double s,double t,double zOffset,double hOffset);
+    std::string Getroadid();
     double Gets();
     double Gett();
     double GetzOffset();
     double GethOffset();
-    double Getpitch();
-    double Getroll();
+    int Getpitch(double & pitch);
+    int Getroll(double & roll);
+    void Setroadid(std::string roadid);
     void Sets(double s);
     void Sett(double t);
     void SetzOffset(double zOffset);
     void SethOffset(double hOffset);
     void Setpitch(double pitch);
     void Setroll(double roll);
+    void Resetpitch();
+    void Resetroll();
 };
 
 class signal_positionInertial
@@ -509,22 +748,24 @@ private:
     double my;
     double mz;
     double mhdg;
-    double mpitch;
-    double mroll;
+    std::vector<double> mpitch;
+    std::vector<double> mroll;
 public:
-    signal_positionInertial(double x,double y,double z,double hdg,double pitch,double roll );
+    signal_positionInertial(double x,double y,double z,double hdg );
     double Getx();
     double Gety();
     double Getz();
     double Gethdg();
-    double Getpitch();
-    double Getroll();
+    int Getpitch(double & pitch);
+    int Getroll(double & roll);
     void Setx(double x);
     void Sety(double y);
     void Setz(double z);
     void Sethdg(double hdg);
     void Setpitch(double pitch);
     void Setroll(double roll);
+    void Resetpitch();
+    void Resetroll();
 };
 
 class signal_laneValidity
@@ -540,6 +781,43 @@ public:
     void SettoLane(int toLane);
 };
 
+class signal_dependency
+{
+private:
+    std::string mstrid;
+    std::vector<std::string> mtype;
+
+public:
+    signal_dependency(std::string id);
+
+    std::string Getid();
+    void Setid(std::string strid);
+
+    int Gettype(std::string & strtype);
+    void Settype(std::string strtype);
+    void Resettype();
+};
+
+class signal_reference
+{
+private:
+    std::string melementType;
+    std::string melementId;
+    std::vector<std::string> mtype;
+
+public:
+    signal_reference(std::string elementType,std::string elementId);
+
+    std::string GetelementType();
+    std::string GetelementId();
+
+    void SetelementType(std::string elementType);
+    void SetelementId(std::string elementId);
+
+    void Settype(std::string type);
+    void Resettype();
+    int Gettype(std::string & type);
+};
 
 //***********************************************************************************
 //Signal
@@ -566,6 +844,16 @@ private:
     signal_positionRoad * mpsignal_positionRoad;
     signal_positionInertial * mpsignal_positionInertial;
     signal_laneValidity * mpsignal_laneValidity;
+
+    std::vector<signal_laneValidity> mlaneValidity;
+    unsigned int mLastAddedlaneValidity;
+
+    std::vector<signal_dependency> mdependency;
+    unsigned int mLastAddedDependency;
+
+    std::vector<signal_reference> mreference;
+    unsigned int mLastAddedReference;
+
 public:
     Signal(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);
@@ -609,12 +897,79 @@ public:
     void Setheight(double height);
     void Setwidth(double width);
     void SetlaneValidity(int fromLane, int toLane);
-    void SetpositionRoad(double s,double t, double zOffset,double hOffset,double pitch,double roll);
-    void SetpositionInertial(double x,double y, double z, double hdg,double pitch,double roll);
+    void SetpositionRoad(std::string roadid, double s,double t, double zOffset,double hOffset);
+    void SetpositionInertial(double x,double y, double z, double hdg);
+
+    vector<signal_laneValidity> * GetlaneValidityVector();
+    signal_laneValidity* GetlaneValidity(unsigned int i);
+    unsigned int GetlaneValidityCount();
+    signal_laneValidity*			GetLastlaneValidity();
+    signal_laneValidity*			GetLastAddedlaneValidity();
+    unsigned int AddlaneValidity(int fromLane, int toLane);
+    unsigned int ClonelaneValidity(unsigned int index);
+    void DeletelaneValidity(unsigned int index);
+
+    vector<signal_dependency> * GetDependencyVector();
+    signal_dependency* GetDependency(unsigned int i);
+    unsigned int GetDependencyCount();
+    signal_dependency*			GetLastDependency();
+    signal_dependency*			GetLastAddedDependency();
+    unsigned int AddDependency(std::string strid);
+    unsigned int CloneDependency(unsigned int index);
+    void DeleteDependency(unsigned int index);
+
+    vector<signal_reference> * GetReferenceVector();
+    signal_reference* GetReference(unsigned int i);
+    unsigned int GetReferenceCount();
+    signal_reference*			GetLastReference();
+    signal_reference*			GetLastAddedReference();
+    unsigned int AddReference(std::string elementType,std::string elementId);
+    unsigned int CloneReference(unsigned int index);
+    void DeleteReference(unsigned int index);
 
+    bool CheckInterval(double s_check);
 
 };
 //----------------------------------------------------------------------------------
 
+//***********************************************************************************
+//Signal Reference
+//***********************************************************************************
+
+class signals_signalReference
+{
+private:
+    double ms;
+    double mt;
+    std::string mid;
+    std::string morientation;
+
+    std::vector<signal_laneValidity> mlaneValidity;
+    unsigned int mLastAddedlaneValidity;
+
+public:
+    signals_signalReference(double s, double t, std::string id, std::string orientation);
+
+    double Gets();
+    double Gett();
+    std::string Getid();
+    std::string Getorientation();
+
+    void Sets(double s);
+    void Sett(double t);
+    void Setid(std::string id);
+    void Setorientation(std::string orientation);
+
+    vector<signal_laneValidity> * GetlaneValidityVector();
+    signal_laneValidity* GetlaneValidity(unsigned int i);
+    unsigned int GetlaneValidityCount();
+    signal_laneValidity*			GetLastlaneValidity();
+    signal_laneValidity*			GetLastAddedlaneValidity();
+    unsigned int AddlaneValidity(int fromLane, int toLane);
+    unsigned int ClonelaneValidity(unsigned int index);
+    void DeletelaneValidity(unsigned int index);
+
+    bool CheckInterval(double s_check);
+};
 
 #endif

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

@@ -61,6 +61,15 @@ unsigned int OpenDrive::AddJunction(string name, string id)
 	mLastAddedJunction=index;
 	return index;
 }
+unsigned int OpenDrive::AddController(string id)
+{
+    unsigned int index=GetControllerCount();
+    // Adds the new controller to the end of the vector
+    mControllerVector.push_back(Controller(id));
+    // Saves the index of the newly added controller
+    mLastAddedController=index;
+    return index;
+}
 
 /**
  * Methods used to delete records from the respective vectors
@@ -73,6 +82,11 @@ void OpenDrive::DeleteJunction(unsigned int index)
 {
 	mJunctionVector.erase(mJunctionVector.begin()+index);
 }
+void OpenDrive::DeleteController(unsigned int index)
+{
+    mControllerVector.erase(mControllerVector.begin()+index);
+}
+
 
 //-------------------------------------------------
 
@@ -93,6 +107,13 @@ Junction* OpenDrive::GetLastJunction()
 	else
 		return NULL;
 }
+Controller* OpenDrive::GetLastController()
+{
+    if (mControllerVector.size()>0)
+        return &(mControllerVector.at(mControllerVector.size()-1));
+    else
+        return NULL;
+}
 
 /**
  * Getters for the last added records in their respective vectors
@@ -105,6 +126,14 @@ Road* OpenDrive::GetLastAddedRoad()
 		return NULL;
 }
 
+Controller* OpenDrive::GetLastAddedController()
+{
+    if(mLastAddedController<mControllerVector.size())
+        return &mControllerVector.at(mLastAddedController);
+    else
+        return NULL;
+}
+
 /**
  * Getter for the OpenDrive header
  */
@@ -147,6 +176,22 @@ unsigned int OpenDrive::GetJunctionCount()
 {	
 	return mJunctionVector.size();	
 }
+//Controller records
+vector<Controller> * OpenDrive::GetControllerVector()
+{
+    return &mControllerVector;
+}
+Controller* OpenDrive::GetController(unsigned int i)
+{
+    if (i < mControllerVector.size())
+            return &(mControllerVector.at(i));
+        else
+            return NULL;
+}
+unsigned int OpenDrive::GetControllerCount()
+{
+    return  static_cast<unsigned int >(mControllerVector.size());
+}
 //-------------------------------------------------
 
 /**
@@ -156,6 +201,7 @@ void OpenDrive::Clear()
 {
 	mRoadVector.clear();
 	mJunctionVector.clear();
+    mControllerVector.clear();
 }
 
 OpenDrive::OpenDrive (const OpenDrive& openDrive)

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

@@ -5,6 +5,7 @@
 #include <string>
 
 #include "Road.h"
+#include "controller.h"
 //--Prototypes--
 //main
 class Header;
@@ -35,12 +36,14 @@ private:
 	 */
 	vector<Road> mRoadVector;
 	vector<Junction> mJunctionVector;
+    vector<Controller> mControllerVector;
 	
 	/**
 	 * Indices of the last added records
 	 */
 	unsigned int mLastAddedRoad;
 	unsigned int mLastAddedJunction;
+    unsigned int mLastAddedController;
 
 //	//-------------------------------------------------
 
@@ -77,12 +80,14 @@ public:
 	 */
 	unsigned int AddRoad(string name, double length, string id, string junction);
 	unsigned int AddJunction(string name, string id);
+    unsigned int AddController(string id);
 
 	/**
 	 * Methods used to delete records from the respective vectors
 	 */
 	void DeleteRoad(unsigned int index);
 	void DeleteJunction(unsigned int index);
+    void DeleteController(unsigned int index);
 
 	//-------------------------------------------------
 
@@ -91,11 +96,13 @@ public:
 	 */
 	Road* GetLastRoad();
 	Junction* GetLastJunction();
+    Controller* GetLastController();
 
 	/**
 	 * Getters for the last added records in their respective vectors
 	 */
 	Road* GetLastAddedRoad();
+    Controller* GetLastAddedController();
 
 	/**
 	 * Getter for the OpenDrive header
@@ -113,6 +120,10 @@ public:
 	vector<Junction> * GetJunctionVector();
 	Junction* GetJunction(unsigned int i);
 	unsigned int GetJunctionCount();
+    //Controller records
+    vector<Controller> * GetControllerVector();
+    Controller* GetController(unsigned int i);
+    unsigned int GetControllerCount();
 	
 	//-------------------------------------------------
 

src/common/common/xodr/OpenDrive/OpenDrive.pri

@@ -8,6 +8,7 @@ HEADERS += \
     $$PWD/OtherStructures.h \
     $$PWD/Road.h \
     $$PWD/RoadGeometry.h \
+    $$PWD/controller.h \
     $$PWD/userData.h
 
 SOURCES += \
@@ -20,4 +21,5 @@ SOURCES += \
     $$PWD/OtherStructures.cpp \
     $$PWD/Road.cpp \
     $$PWD/RoadGeometry.cpp \
+    $$PWD/controller.cpp \
     $$PWD/userData.cpp

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

@@ -57,6 +57,8 @@ public:
 	bool ReadLane (LaneSection* laneSection, TiXmlElement *node, short int laneType);
 	bool ReadLaneWidth(Lane* lane, TiXmlElement *node);
 	bool ReadLaneRoadMark(Lane* lane, TiXmlElement *node);
+    bool ReadLaneRoadMarkType(LaneRoadMark * planeRoadMark,TiXmlElement *node);
+    bool ReadLaneRoadMarkTypeLine(LaneRoadMarkType * planeRoadMarkType, TiXmlElement *node);
 	bool ReadLaneMaterial(Lane* lane, TiXmlElement *node);
 	bool ReadLaneVisibility(Lane* lane, TiXmlElement *node);
 	bool ReadLaneSpeed(Lane* lane, TiXmlElement *node);
@@ -72,6 +74,9 @@ public:
 
     bool ReadRoadNoavoids(Road * road, TiXmlElement *node);
     bool ReadRoadNoavoid(Road * road,TiXmlElement * node);
+
+    bool ReadRoadPriority(Road * road,TiXmlElement * node);
+
 	//--------------
 
 	bool ReadObjects (Road* road, TiXmlElement *node);
@@ -84,20 +89,42 @@ public:
     bool ReadObjectOutlinesOutlinecornerRoad(Object_outlines_outline * pObject_outline,TiXmlElement * node);
     bool ReadObjectOutlinesOutlinecornerLocal(Object_outlines_outline * pObject_outline,TiXmlElement * node);
 	bool ReadSignals (Road* road, TiXmlElement *node);
+    bool ReadSignals_signalReference(Road * road,TiXmlElement * node);
+    bool ReadSignals_signalReference_laneValidity(signals_signalReference * pSignals_signalReference,TiXmlElement * node);
     bool ReadSignal(Road * road,TiXmlElement * node);
+    bool ReadSignal_positionRoad(Signal * pSignal,TiXmlElement * node);
     bool ReadSignal_positionInertial(Signal * pSignal, TiXmlElement *node);
     bool ReadSignal_laneValidity(Signal * pSignal,TiXmlElement * node);
+    bool ReadSignal_dependency(Signal * pSignal,TiXmlElement * node);
+    bool ReadSignal_reference(Signal * pSignal,TiXmlElement * node);
+    bool ReadObjectsBridge(Road * road,TiXmlElement * node);
+    bool ReadObjectsTunnel(Road * road,TiXmlElement * node);
+    bool ReadObjectsObjectReference(Road * road,TiXmlElement * node);
     bool ReadObject(Road * road,TiXmlElement * node);
     bool ReadObjectMarkings(Object * pObject,TiXmlElement * node);
     bool ReadObjectMarkingsMarking(Object_markings * pObject_Markings,TiXmlElement * node);
     bool ReadObjectMarkingsMarkingcornerReference(Object_markings_marking * pObject_Marking,TiXmlElement * node);
+    bool ReadObjectBorders(Object * pObject,TiXmlElement * node);
+    bool ReadObjectBordersBorder(Object_borders * pObject_Borders,TiXmlElement * node);
+    bool ReadObjectBordersBordercornerReference(Object_borders_border * pObject_Border,TiXmlElement * node);
+    bool ReadObjectvalidity(Object * pObject,TiXmlElement * node);
+    bool ReadBridgevalidity(Objects_bridge * pBridge,TiXmlElement * node);
+    bool ReadTunnelvalidity(Objects_tunnel * pTunnel,TiXmlElement * node);
+    bool ReadObjectReferencevalidity(Objects_objectReference * pObjectReference,TiXmlElement * node);
 
 	//--------------
 
 	bool ReadSurface (Road* road, TiXmlElement *node);
+    bool ReadSurfaceCRG(Road* road,TiXmlElement *node);
 	//--------------
 
+    bool ReadRailroad(Road* road, TiXmlElement *node);
+    bool ReadRailroadSwitch(Road* road, TiXmlElement *node);
+
+    //--------------
+
 	bool ReadController (TiXmlElement *node);
+    bool ReadController_Control(Controller * pController,TiXmlElement * node);
 	//--------------
 
 	bool ReadJunction (TiXmlElement *node);
@@ -108,6 +135,8 @@ public:
 	bool ReadJunctionPriority (Junction* junction, TiXmlElement *node);
 	bool ReadJunctionController (Junction* junction, TiXmlElement *node);
 	//--------------
+
+    bool ParseGeoReferenceLon0Lat0(std::string strgr, double & lon0,double & lat0);
 };
 
 

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

@@ -184,19 +184,34 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
         WriteRoadBorrows(nodeRoad,road);
     }
 
+    int nPriority;
+    if(road->GetRoadPriority(nPriority) == 1)
+    {
+        WriteRoadPriority(nodeRoad,road);
+    }
+
     if(road->GetRoadNoavoidCount()>0)
     {
         WriteRoadNoavoids(nodeRoad,road);
     }
 
-	/*
+    if(road->GetSurfaceCRGCount()>0)
+    {
+        WriteSurface(nodeRoad,road);
+    }
+
+    if(road->GetRailroadSwitchCount())
+    {
+        WriteRailroad(node,road);
+    }
+
 	//Proceed to Surface
-	subNode=node->FirstChildElement("surface");
-	if (subNode)
-	{
-	WriteSurface(road, subNode);
-	}
-	*/
+//	subNode=node->FirstChildElement("surface");
+//	if (subNode)
+//	{
+//	WriteSurface(road, subNode);
+//	}
+
 
 	return true;
 }
@@ -214,7 +229,8 @@ bool  OpenDriveXmlWriter::WriteRoadLinks (TiXmlElement *node, Road* road)
 		nodeLink->LinkEndChild(nodeLinkPredecessor);
 		nodeLinkPredecessor->SetAttribute("elementType", lPredecessor->GetElementType());
 		nodeLinkPredecessor->SetAttribute("elementId", lPredecessor->GetElementId());
-		nodeLinkPredecessor->SetAttribute("contactPoint", lPredecessor->GetContactPoint());
+        if(lPredecessor->GetContactPoint() != "NA")
+            nodeLinkPredecessor->SetAttribute("contactPoint", lPredecessor->GetContactPoint());
         if(lPredecessor->GetElementS()>=0)
         {
             std::stringstream ss;
@@ -233,7 +249,8 @@ bool  OpenDriveXmlWriter::WriteRoadLinks (TiXmlElement *node, Road* road)
 		nodeLink->LinkEndChild(nodeLinkSuccessor);
 		nodeLinkSuccessor->SetAttribute("elementType", lSuccessor->GetElementType());
 		nodeLinkSuccessor->SetAttribute("elementId", lSuccessor->GetElementId());
-		nodeLinkSuccessor->SetAttribute("contactPoint", lSuccessor->GetContactPoint());
+        if(lSuccessor->GetContactPoint() != "NA")
+            nodeLinkSuccessor->SetAttribute("contactPoint", lSuccessor->GetContactPoint());
         if(lSuccessor->GetElementS()>=0)
         {
             std::stringstream ss;
@@ -567,7 +584,7 @@ bool OpenDriveXmlWriter::WriteElevationProfile (TiXmlElement *node, Road* road)
 
 bool OpenDriveXmlWriter::WriteLateralProfile (TiXmlElement *node, Road* road)
 {
-	double s, a, b, c, d;
+    double s, t, a, b, c, d;
 	string side;
 
 	TiXmlElement* nodeLateralProfile = new TiXmlElement("lateralProfile");
@@ -607,6 +624,45 @@ bool OpenDriveXmlWriter::WriteLateralProfile (TiXmlElement *node, Road* road)
 		nodeSuperElevation->SetAttribute("d",sd.str());
 	}
 
+    unsigned int lShapeCount = road->GetShapeCount();
+    for(unsigned int i=0; i<lShapeCount; i++)
+    {
+        Shape *lShape = road->GetShape(i);
+        s=lShape->GetS();
+        t=lShape->GetT();
+        a=lShape->GetA();
+        b=lShape->GetB();
+        c=lShape->GetC();
+        d=lShape->GetD();
+
+        TiXmlElement *nodeShape = new TiXmlElement("shape");
+        nodeLateralProfile->LinkEndChild(nodeShape);
+
+        std::stringstream ss;
+        ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
+        nodeShape->SetAttribute("s",ss.str());
+
+        std::stringstream st;
+        st << setprecision(16) << setiosflags (ios_base::scientific) << t;
+        nodeShape->SetAttribute("t",ss.str());
+
+        std::stringstream sa;
+        sa << setprecision(16) << setiosflags (ios_base::scientific) << a;
+        nodeShape->SetAttribute("a",sa.str());
+
+        std::stringstream sb;
+        sb << setprecision(16) << setiosflags (ios_base::scientific) << b;
+        nodeShape->SetAttribute("b",sb.str());
+
+        std::stringstream sc;
+        sc << setprecision(16) << setiosflags (ios_base::scientific) << c;
+        nodeShape->SetAttribute("c",sc.str());
+
+        std::stringstream sd;
+        sd << setprecision(16) << setiosflags (ios_base::scientific) << d;
+        nodeShape->SetAttribute("d",sd.str());
+    }
+
 
 	unsigned int lCrossfallCount = road->GetCrossfallCount();
 	for(unsigned int i=0; i<lCrossfallCount; i++)
@@ -992,10 +1048,88 @@ bool OpenDriveXmlWriter::WriteLaneRoadMark(TiXmlElement *node, LaneRoadMark* lan
 	nodeLaneRoadMark->SetAttribute("width",swidth.str());
 	nodeLaneRoadMark->SetAttribute("laneChange",laneChange);
 
+    LaneRoadMarkType laneRoadMarkType("broken",0.15);
+    if(laneRoadMark->GetLaneRoadMarkType(laneRoadMarkType) == 1)
+    {
+        WriteLaneRoadMarkType(nodeLaneRoadMark,&laneRoadMarkType);
+    }
+
 	return true;
 }
 //--------------
 
+bool OpenDriveXmlWriter::WriteLaneRoadMarkType(TiXmlElement *node, LaneRoadMarkType * laneRoadMarkType)
+{
+    std::string name;
+    double width;
+    name = laneRoadMarkType->Getname();
+    width = laneRoadMarkType->GetWidth();
+
+    TiXmlElement* nodeLaneRoadMarkType = new TiXmlElement("type");
+    node->LinkEndChild(nodeLaneRoadMarkType);
+
+    nodeLaneRoadMarkType->SetAttribute("name",name);
+    nodeLaneRoadMarkType->SetDoubleAttribute("width",width);
+
+    unsigned int nCount = laneRoadMarkType->GetLaneRoadMarkLineCount();
+    unsigned int i;
+    for(i=0;i<nCount;i++)
+    {
+        LaneRoadMarkLine * pLaneRoadMarkLine = laneRoadMarkType->GetLaneRoadMarkLine(i);
+        if(pLaneRoadMarkLine != NULL)
+        {
+            WriteLaneRoadMarkLine(nodeLaneRoadMarkType,pLaneRoadMarkLine);
+        }
+    }
+
+    return true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteLaneRoadMarkLine(TiXmlElement *node, LaneRoadMarkLine * laneRoadMarkLine)
+{
+    double length;
+    double space;
+    double tOffset;
+    double sOffset;
+
+    length = laneRoadMarkLine->Getlength();
+    space = laneRoadMarkLine->Getspace();
+    tOffset = laneRoadMarkLine->GettOffset();
+    sOffset = laneRoadMarkLine->GetsOffset();
+
+    std::string rule;
+    std::string color;
+    double width;
+
+    TiXmlElement* nodeLaneRoadMarkTypeLine = new TiXmlElement("line");
+    node->LinkEndChild(nodeLaneRoadMarkTypeLine);
+
+    nodeLaneRoadMarkTypeLine->SetDoubleAttribute("length",length);
+    nodeLaneRoadMarkTypeLine->SetDoubleAttribute("space",space);
+    nodeLaneRoadMarkTypeLine->SetDoubleAttribute("tOffset",tOffset);
+    nodeLaneRoadMarkTypeLine->SetDoubleAttribute("sOffset",sOffset);
+
+    if(laneRoadMarkLine->Getcolor(color) == 1)
+    {
+        nodeLaneRoadMarkTypeLine->SetAttribute("color",color);
+    }
+
+    if(laneRoadMarkLine->Getrule(rule) == 1)
+    {
+        nodeLaneRoadMarkTypeLine->SetAttribute("rule",rule);
+    }
+
+    if(laneRoadMarkLine->Getwidth(width) == 1)
+    {
+        nodeLaneRoadMarkTypeLine->SetDoubleAttribute("width",width);
+    }
+
+    return  true;
+}
+//--------------
+
 bool OpenDriveXmlWriter::WriteLaneMaterial(TiXmlElement *node, LaneMaterial* laneMaterial)
 {
 	double sOffset;
@@ -1154,9 +1288,120 @@ bool OpenDriveXmlWriter::WriteObjects (TiXmlElement *node, Road* road)
         WriteObject(nodeObjects, road->GetObject(i));
     }
 
+    unsigned int lObjectsBridgeCount = road->GetObjectsBridgeCount();
+    for(unsigned int i=0; i<lObjectsBridgeCount; i++)
+    {
+        WriteObjectsBridge(nodeObjects, road->GetObjectsBridge(i));
+    }
+
+    unsigned int lObjectsTunnelCount = road->GetObjectsTunnelCount();
+    for(unsigned int i=0;i<lObjectsTunnelCount; i++)
+    {
+        WriteObjectsTunnel(nodeObjects, road->GetObjectsTunnel(i));
+    }
+
+    unsigned int lObjectsObjectReferenceCount = road->GetObjectsObjectReferenceCount();
+    for(unsigned int i=0;i<lObjectsObjectReferenceCount; i++)
+    {
+        WriteObjectsObjectReference(nodeObjects, road->GetObjectsObjectReference(i));
+    }
+
 	return true;
 }
 //--------------
+bool OpenDriveXmlWriter::WriteObjectsTunnel(TiXmlElement * node, Objects_tunnel * pObjectsTunnel)
+{
+    TiXmlElement* nodeObjectsTunnel = new TiXmlElement("tunnel");
+    node->LinkEndChild(nodeObjectsTunnel);
+
+    nodeObjectsTunnel->SetDoubleAttribute("s",pObjectsTunnel->GetS());
+    nodeObjectsTunnel->SetDoubleAttribute("length",pObjectsTunnel->Getlength());
+    nodeObjectsTunnel->SetAttribute("id",pObjectsTunnel->Getid());
+    nodeObjectsTunnel->SetAttribute("type",pObjectsTunnel->Gettype());
+
+    std::string strname;
+    if(pObjectsTunnel->GetName(strname) == 1)
+    {
+        nodeObjectsTunnel->SetAttribute("name",strname);
+    }
+
+    int lighting,daylight;
+
+    if(pObjectsTunnel->Getlighting(lighting) == 1)
+    {
+        nodeObjectsTunnel->SetAttribute("lighting",lighting);
+    }
+
+    if(pObjectsTunnel->Getdaylight(daylight) == 1)
+    {
+        nodeObjectsTunnel->SetAttribute("daylight",daylight);
+    }
+
+    unsigned int llaneValidityCount = pObjectsTunnel->GetObjectlaneValidityCount();
+    for(unsigned int i=0; i<llaneValidityCount; i++)
+    {
+        WriteObjectlaneValidity(nodeObjectsTunnel, pObjectsTunnel->GetObjectlaneValidity(i));
+    }
+
+    return true;
+}
+//--------------
+bool OpenDriveXmlWriter::WriteObjectsObjectReference(TiXmlElement * node, Objects_objectReference * pObjectsObjectReference)
+{
+    TiXmlElement* nodeObjectsObjectReference = new TiXmlElement("objectReference");
+    node->LinkEndChild(nodeObjectsObjectReference);
+
+    nodeObjectsObjectReference->SetDoubleAttribute("s",pObjectsObjectReference->GetS());
+    nodeObjectsObjectReference->SetDoubleAttribute("t",pObjectsObjectReference->Gett());
+    nodeObjectsObjectReference->SetAttribute("id",pObjectsObjectReference->Getid());
+    nodeObjectsObjectReference->SetAttribute("orientation",pObjectsObjectReference->Getorientation());
+
+    double zOffset,validLength;
+    if(pObjectsObjectReference->GetzOffset(zOffset) == 1)
+    {
+        nodeObjectsObjectReference->SetDoubleAttribute("zOffset",zOffset);
+    }
+
+    if(pObjectsObjectReference->GetvalidLength(validLength) == 1)
+    {
+        nodeObjectsObjectReference->SetDoubleAttribute("validLength",validLength);
+    }
+
+
+    unsigned int llaneValidityCount = pObjectsObjectReference->GetObjectlaneValidityCount();
+    for(unsigned int i=0; i<llaneValidityCount; i++)
+    {
+        WriteObjectlaneValidity(nodeObjectsObjectReference, pObjectsObjectReference->GetObjectlaneValidity(i));
+    }
+
+    return true;
+}
+//--------------
+bool OpenDriveXmlWriter::WriteObjectsBridge(TiXmlElement * node, Objects_bridge * pObjectsBridge)
+{
+    TiXmlElement* nodeObjectsBridge = new TiXmlElement("bridge");
+    node->LinkEndChild(nodeObjectsBridge);
+
+    nodeObjectsBridge->SetDoubleAttribute("s",pObjectsBridge->GetS());
+    nodeObjectsBridge->SetDoubleAttribute("length",pObjectsBridge->Getlength());
+    nodeObjectsBridge->SetAttribute("id",pObjectsBridge->Getid());
+    nodeObjectsBridge->SetAttribute("type",pObjectsBridge->Gettype());
+
+    std::string strname;
+    if(pObjectsBridge->GetName(strname) == 1)
+    {
+        nodeObjectsBridge->SetAttribute("name",strname);
+    }
+
+    unsigned int llaneValidityCount = pObjectsBridge->GetObjectlaneValidityCount();
+    for(unsigned int i=0; i<llaneValidityCount; i++)
+    {
+        WriteObjectlaneValidity(nodeObjectsBridge, pObjectsBridge->GetObjectlaneValidity(i));
+    }
+
+    return true;
+}
+//--------------
 bool OpenDriveXmlWriter::WriteObject(TiXmlElement *node, Object *pObject)
 {
     TiXmlElement* nodeObject = new TiXmlElement("object");
@@ -1235,14 +1480,14 @@ bool OpenDriveXmlWriter::WriteObject(TiXmlElement *node, Object *pObject)
 
     unsigned int i;
 
-    unsigned nrepeatcount = pObject->GetObjectrepeatCount();
+    unsigned int nrepeatcount = pObject->GetObjectrepeatCount();
     for(i=0;i<nrepeatcount;i++)
     {
         Object_repeat * pObject_repeat = pObject->GetObjectrepeat(i);
         WriteObjectrepeat(nodeObject,pObject_repeat);
     }
 
-    unsigned nmaterialcount = pObject->GetObjectmaterialCount();
+    unsigned int nmaterialcount = pObject->GetObjectmaterialCount();
     for(i=0;i<nmaterialcount;i++)
     {
         Object_material * pObject_material = pObject->GetObjectmaterial(i);
@@ -1256,7 +1501,7 @@ bool OpenDriveXmlWriter::WriteObject(TiXmlElement *node, Object *pObject)
         WriteObjectOutlines(nodeObject,pObject_outlines);
     }
 
-    unsigned noutlinecount = pObject->GetObjectoutlineCount();
+    unsigned int noutlinecount = pObject->GetObjectoutlineCount();
     for(i=0;i<noutlinecount;i++)
     {
         Object_outlines_outline * pObject_outlines_outline = pObject->GetObjectoutline(i);
@@ -1269,6 +1514,19 @@ bool OpenDriveXmlWriter::WriteObject(TiXmlElement *node, Object *pObject)
         WriteObjectMarkings(nodeObject,pObject_markings);
     }
 
+    Object_borders * pObject_borders = pObject->Getborders();
+    if(pObject_borders != NULL)
+    {
+        WriteObjectborders(nodeObject,pObject_borders);
+    }
+
+    unsigned int nvaliditycount = pObject->GetObjectlaneValidityCount();
+    for(i=0;i<nvaliditycount;i++)
+    {
+        Object_laneValidity * pObject_lanevalidity = pObject->GetObjectlaneValidity(i);
+        WriteObjectlaneValidity(nodeObject,pObject_lanevalidity);
+    }
+
 
     return true;
 }
@@ -1530,7 +1788,82 @@ bool OpenDriveXmlWriter::WriteObjectMarkingsMarkingcornerReference(TiXmlElement
     TiXmlElement * nodecornerReference = new TiXmlElement("cornerReference");
     node->LinkEndChild(nodecornerReference);
 
-    nodecornerReference->SetAttribute("id",pObject_markings_marking_cornerReference->Getid());
+    nodecornerReference->SetAttribute("id",static_cast<int>( pObject_markings_marking_cornerReference->Getid()));
+
+    return true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteObjectborders(TiXmlElement * node,Object_borders * pObject_borders)
+{
+    TiXmlElement * nodeObjectboders = new TiXmlElement("borders");
+    node->LinkEndChild(nodeObjectboders);
+
+    unsigned int lbordercount = pObject_borders->GetBorderCount();
+    if(lbordercount == 0)
+    {
+        std::cout<<"borders must have at least 1 border"<<std::endl;
+        return false;
+    }
+    unsigned int i;
+    for(i=0;i<lbordercount;i++)
+    {
+        WriteObjectbordersborder(nodeObjectboders,pObject_borders->GetBorder(i));
+    }
+
+    return true;
+}
+
+//--------------
+
+
+bool OpenDriveXmlWriter::WriteObjectbordersborder(TiXmlElement * node,Object_borders_border * pObject_borders_border)
+{
+    TiXmlElement * nodeborder = new TiXmlElement("border");
+    node->LinkEndChild(nodeborder);
+
+    double width;
+    std::string strtype;
+    int outlineId;
+    bool useCompletOutline;
+
+    width = pObject_borders_border->Getwidth();
+    nodeborder->SetDoubleAttribute("width",width);
+
+    strtype = pObject_borders_border->GetborderType();
+    nodeborder->SetAttribute("type",strtype);
+
+    outlineId = pObject_borders_border->Getoutlineid();
+    nodeborder->SetAttribute("outlineId",outlineId);
+
+    if(pObject_borders_border->GetuserCompleteOutline(useCompletOutline) == 1)
+    {
+        if(useCompletOutline == true)
+            nodeborder->SetAttribute("useCompleteOutline","true");
+        else
+            nodeborder->SetAttribute("useCompleteOutline","false");
+    }
+
+    unsigned int lcornerCount = pObject_borders_border->GetcornerReferenceCount();
+    unsigned int i;
+    for(i=0;i<lcornerCount;i++)
+    {
+        WriteObjectbordersbordercornerReference(nodeborder,pObject_borders_border->GetcornerReference(i));
+    }
+
+    return true;
+}
+
+//--------------
+
+
+bool OpenDriveXmlWriter::WriteObjectbordersbordercornerReference(TiXmlElement * node,Object_markings_marking_cornerReference * pObject_markings_marking_cornerReference)
+{
+    TiXmlElement * nodecornerReference = new TiXmlElement("cornerReference");
+    node->LinkEndChild(nodecornerReference);
+
+    nodecornerReference->SetAttribute("id",static_cast<int>( pObject_markings_marking_cornerReference->Getid()));
 
     return true;
 }
@@ -1556,6 +1889,17 @@ bool OpenDriveXmlWriter::WriteObjectOutlinesOutlinecornerRoad(TiXmlElement *node
     return true;
 }
 
+bool OpenDriveXmlWriter::WriteObjectlaneValidity(TiXmlElement * node,Object_laneValidity * pObject_laneValidity)
+{
+    TiXmlElement * nodelaneValidity = new TiXmlElement("validity");
+    node->LinkEndChild(nodelaneValidity);
+
+    nodelaneValidity->SetAttribute("fromLane",pObject_laneValidity->GetfromLane());
+    nodelaneValidity->SetAttribute("toLane",pObject_laneValidity->GettoLane());
+
+    return true;
+}
+
 //--------------
 
 bool OpenDriveXmlWriter::WriteObjectOutlinesOutlinecornerLocal(TiXmlElement *node, Object_outlines_outline_cornerLocal *pObject_outlines_outline_cornerLocal)
@@ -1578,6 +1922,28 @@ bool OpenDriveXmlWriter::WriteObjectOutlinesOutlinecornerLocal(TiXmlElement *nod
 
 //--------------
 
+
+bool OpenDriveXmlWriter::WriteRoadPriority(TiXmlElement * node,Road * road)
+{
+    TiXmlElement* nodeRoadPriority = new TiXmlElement("userData");
+    nodeRoadPriority->SetAttribute("code","roadPriority");
+    node->LinkEndChild(nodeRoadPriority);
+
+    int nPriority;
+    if(road->GetRoadPriority(nPriority) == 1)
+    {
+        TiXmlElement* nodeRoadPV = new TiXmlElement("roadPriority");
+        nodeRoadPriority->LinkEndChild(nodeRoadPV);
+
+        nodeRoadPV->SetAttribute("Priority",nPriority);
+
+    }
+
+    return true;
+}
+
+//--------------
+
 bool OpenDriveXmlWriter::WriteRoadBorrow(TiXmlElement *node, RoadBorrow *pRoadBorrow)
 {
     TiXmlElement* nodeRoadBorrow = new TiXmlElement("roadborrow");
@@ -1645,9 +2011,37 @@ bool OpenDriveXmlWriter::WriteSignals (TiXmlElement *node, Road* road)
         WriteSignal(nodeSignals, road->GetSignal(i));
     }
 
+    unsigned int lSignalReferenceCount = road->GetSignalReferencCount();
+    for(unsigned int i=0; i<lSignalReferenceCount; i++)
+    {
+        WriteSignalReference(nodeSignals,road->GetSignalReference(i));
+    }
+
 	return true;
 }
 
+//--------------
+
+bool OpenDriveXmlWriter::WriteSignalReference(TiXmlElement * node, signals_signalReference * pSignalReference)
+{
+    TiXmlElement* nodeSignalReference = new TiXmlElement("signalReference");
+    node->LinkEndChild(nodeSignalReference);
+
+    nodeSignalReference->SetDoubleAttribute("s",pSignalReference->Gets());
+    nodeSignalReference->SetDoubleAttribute("t",pSignalReference->Gett());
+    nodeSignalReference->SetAttribute("id",pSignalReference->Getid());
+    nodeSignalReference->SetAttribute("orientation",pSignalReference->Getorientation());
+
+    unsigned int llaneValidityCount = pSignalReference->GetlaneValidityCount();
+    for(unsigned int i =0;i<llaneValidityCount;i++)
+    {
+        WriteSignal_laneValidity(nodeSignalReference,pSignalReference->GetlaneValidity(i));
+    }
+
+}
+
+//--------------
+
 bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
 {
     TiXmlElement* nodeSignal = new TiXmlElement("signal");
@@ -1673,10 +2067,30 @@ bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
     nodeSignal->SetDoubleAttribute("height",pSignal->Getheight());
     nodeSignal->SetDoubleAttribute("width",pSignal->Getwidth());
 
-    signal_laneValidity * psignal_lanevalidity = pSignal->GetlaneValidity();
-    if(psignal_lanevalidity != 0)
+
+    //Not Right, but maybe code in plan,use.
+//    signal_laneValidity * psignal_lanevalidity = pSignal->GetlaneValidity();
+//    if(psignal_lanevalidity != 0)
+//    {
+//        WriteSignal_laneValidity(nodeSignal,psignal_lanevalidity);
+//    }
+
+    unsigned int llaneValidityCount = pSignal->GetlaneValidityCount();
+    for(unsigned int i=0;i<llaneValidityCount;i++)
+    {
+        WriteSignal_laneValidity(nodeSignal,pSignal->GetlaneValidity(i));
+    }
+
+    unsigned int ldependencyCount = pSignal->GetDependencyCount();
+    for(unsigned int i=0;i<ldependencyCount;i++)
+    {
+        WriteSignal_dependency(nodeSignal,pSignal->GetDependency(i));
+    }
+
+    unsigned int lreferencyCount = pSignal->GetReferenceCount();
+    for(unsigned int i=0;i<lreferencyCount;i++)
     {
-        WriteSignal_laneValidity(nodeSignal,psignal_lanevalidity);
+        WriteSignal_referency(nodeSignal,pSignal->GetReference(i));
     }
 
     signal_positionInertial * psignal_positionInertial = pSignal->GetpositionInertial();
@@ -1685,24 +2099,65 @@ bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
         WriteSignal_positionInertial(nodeSignal,psignal_positionInertial);
     }
 
+    signal_positionRoad * psignal_positionRoad = pSignal->GetpositionRoad();
+    if(psignal_positionRoad != NULL)
+    {
+        WriteSignal_positionRoad(nodeSignal,psignal_positionRoad);
+    }
+
+    return true;
+}
+//--------------
+
+bool OpenDriveXmlWriter::WriteSignal_positionRoad(TiXmlElement * node, signal_positionRoad * pSignal_positionRoad)
+{
+    TiXmlElement* nodepositionRoad = new TiXmlElement("positionRoad");
+
+    node->LinkEndChild(nodepositionRoad);
+
+    nodepositionRoad->SetAttribute("roadId",pSignal_positionRoad->Getroadid());
+    nodepositionRoad->SetDoubleAttribute("s",pSignal_positionRoad->Gets());
+    nodepositionRoad->SetDoubleAttribute("t",pSignal_positionRoad->Gett());
+    nodepositionRoad->SetDoubleAttribute("zOffset",pSignal_positionRoad->GetzOffset());
+    nodepositionRoad->SetDoubleAttribute("hOffset",pSignal_positionRoad->GethOffset());
+
+    double pitch,roll;
+    if(pSignal_positionRoad->Getpitch(pitch) == 1)
+    {
+        nodepositionRoad->SetDoubleAttribute("pitch",pitch);
+    }
+    if(pSignal_positionRoad->Getroll(roll) == 1)
+    {
+        nodepositionRoad->SetDoubleAttribute("roll",roll);
+    }
+
     return true;
 }
 
+//--------------
 bool OpenDriveXmlWriter::WriteSignal_positionInertial(TiXmlElement *node, signal_positionInertial *pSignal_positionInertial)
 {
     TiXmlElement* nodepositionInertial = new TiXmlElement("positionInertial");
 
     node->LinkEndChild(nodepositionInertial);
 
-    nodepositionInertial->SetAttribute("x",pSignal_positionInertial->Getx());
-    nodepositionInertial->SetAttribute("y",pSignal_positionInertial->Gety());
-    nodepositionInertial->SetAttribute("z",pSignal_positionInertial->Getz());
-    nodepositionInertial->SetAttribute("hdg",pSignal_positionInertial->Gethdg());
-    nodepositionInertial->SetAttribute("pitch",pSignal_positionInertial->Getpitch());
-    nodepositionInertial->SetAttribute("roll",pSignal_positionInertial->Getroll());
+    nodepositionInertial->SetDoubleAttribute("x",pSignal_positionInertial->Getx());
+    nodepositionInertial->SetDoubleAttribute("y",pSignal_positionInertial->Gety());
+    nodepositionInertial->SetDoubleAttribute("z",pSignal_positionInertial->Getz());
+    nodepositionInertial->SetDoubleAttribute("hdg",pSignal_positionInertial->Gethdg());
+    double pitch,roll;
+    if(pSignal_positionInertial->Getpitch(pitch) == 1)
+    {
+        nodepositionInertial->SetDoubleAttribute("pitch",pitch);
+    }
+    if(pSignal_positionInertial->Getroll(roll) == 1)
+    {
+        nodepositionInertial->SetDoubleAttribute("roll",roll);
+    }
 
     return true;
 }
+//--------------
 
 bool OpenDriveXmlWriter::WriteSignal_laneValidity(TiXmlElement *node, signal_laneValidity *pSignal_laneValidity)
 {
@@ -1715,17 +2170,245 @@ bool OpenDriveXmlWriter::WriteSignal_laneValidity(TiXmlElement *node, signal_lan
 
     return true;
 }
+//--------------
+bool OpenDriveXmlWriter::WriteSignal_dependency(TiXmlElement * node, signal_dependency * pSignal_dependency)
+{
+    TiXmlElement* nodedependency = new TiXmlElement("dependency");
+
+    node->LinkEndChild(nodedependency);
+
+    nodedependency->SetAttribute("id",pSignal_dependency->Getid());
+
+    std::string strtype;
+    if(pSignal_dependency->Gettype(strtype) == 1)
+    {
+        nodedependency->SetAttribute("type",strtype);
+    }
+
+    return true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteSignal_referency(TiXmlElement * node, signal_reference * pSignal_Referency)
+{
+    TiXmlElement* nodereferency = new TiXmlElement("reference");
+
+    node->LinkEndChild(nodereferency);
+
+    nodereferency->SetAttribute("elementType",pSignal_Referency->GetelementType());
+    nodereferency->SetAttribute("elementId",pSignal_Referency->GetelementId());
+
+    std::string strtype;
+    if(pSignal_Referency->Gettype(strtype) == 1)
+    {
+        nodereferency->SetAttribute("type",strtype);
+    }
+
+    return true;
+}
 
 //--------------
 
-bool OpenDriveXmlWriter::WriteSurface (TiXmlElement *node, Road* road)
+bool OpenDriveXmlWriter::WriteSurface (TiXmlElement *node,  Road * road)
 {
+    TiXmlElement * nodeSurface = new TiXmlElement("surface");
+    node->LinkEndChild(nodeSurface);
+
+    unsigned int lCRGCount = road->GetSurfaceCRGCount();
+    for(unsigned int i=0;i<lCRGCount;i++)
+    {
+        WriteSurfaceCRG(nodeSurface,road->GetSurfaceCRG(i));
+    }
 	return true;
 }
+
 //--------------
 
-bool OpenDriveXmlWriter::WriteController (TiXmlElement *node)
-{	return true;	}
+bool OpenDriveXmlWriter::WriteSurfaceCRG(TiXmlElement * node, surface_CRG * psurfaceCRG)
+{
+    TiXmlElement * nodeSurfaceCRG = new TiXmlElement("CRG");
+    node->LinkEndChild(nodeSurfaceCRG);
+
+    nodeSurfaceCRG->SetAttribute("file",psurfaceCRG->Getfile());
+    nodeSurfaceCRG->SetDoubleAttribute("sStart",psurfaceCRG->GetsStart());
+    nodeSurfaceCRG->SetDoubleAttribute("sEnd",psurfaceCRG->GetsEnd());
+    nodeSurfaceCRG->SetAttribute("orientation",psurfaceCRG->Getorientation());
+    nodeSurfaceCRG->SetAttribute("mode",psurfaceCRG->Getmode());
+
+    std::string purpose;
+    if(psurfaceCRG->Getpurpose(purpose) == 1)
+    {
+        nodeSurfaceCRG->SetAttribute("purpose",purpose);
+    }
+
+    double sOffset,tOffset,zOffset,zScale,hOffset;
+    if(psurfaceCRG->GetsOffset(sOffset) == 1)
+    {
+        nodeSurfaceCRG->SetDoubleAttribute("sOffset",sOffset);
+    }
+    if(psurfaceCRG->GettOffset(tOffset) == 1)
+    {
+        nodeSurfaceCRG->SetDoubleAttribute("tOffset",tOffset);
+    }
+    if(psurfaceCRG->GetzOffset(zOffset) == 1)
+    {
+        nodeSurfaceCRG->SetDoubleAttribute("zOffset",zOffset);
+    }
+    if(psurfaceCRG->GetzScale(zScale) == 1)
+    {
+        nodeSurfaceCRG->SetDoubleAttribute("zScale",zScale);
+    }
+    if(psurfaceCRG->GethOffset(hOffset) == 1)
+    {
+        nodeSurfaceCRG->SetDoubleAttribute("hOffset",hOffset);
+    }
+
+    return true;
+
+
+}
+//--------------
+
+bool OpenDriveXmlWriter::WriteRailroad(TiXmlElement *node, Road * road)
+{
+    TiXmlElement * nodeRailroad = new TiXmlElement("railroad");
+    node->LinkEndChild(nodeRailroad);
+
+    unsigned int lSwitchCount = road->GetRailroadSwitchCount();
+    for(unsigned int i=0;i<lSwitchCount;i++)
+    {
+        WriteRailroadSwitch(nodeRailroad,road->GetRailroadSwitch(i));
+    }
+    return true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteRailroadSwitch(TiXmlElement *node,Railroad_Switch * pRailroad_Switch)
+{
+    TiXmlElement * nodeSwitch = new TiXmlElement("switch");
+    node->LinkEndChild(nodeSwitch);
+
+    nodeSwitch->SetAttribute("name", pRailroad_Switch->Getname());
+    nodeSwitch->SetAttribute("id", pRailroad_Switch->Getid());
+    nodeSwitch->SetAttribute("position", pRailroad_Switch->Getposition());
+
+
+    WriteRailroadSwitchMainTrack(nodeSwitch,pRailroad_Switch->GetMainTrack());
+    WriteRailroadSwitchSideTrack(nodeSwitch,pRailroad_Switch->GetSideTrack());
+
+    Railroad_Switch_Partner partner;
+    if(pRailroad_Switch->GetPartner(partner) == 1)
+    {
+        WriteRailroadSwitchPartner(nodeSwitch,&partner);
+    }
+
+    return  true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteRailroadSwitchMainTrack(TiXmlElement *node,Railroad_Switch_MainTrack * pRailroad_Switch_MainTrack)
+{
+    TiXmlElement * nodeMainTrack = new TiXmlElement("mainTrack");
+    node->LinkEndChild(nodeMainTrack);
+
+    nodeMainTrack->SetAttribute("id", pRailroad_Switch_MainTrack->Getid());
+    nodeMainTrack->SetDoubleAttribute("s", pRailroad_Switch_MainTrack->Gets());
+    nodeMainTrack->SetAttribute("dir", pRailroad_Switch_MainTrack->Getdir());
+
+    return true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteRailroadSwitchSideTrack(TiXmlElement *node,Railroad_Switch_SideTrack * pRailroad_Switch_SideTrack)
+{
+    TiXmlElement * nodeSideTrack = new TiXmlElement("sideTrack");
+    node->LinkEndChild(nodeSideTrack);
+
+    nodeSideTrack->SetAttribute("id", pRailroad_Switch_SideTrack->Getid());
+    nodeSideTrack->SetDoubleAttribute("s", pRailroad_Switch_SideTrack->Gets());
+    nodeSideTrack->SetAttribute("dir", pRailroad_Switch_SideTrack->Getdir());
+
+    return  true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteRailroadSwitchPartner(TiXmlElement *node, Railroad_Switch_Partner * pRailroad_Switch_Partner)
+{
+    TiXmlElement * nodePartner = new TiXmlElement("partner");
+    node->LinkEndChild(nodePartner);
+
+    nodePartner->SetAttribute("id", pRailroad_Switch_Partner->Getid());
+
+    std::string name;
+    if(pRailroad_Switch_Partner->Getname(name) == 1)
+    {
+        nodePartner->SetAttribute("name",name);
+    }
+
+    return  true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteController (TiXmlElement *node,Controller* controller)
+{
+    //A controller must have at least one control
+    if(controller->GetControlCount() == 0)return false;
+
+    string id;
+    id = controller->Getid();
+
+    TiXmlElement * nodeController = new TiXmlElement("controller");
+    node->LinkEndChild(nodeController);
+
+    nodeController->SetAttribute("id",id);
+
+    std::string name;
+    unsigned int sequence;
+    if(controller->Getname(name) == 1)
+    {
+        nodeController->SetAttribute("name",name);
+    }
+    if(controller->Getsequence(sequence) == 1)
+    {
+        nodeController->SetAttribute("sequence",static_cast<int>(sequence));
+    }
+
+    unsigned int lControlCount = controller->GetControlCount();
+    for(unsigned int i=0;i<lControlCount;i++)
+    {
+        WriteController_Control(nodeController,controller->GetControl(i));
+    }
+
+    return true;
+}
+
+//--------------
+
+bool OpenDriveXmlWriter::WriteController_Control(TiXmlElement *node,Controller_control* control)
+{
+
+    TiXmlElement * nodeControl = new TiXmlElement("control");
+    node->LinkEndChild(nodeControl);
+
+    std::string signalId = control->GetsignalId();
+
+    nodeControl->SetAttribute("signalId",signalId);
+
+    std::string type;
+    if(control->Gettype(type) == 1)
+    {
+        nodeControl->SetAttribute("type",type);
+    }
+
+    return true;
+}
+
 //--------------
 
 bool OpenDriveXmlWriter::WriteJunction (TiXmlElement *node, Junction *junction)
@@ -1885,10 +2568,17 @@ bool OpenDriveXmlWriter::WriteFile(std::string fileName)
 		WriteJunction(rootNode, mOpenDrive->GetJunction(i));
 	}
 
+    //Write controller
+    unsigned int controllerCount = mOpenDrive->GetControllerCount();
+    for(unsigned int i=0;i<controllerCount; i++)
+    {
+        WriteController(rootNode,mOpenDrive->GetController(i));
+    }
+
 	// Saves the XML structure to the file
-	doc.SaveFile( fileName ); 
+    return doc.SaveFile( fileName );
 
-	return true;
+//	return true;
 
 }
 //--------------

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

@@ -58,6 +58,8 @@ public:
 	bool WriteLane (TiXmlElement *node, Lane* lane);
 	bool WriteLaneWidth(TiXmlElement *node, LaneWidth* laneWidth);
 	bool WriteLaneRoadMark(TiXmlElement *node, LaneRoadMark* laneRoadMark);
+    bool WriteLaneRoadMarkType(TiXmlElement *node, LaneRoadMarkType * laneRoadMarkType);
+    bool WriteLaneRoadMarkLine(TiXmlElement *node, LaneRoadMarkLine * laneRoadMarkLine);
 	bool WriteLaneMaterial(TiXmlElement *node, LaneMaterial* laneMaterial);
 	bool WriteLaneVisibility(TiXmlElement *node, LaneVisibility* laneVisibility);
 	bool WriteLaneSpeed(TiXmlElement *node, LaneSpeed* laneSpeed);
@@ -65,13 +67,19 @@ public:
 	bool WriteLaneHeight(TiXmlElement *node, LaneHeight* laneHeight);
     bool WriteLaneOffset (TiXmlElement *node, LaneOffset *laneOffset);
     bool WriteLaneBorder(TiXmlElement *node, LaneBorder* laneWidth);
+
+
 	//--------------
 
+    bool WriteRoadPriority(TiXmlElement * node,Road * road);
     bool WriteRoadNoavoids(TiXmlElement *node, Road* road);
     bool WriteRoadNoavoid(TiXmlElement *node,RoadNoavoid * pRoadNoavoid);
     bool WriteRoadBorrows(TiXmlElement *node, Road* road);
-    bool WriteRoadBorrow(TiXmlElement *node,RoadBorrow * pRoadBorrow);
+    bool WriteRoadBorrow(TiXmlElement *node,RoadBorrow * pRoadBorrow);   
 	bool WriteObjects (TiXmlElement *node, Road* road);
+    bool WriteObjectsBridge(TiXmlElement * node, Objects_bridge * pObjectsBridge);
+    bool WriteObjectsTunnel(TiXmlElement * node, Objects_tunnel * pObjectsTunnel);
+    bool WriteObjectsObjectReference(TiXmlElement * node, Objects_objectReference * pObjectsObjectReference);
     bool WriteObject(TiXmlElement * node, Object * pObject);
     bool WriteObjectParkingSpace(TiXmlElement * node,Object_parkingSpace * pObject_parkingSpace);
     bool WriteObjectrepeat(TiXmlElement * node,Object_repeat * pObject_repeat);
@@ -82,18 +90,36 @@ public:
     bool WriteObjectOutlinesOutlinecornerLocal(TiXmlElement * node,Object_outlines_outline_cornerLocal * pObject_outlines_outline_cornerLocal);
 	bool WriteSignals (TiXmlElement *node, Road* road);
     bool WriteSignal(TiXmlElement * node, Signal * pSignal);
+    bool WriteSignalReference(TiXmlElement * node, signals_signalReference * pSignalReference);
     bool WriteSignal_positionInertial(TiXmlElement * node, signal_positionInertial * pSignal_positionInertial);
+    bool WriteSignal_positionRoad(TiXmlElement * node, signal_positionRoad * pSignal_positionRoad);
     bool WriteSignal_laneValidity(TiXmlElement * node, signal_laneValidity * pSignal_laneValidity);
+    bool WriteSignal_dependency(TiXmlElement * node, signal_dependency * pSignal_dependency);
+    bool WriteSignal_referency(TiXmlElement * node, signal_reference * pSignal_Referency);
     bool WriteObjectMarkings(TiXmlElement * node,Object_markings * pObject_markings);
     bool WriteObjectMarkingsMarking(TiXmlElement * node,Object_markings_marking * pObject_markings_marking);
     bool WriteObjectMarkingsMarkingcornerReference(TiXmlElement * node,Object_markings_marking_cornerReference * pObject_markings_marking_cornerReference);
+    bool WriteObjectborders(TiXmlElement * node,Object_borders * pObject_borders);
+    bool WriteObjectbordersborder(TiXmlElement * node,Object_borders_border * pObject_borders_border);
+    bool WriteObjectbordersbordercornerReference(TiXmlElement * node,Object_markings_marking_cornerReference * pObject_markings_marking_cornerReference);
+    bool WriteObjectlaneValidity(TiXmlElement * node,Object_laneValidity * pObject_laneValidity);
 
     //--------------
 
-	bool WriteSurface (TiXmlElement *node, Road* road);
+    bool WriteSurface (TiXmlElement *node, Road * road);
+    bool WriteSurfaceCRG(TiXmlElement * node, surface_CRG * psurfaceCRG);
 	//--------------
 
-	bool WriteController (TiXmlElement *node);
+    bool WriteRailroad(TiXmlElement *node, Road * road);
+    bool WriteRailroadSwitch(TiXmlElement *node,Railroad_Switch * pRailroad_Switch);
+    bool WriteRailroadSwitchMainTrack(TiXmlElement *node,Railroad_Switch_MainTrack * pRailroad_Switch_MainTrack);
+    bool WriteRailroadSwitchSideTrack(TiXmlElement *node,Railroad_Switch_SideTrack * pRailroad_Switch_SideTrack);
+    bool WriteRailroadSwitchPartner(TiXmlElement *node, Railroad_Switch_Partner * pRailroad_Switch_Partner);
+
+    //--------------
+
+    bool WriteController (TiXmlElement *node,Controller* controller);
+    bool WriteController_Control(TiXmlElement *node,Controller_control* control);
 	//--------------
 
 	bool WriteJunction (TiXmlElement *node, Junction *junction);

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

@@ -87,4 +87,120 @@ double ThirdOrderPolynom::GetValue(double s_check)
 	double value = mA+ mB*ds+ mC*ds*ds + mD*ds*ds*ds;
 	return value;
 }
+
+/**
+ * Returns the value at sample S
+ */
+double ThirdOrderPolynom::GetValueGradient(double s_check)
+{
+    double ds = s_check-mS;
+    return mB + 2.0*mC*ds+ 3.0*mD*ds*ds;
+}
+//----------------------------------------------------------------------------------
+
+
+//***********************************************************************************
+//Polynom of 2D third order
+//***********************************************************************************
+/**
+ * Constructor that initializes the polynom with base properties
+ *
+ * @param s S offset
+ * @param a A parameter of the polynom
+ * @param b B parameter of the polynom
+ * @param c C parameter of the polynom
+ * @param d D parameter of the polynom
+ */
+Third2DOrderPolynom::Third2DOrderPolynom (double s, double t,double a, double b, double c, double d)
+{
+    mS=s;mT=t; mA=a; mB=b; mC=c; mD=d;
+}
+
+/**
+ * Getters for base properties
+ */
+double Third2DOrderPolynom::GetS()
+{
+    return mS;
+}
+double Third2DOrderPolynom::GetT()
+{
+    return mT;
+}
+double Third2DOrderPolynom::GetA()
+{
+    return mA;
+}
+double Third2DOrderPolynom::GetB()
+{
+    return mB;
+}
+double Third2DOrderPolynom::GetC()
+{
+    return mC;
+}
+
+double Third2DOrderPolynom::GetD()
+{
+    return mD;
+}
+
+/**
+ * Setters for base properties
+ */
+void Third2DOrderPolynom::SetS(double s)
+{
+    mS=s;
+}
+void Third2DOrderPolynom::SetT(double t)
+{
+    mT=t;
+}
+void Third2DOrderPolynom::SetA(double a)
+{
+    mA=a;
+}
+void Third2DOrderPolynom::SetB(double b)
+{
+    mB=b;
+}
+void Third2DOrderPolynom::SetC(double c)
+{
+    mC=c;
+}
+void Third2DOrderPolynom::SetD(double d)
+{
+    mD=d;
+}
+
+
+/**
+ * Method to check if sample S is inside the record interval
+ */
+bool Third2DOrderPolynom::CheckInterval (double s_check,double t_check)
+{
+    if (s_check>=mS)
+    {
+        if(t_check>=mT)
+        {
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+    else
+        return false;
+}
+
+/**
+ * Returns the value at sample S
+ */
+double Third2DOrderPolynom::GetValue(double t_check)
+{
+    double dt = t_check-mT;
+    double value = mA+ mB*dt+ mC*dt*dt + mD*dt*dt*dt;
+    return value;
+}
 //----------------------------------------------------------------------------------

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

@@ -58,9 +58,77 @@ public:
 	 */	
 	double GetValue(double s_check);
 
+    /**
+     * Returns gradient value at Sample S
+     */
+    double GetValueGradient(double s_check);
+
 };
 //----------------------------------------------------------------------------------
 
 
+/**
+ * Polynom of 2D third order
+ *
+ */
+
+
+class Third2DOrderPolynom
+{
+protected:
+    double mS;
+    double mT;
+    double mA;
+    double mB;
+    double mC;
+    double mD;
+public:
+    /**
+     * Constructor that initializes the polynom with base properties
+     *
+     * @param s S offset
+     * @param a A parameter of the polynom
+     * @param b B parameter of the polynom
+     * @param c C parameter of the polynom
+     * @param d D parameter of the polynom
+     */
+    Third2DOrderPolynom (double s, double t, double a, double b, double c, double d);
+
+
+    /**
+     * Setters for base properties
+     */
+    void SetS(double s);
+    void SetT(double t);
+    void SetA(double a);
+    void SetB(double b);
+    void SetC(double c);
+    void SetD(double d);
+
+
+    /**
+     * Getters for base properties
+     */
+    double GetS();
+    double GetT();
+    double GetA();
+    double GetB();
+    double GetC();
+    double GetD();
+
+
+    /**
+     * Method to check if sample S is inside the record interval
+     */
+    bool CheckInterval (double s_check,double t_check);
+
+    /**
+     * Returns the value at sample S
+     */
+    double GetValue(double t_check);
+
+};
+//----------------------------------------------------------------------------------
+
 
 #endif

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

@@ -3,7 +3,7 @@
 #include <math.h>
 
 
-
+#include <iostream>
 
 //***********************************************************************************
 //Road segment
@@ -64,6 +64,9 @@ Road::Road (const Road& road)
     mLaneOffsetVector=road.mLaneOffsetVector;
     mRoadBorrowVector = road.mRoadBorrowVector;
     mRoadNoavoidVector = road.mRoadNoavoidVector;
+    mSurfaceCRGVector = road.mSurfaceCRGVector;
+    mRoadPriorityVector = road.mRoadPriorityVector;
+    mShapeVector = road.mShapeVector;
 
 }
 
@@ -109,6 +112,9 @@ const Road& Road::operator=(const Road& otherRoad)
         mLaneOffsetVector=otherRoad.mLaneOffsetVector;
         mRoadBorrowVector = otherRoad.mRoadBorrowVector;
         mRoadNoavoidVector = otherRoad.mRoadNoavoidVector;
+        mSurfaceCRGVector = otherRoad.mSurfaceCRGVector;
+        mRoadPriorityVector = otherRoad.mRoadPriorityVector;
+        mShapeVector = otherRoad.mShapeVector;
 	}
 	return *this;
 }
@@ -241,6 +247,22 @@ unsigned int Road::GetElevationCount()
 {
 	return mElevationVector.size();
 }
+// Road Shape records
+vector<Shape> *Road::GetShapeVector()
+{
+    return &mShapeVector;
+}
+Shape*	Road::GetShape(unsigned int i)
+{
+    if ((mShapeVector.size()>0)&&(i<mShapeVector.size()))
+        return &mShapeVector.at(i);
+    else
+        return NULL;
+}
+unsigned int Road::GetShapeCount()
+{
+    return static_cast<unsigned int>(mShapeVector.size()) ;
+}
 // Road superelevation records
 vector<SuperElevation> *Road::GetSuperElevationVector()
 {
@@ -287,7 +309,7 @@ LaneSection*	Road::GetLaneSection(unsigned int i)
 }
 unsigned int Road::GetLaneSectionCount()
 {
-	return mLaneSectionsVector.size();
+    return static_cast<unsigned int>(mLaneSectionsVector.size()) ;
 }
 
 // Road lane offset records
@@ -306,6 +328,66 @@ unsigned int Road::GetLaneOffsetCount()
 {
     return mLaneOffsetVector.size();
 }
+
+//Road Bridge records
+
+vector<Objects_bridge> *Road::GetObjectsBridgeVector()
+{
+    return  &mObjectsBridgeVector;
+}
+
+Objects_bridge*	Road::GetObjectsBridge(unsigned int i)
+{
+    if ((mObjectsBridgeVector.size()>0)&&(i<mObjectsBridgeVector.size()))
+        return &mObjectsBridgeVector.at(i);
+    else
+        return NULL;
+}
+
+
+unsigned int Road::GetObjectsBridgeCount()
+{
+    return  static_cast<unsigned int>(mObjectsBridgeVector.size()) ;
+}
+
+//Road Tunnel records
+vector<Objects_tunnel> *Road::GetObjectsTunnelVector()
+{
+    return &mObjectsTunnelVector;
+}
+
+Objects_tunnel*	Road::GetObjectsTunnel(unsigned int i)
+{
+    if ((mObjectsTunnelVector.size()>0)&&(i<mObjectsTunnelVector.size()))
+        return &mObjectsTunnelVector.at(i);
+    else
+        return NULL;
+}
+
+unsigned int Road::GetObjectsTunnelCount()
+{
+    return  static_cast<unsigned int>(mObjectsTunnelVector.size()) ;
+}
+
+// Road objectReference records
+vector<Objects_objectReference> *Road::GetObjectsObjectReferenceVector()
+{
+    return &mObjectsObjectReferenceVector;
+}
+
+Objects_objectReference*	Road::GetObjectsObjectReference(unsigned int i)
+{
+    if ((mObjectsObjectReferenceVector.size()>0)&&(i<mObjectsObjectReferenceVector.size()))
+        return &mObjectsObjectReferenceVector.at(i);
+    else
+        return NULL;
+}
+
+unsigned int Road::GetObjectsObjectReferenceCount()
+{
+    return  static_cast<unsigned int>(mObjectsObjectReferenceVector.size()) ;
+}
+
 // Road object records
 vector<Object> *Road::GetObjectVector()
 {
@@ -343,8 +425,27 @@ Signal*	Road::GetSignal(unsigned int i)
 }
 unsigned int Road::GetSignalCount()
 {
-	return mSignalsVector.size();
+    return static_cast<unsigned int >(mSignalsVector.size()) ;
+}
+
+// Road signal Reference records
+vector<signals_signalReference> *Road::GetSignalReferenceVector()
+{
+    return  &mSignalReferenceVector;
 }
+signals_signalReference*	Road::GetSignalReference(unsigned int i)
+{
+    if ((mSignalReferenceVector.size()>0)&&(i<mSignalReferenceVector.size()))
+        return &mSignalReferenceVector.at(i);
+    else
+        return NULL;
+}
+unsigned int Road::GetSignalReferencCount()
+{
+    return static_cast<unsigned int >(mSignalReferenceVector.size()) ;
+}
+
+
 unsigned int Road::GetRoadBorrowCount()
 {
     return mRoadBorrowVector.size();
@@ -375,6 +476,45 @@ unsigned int Road::GetRoadNoavoidCount()
 {
     return mRoadNoavoidVector.size();
 }
+//-------------------------------------------------
+vector<surface_CRG> *Road::GetSurfaceCRGVector()
+{
+    return &mSurfaceCRGVector;
+}
+
+surface_CRG*	Road::GetSurfaceCRG(unsigned int i)
+{
+    if((mSurfaceCRGVector.size()>0)&&(i<mSurfaceCRGVector.size()))
+        return &mSurfaceCRGVector.at(i);
+    else
+        return NULL;
+}
+
+unsigned int Road::GetSurfaceCRGCount()
+{
+    return static_cast<unsigned int>(mSurfaceCRGVector.size());
+}
+
+//-------------------------------------------------
+
+vector<Railroad_Switch> *Road::GetRailroadSwitchVector()
+{
+    return &mRailroadSwitchVector;
+}
+
+Railroad_Switch*	Road::GetRailroadSwitch(unsigned int i)
+{
+    if((mRailroadSwitchVector.size()>0)&&(i<mRailroadSwitchVector.size()))
+        return &mRailroadSwitchVector.at(i);
+    else
+        return NULL;
+}
+
+unsigned int Road::GetRailroadSwitchCount()
+{
+    return static_cast<unsigned int>(mRailroadSwitchVector.size());
+}
+
 //-------------------------------------------------
 
 /**
@@ -403,6 +543,13 @@ Elevation*	Road::GetLastElevation()
 	else
 		return NULL;
 }
+Shape*      Road::GetLastShape()
+{
+    if(mShapeVector.size()>0)
+        return &mShapeVector.at(mShapeVector.size()-1);
+    else
+        return  NULL;
+}
 SuperElevation*	Road::GetLastSuperElevation()
 {	
 	if (mSuperElevationVector.size()>0)
@@ -424,6 +571,27 @@ LaneSection*	Road::GetLastLaneSection()
 	else
 		return NULL;
 }
+Objects_bridge* Road::GetLastObjectsBridge()
+{
+    if (mObjectsBridgeVector.size()>0)
+        return &mObjectsBridgeVector.at(mObjectsBridgeVector.size()-1);
+    else
+        return NULL;
+}
+Objects_objectReference* Road::GetLastObjectsObjectReference()
+{
+    if (mObjectsObjectReferenceVector.size()>0)
+        return &mObjectsObjectReferenceVector.at(mObjectsObjectReferenceVector.size()-1);
+    else
+        return NULL;
+}
+Objects_tunnel* Road::GetLastObjectsTunnel()
+{
+    if (mObjectsTunnelVector.size()>0)
+        return &mObjectsTunnelVector.at(mObjectsTunnelVector.size()-1);
+    else
+        return NULL;
+}
 Object*	Road::GetLastObject()
 {	
 	if (mObjectsVector.size()>0)
@@ -438,6 +606,15 @@ Signal*	Road::GetLastSignal()
 	else
 		return NULL;
 }
+
+signals_signalReference* Road::GetLastSignalReference()
+{
+    if (mSignalReferenceVector.size()>0)
+        return &mSignalReferenceVector.at(mSignalReferenceVector.size()-1);
+    else
+        return NULL;
+}
+
 RoadBorrow* Road::GetLastRoadBorrow()
 {
     if(mRoadBorrowVector.size()>0)
@@ -454,6 +631,22 @@ RoadNoavoid* Road::GetLastRoadNoavoid()
         return NULL;
 }
 
+surface_CRG *       Road::GetLastSurfaceCRG()
+{
+    if(mSurfaceCRGVector.size()>0)
+        return &mSurfaceCRGVector.at(mSurfaceCRGVector.size()-1);
+    else
+        return NULL;
+}
+
+Railroad_Switch * Road::GetLastRailroadSwitch()
+{
+    if(mRailroadSwitchVector.size()>0)
+        return &mRailroadSwitchVector.at(mRailroadSwitchVector.size()-1);
+    else
+        return NULL;
+}
+
 
 /**
  * Getters for the last added child records in their respective vectors
@@ -479,6 +672,13 @@ Elevation* Road::GetLastAddedElevation()
 	else
 		return NULL;
 }
+Shape*     Road::GetLastAddedShape()
+{
+    if(mLastAddedShape<mShapeVector.size())
+        return &mShapeVector.at(mLastAddedShape);
+    else
+        return NULL;
+}
 SuperElevation* Road::GetLastAddedSuperElevation()
 {
 	if(mLastAddedSuperElevation<mSuperElevationVector.size())
@@ -500,6 +700,27 @@ LaneSection* Road::GetLastAddedLaneSection()
 	else
 		return NULL;
 }
+Objects_bridge* Road::GetLastAddedObjectsBridge()
+{
+    if(mLastAddedObjectsBridge<mObjectsBridgeVector.size())
+        return &mObjectsBridgeVector.at(mLastAddedObjectsBridge);
+    else
+        return NULL;
+}
+Objects_tunnel* Road::GetLastAddedObjectsTunnel()
+{
+    if(mLastAddedObjectsTunnel<mObjectsTunnelVector.size())
+        return &mObjectsTunnelVector.at(mLastAddedObjectsTunnel);
+    else
+        return NULL;
+}
+Objects_objectReference* Road::GetLastAddedObjectsObjectReference()
+{
+    if(mLastAddedObjectsObjectReference<mObjectsObjectReferenceVector.size())
+        return &mObjectsObjectReferenceVector.at(mLastAddedObjectsObjectReference);
+    else
+        return NULL;
+}
 Object* Road::GetLastAddedObject()
 {
 	if(mLastAddedObject<mObjectsVector.size())
@@ -521,6 +742,17 @@ RoadBorrow* Road::GetLastAddedRoadBorrow()
     else
         return NULL;
 }
+
+signals_signalReference* Road::GetLastAddedSignalReference()
+{
+    if(mLastAddedSignalReference<mSignalReferenceVector.size())
+    {
+        return &mSignalReferenceVector.at(mLastAddedSignalReference);
+    }
+    else
+        return NULL;
+}
+
 RoadNoavoid* Road::GetLastAddedRoadNoavoid()
 {
     if(mLastAddedRoadNoavoid<mRoadNoavoidVector.size())
@@ -528,6 +760,21 @@ RoadNoavoid* Road::GetLastAddedRoadNoavoid()
     else
         return NULL;
 }
+surface_CRG*        Road::GetLastAddedSurfaceCRG()
+{
+    if(mLastAddedSurfaceCRG<mSurfaceCRGVector.size())
+        return &mSurfaceCRGVector.at(mLastAddedSurfaceCRG);
+    else
+        return NULL;
+}
+
+Railroad_Switch * Road::GetLastAddedRailroadSwitch()
+{
+    if(mLastAddedRailroadSwitch<mRailroadSwitchVector.size())
+        return &mRailroadSwitchVector.at(mLastAddedRailroadSwitch);
+    else
+        return NULL;
+}
 //-------------------------------------------------
 
 /**
@@ -667,6 +914,17 @@ unsigned int Road::AddElevation(double s, double a, double b, double c, double d
 	return index;
 }
 //-------------
+unsigned int Road::AddShape(double s, double t, double a, double b, double c, double d)
+{
+    // Check the first method in the group for details
+
+    unsigned int index = CheckShapeInterval(s,t)+1;
+    if(index>=GetShapeCount()) mShapeVector.push_back(Shape(s,t,a,b,c,d));
+    else mShapeVector.insert(mShapeVector.begin()+index, Shape(s,t,a,b,c,d));
+    mLastAddedShape=index;
+    return index;
+}
+//-------------
 unsigned int Road::AddSuperElevation(double s, double a, double b, double c, double d)
 {	
 	// Check the first method in the group for details
@@ -727,6 +985,49 @@ unsigned int Road::AddRoadNoavoid(double s, double length)
     return index;
 }
 
+unsigned int Road::AddSurfaceCRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode)
+{
+    unsigned int index = CheckSurfaceCRGInterval(sStart)+1;
+    if(index>=GetSurfaceCRGCount()) mSurfaceCRGVector.push_back(surface_CRG(file, sStart, sEnd, orientation, mode));
+    else mSurfaceCRGVector.insert(mSurfaceCRGVector.begin()+index, surface_CRG(file, sStart, sEnd, orientation, mode));
+    mLastAddedSurfaceCRG=index;
+    return index;
+
+}
+
+unsigned int Road::AddRailroadSwitch(std::string name, std::string id,std::string position, Railroad_Switch_MainTrack mainTrack, Railroad_Switch_SideTrack sideTrack)
+{
+    mRailroadSwitchVector.push_back(Railroad_Switch(name,id,position,mainTrack,sideTrack));
+    return static_cast<unsigned int >(mRailroadSwitchVector.size() -1);
+}
+
+//-------------
+unsigned int Road::AddObjectsTunnel(double s,double length,string id,string type)
+{
+    unsigned int index = static_cast<unsigned int>(CheckRoadObjectsTunnelInterval(s)+1) ;
+    if(index>=GetObjectsTunnelCount()) mObjectsTunnelVector.push_back(Objects_tunnel(s,length,id,type));
+    else mObjectsTunnelVector.insert(mObjectsTunnelVector.begin()+index, Objects_tunnel(s,length,id,type));
+    mLastAddedObjectsTunnel=index;
+    return index;
+}
+//-------------
+unsigned int Road::AddObjectsObjectReference(double s,double t,string id,string orientation)
+{
+    unsigned int index = static_cast<unsigned int>(CheckRoadObjectsTunnelInterval(s)+1) ;
+    if(index>=GetObjectsObjectReferenceCount()) mObjectsObjectReferenceVector.push_back(Objects_objectReference(s,t,id,orientation));
+    else mObjectsObjectReferenceVector.insert(mObjectsObjectReferenceVector.begin()+index, Objects_objectReference(s,t,id,orientation));
+    mLastAddedObjectsObjectReference=index;
+    return index;
+}
+//-------------
+unsigned int Road::AddObjectsBridge(double s,double length,string id,string type)
+{
+    unsigned int index = static_cast<unsigned int>(CheckRoadObjectsBridgeInterval(s)+1) ;
+    if(index>=GetObjectsBridgeCount()) mObjectsBridgeVector.push_back(Objects_bridge(s,length,id,type));
+    else mObjectsBridgeVector.insert(mObjectsBridgeVector.begin()+index, Objects_bridge(s,length,id,type));
+    mLastAddedObjectsBridge=index;
+    return index;
+}
 //-------------
 unsigned int Road::AddObject(string id,double s,double t,double zOffset)
 {	
@@ -743,18 +1044,26 @@ unsigned int Road::AddSignal(double s,double t,string id,string name,bool dynami
 {
 	// Check the first method in the group for details
 
-	unsigned int index=GetSignalCount();
-    Signal x(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
-             subtype,hOffset,pitch,roll,height,width);
-    mSignalsVector.push_back(x);
+    unsigned int index = static_cast<unsigned int>(CheckSignalInterval(s)+1) ;
+    if(index>=GetSignalCount()) mSignalsVector.push_back(    Signal(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
+                                                                      subtype,hOffset,pitch,roll,height,width));
+    else mSignalsVector.insert(mSignalsVector.begin()+index, Signal(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
+                                                                    subtype,hOffset,pitch,roll,height,width));
+    mLastAddedSignal=index;
+    return index;
 
-//    mSignalsVector.push_back(Signal(s,t,id,name,dynamic,orientation,zOffset,type,country,countryRevision,
-//                                    subtype,hOffset,pitch,roll,height,width));
-	mLastAddedSignal=index;
-	return index;
 }
 //-----------
 
+unsigned int Road::AddSignalReference(double s,double t,std::string id,std::string orientation)
+{
+    unsigned int index = static_cast<unsigned int>(CheckSignalReferencInterval(s)+1) ;
+    if(index>=GetSignalReferencCount()) mSignalReferenceVector.push_back(signals_signalReference(s,t,id,orientation));
+    else mSignalReferenceVector.insert(mSignalReferenceVector.begin()+index, signals_signalReference(s,t,id,orientation));
+    mLastAddedSignalReference=index;
+    return index;
+}
+//-----------
 
 /**
  * Methods used to clone child records in the respective vectors
@@ -782,6 +1091,17 @@ unsigned int Road::CloneElevation(unsigned int index)
 	mLastAddedElevation=index+1;
 	return mLastAddedElevation;
 }
+unsigned int Road::CloneShape(unsigned int index)
+{
+    // Check the first method in the group for details
+
+    if(index<mShapeVector.size()-1)
+        mShapeVector.insert(mShapeVector.begin()+index+1, mShapeVector[index]);
+    else if(index==mShapeVector.size()-1)
+        mShapeVector.push_back(mShapeVector[index]);
+    mLastAddedShape=index+1;
+    return mLastAddedShape;
+}
 unsigned int Road::CloneSuperElevation(unsigned int index)
 {
 	// Check the first method in the group for details
@@ -905,6 +1225,33 @@ unsigned int Road::CloneLaneSectionEnd(unsigned int index)
 	mLastAddedLaneSection=index+1;
 	return mLastAddedLaneSection;
 }
+unsigned int Road::CloneObjectsBridge(unsigned int index)
+{
+    if(index<mObjectsBridgeVector.size()-1)
+        mObjectsBridgeVector.insert(mObjectsBridgeVector.begin()+index+1, mObjectsBridgeVector[index]);
+    else if(index==mObjectsBridgeVector.size()-1)
+        mObjectsBridgeVector.push_back(mObjectsBridgeVector[index]);
+    mLastAddedObjectsBridge=index+1;
+    return mLastAddedObjectsBridge;
+}
+unsigned int Road::CloneObjectsTunnel(unsigned int index)
+{
+    if(index<mObjectsTunnelVector.size()-1)
+        mObjectsTunnelVector.insert(mObjectsTunnelVector.begin()+index+1, mObjectsTunnelVector[index]);
+    else if(index==mObjectsTunnelVector.size()-1)
+        mObjectsTunnelVector.push_back(mObjectsTunnelVector[index]);
+    mLastAddedObjectsTunnel=index+1;
+    return mLastAddedObjectsTunnel;
+}
+unsigned int Road::CloneObjectsObjectReference(unsigned int index)
+{
+    if(index<mObjectsObjectReferenceVector.size()-1)
+        mObjectsObjectReferenceVector.insert(mObjectsObjectReferenceVector.begin()+index+1, mObjectsObjectReferenceVector[index]);
+    else if(index==mObjectsObjectReferenceVector.size()-1)
+        mObjectsObjectReferenceVector.push_back(mObjectsObjectReferenceVector[index]);
+    mLastAddedObjectsObjectReference=index+1;
+    return mLastAddedObjectsObjectReference;
+}
 unsigned int Road::CloneObject(unsigned int index)
 {
 	// Check the first method in the group for details
@@ -927,6 +1274,15 @@ unsigned int Road::CloneSignal(unsigned int index)
 	mLastAddedSignal=index+1;
 	return mLastAddedSignal;
 }
+unsigned int Road::CloneSignalReference(unsigned int index)
+{
+    if(index<mSignalReferenceVector.size()-1)
+        mSignalReferenceVector.insert(mSignalReferenceVector.begin()+index+1, mSignalReferenceVector[index]);
+    else if(index==mSignalReferenceVector.size()-1)
+        mSignalReferenceVector.push_back(mSignalReferenceVector[index]);
+    mLastAddedSignalReference=index+1;
+    return mLastAddedSignalReference;
+}
 unsigned int Road::CloneRoadBorrow(unsigned int index)
 {
     // Check the first method in the group for details
@@ -949,6 +1305,26 @@ unsigned int Road::CloneRoadNoavoid(unsigned int index)
     return mLastAddedRoadNoavoid;
 }
 
+unsigned int Road::CloneSurfaceCRG(unsigned int index)
+{
+    if(index<mSurfaceCRGVector.size()-1)
+        mSurfaceCRGVector.insert(mSurfaceCRGVector.begin()+index+1, mSurfaceCRGVector[index]);
+    else if(index==mSurfaceCRGVector.size()-1)
+        mSurfaceCRGVector.push_back(mSurfaceCRGVector[index]);
+    mLastAddedSurfaceCRG=index+1;
+    return mLastAddedSurfaceCRG;
+}
+
+unsigned int Road::CloneRailroadSwitch(unsigned int index)
+{
+    if(index<mRailroadSwitchVector.size()-1)
+        mRailroadSwitchVector.insert(mRailroadSwitchVector.begin()+index+1, mRailroadSwitchVector[index]);
+    else if(index==mRailroadSwitchVector.size()-1)
+        mRailroadSwitchVector.push_back(mRailroadSwitchVector[index]);
+    mLastAddedRailroadSwitch=index+1;
+    return mLastAddedRailroadSwitch;
+}
+
 
 /**
  * Methods used to delete child records from the respective vectors
@@ -965,6 +1341,10 @@ void Road::DeleteElevation(unsigned int index)
 {
 	mElevationVector.erase(mElevationVector.begin()+index);
 }
+void Road::DeleteShape(unsigned int index)
+{
+    mShapeVector.erase(mShapeVector.begin()+index);
+}
 void Road::DeleteSuperElevation(unsigned int index)
 {
 	mSuperElevationVector.erase(mSuperElevationVector.begin()+index);
@@ -981,6 +1361,18 @@ void Road::DeleteLaneOffset(unsigned int index)
 {
     mLaneOffsetVector.erase(mLaneOffsetVector.begin()+index);
 }
+void Road::DeleteObjectsBridge(unsigned int index)
+{
+    mObjectsBridgeVector.erase(mObjectsBridgeVector.begin()+index);
+}
+void Road::DeleteObjectsTunnel(unsigned int index)
+{
+    mObjectsTunnelVector.erase(mObjectsTunnelVector.begin()+index);
+}
+void Road::DeleteObjectsObjectReference(unsigned int index)
+{
+    mObjectsObjectReferenceVector.erase(mObjectsObjectReferenceVector.begin()+index);
+}
 void Road::DeleteObject(unsigned int index)
 {
 	mObjectsVector.erase(mObjectsVector.begin()+index);
@@ -989,6 +1381,10 @@ void Road::DeleteSignal(unsigned int index)
 {
 	mSignalsVector.erase(mSignalsVector.begin()+index);
 }
+void Road::DeleteSignalReference(unsigned int index)
+{
+    mSignalReferenceVector.erase(mSignalReferenceVector.begin()+index);
+}
 
 void Road::DeleteRoadBorrow(unsigned int index)
 {
@@ -1002,6 +1398,16 @@ void Road::DeleteRoadNoavoid(unsigned int index)
     mRoadNoavoidVector.erase(mRoadNoavoidVector.begin()+index);
 }
 
+void Road::DeleteSurfaceCRG(unsigned int index)
+{
+    mSurfaceCRGVector.erase(mSurfaceCRGVector.begin()+index);
+}
+
+void Road::DeleteRailroadSwitch(unsigned int index)
+{
+    mRailroadSwitchVector.erase(mRailroadSwitchVector.begin()+index);
+}
+
 //-------------------------------------------------
 // EVALUATION METHODS
 
@@ -1125,7 +1531,7 @@ double Road::GetRoadRightWidth(double s_check)
     LaneSection * pLS =&mLaneSectionsVector[0];
     if(mLaneSectionsVector.size() > 1)
     {
-        unsigned int nLSCount = mLaneSectionsVector.size();
+        unsigned int nLSCount = static_cast<unsigned int>(mLaneSectionsVector.size()) ;
         unsigned int i;
         for(i=0;i<(nLSCount -1);i++)
         {
@@ -1139,7 +1545,7 @@ double Road::GetRoadRightWidth(double s_check)
 double Road::GetLaneOffsetValue(double s_check)
 {
     if(mLaneOffsetVector.size() == 0)return 0.0;
-    unsigned int noffsetcount = mLaneOffsetVector.size();
+    unsigned int noffsetcount = static_cast<unsigned int >(mLaneOffsetVector.size()) ;
     unsigned int i;
     LaneOffset * pLO = NULL;
     for(i=0;i<noffsetcount;i++)
@@ -1237,6 +1643,33 @@ double  Road::GetElevationValue (double s_check)
 		retVal= (mElevationVector.at(index).GetValue(s_check));
 	return retVal;
 
+}
+//-----------
+int  Road::CheckShapeInterval(double s_check,double t_check)
+{
+    int res=-1;
+    //Go through all the road type records
+    for (unsigned int i=0;i<mShapeVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mShapeVector.at(i).CheckInterval(s_check,t_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
+}
+//-----------
+double  Road::GetShapeValue (double s_check,double t_check)
+{
+    double retVal=0;
+    //find the record where s_check belongs
+    int index=CheckShapeInterval(s_check,t_check);
+    //If found, return the type
+    if (index>=0)
+        retVal= (mShapeVector.at(index).GetValue(t_check));
+    return retVal;
+
 }
 //-----------
 int  Road::CheckSuperElevationInterval(double s_check)
@@ -1358,15 +1791,15 @@ int Road::CheckRoadNoavoidInterval(double s_check)
 
 }
 //-----------
-int Road::CheckLaneOffsetInterval(double s_check)
+int Road::CheckRoadObjectsBridgeInterval(double s_check)
 {
     int res=-1;
-    //Go through all the lane section records
-    for (unsigned int i=0;i<mLaneOffsetVector.size();i++)
+    //Go through all the bridge records
+    for (unsigned int i=0;i<mObjectsBridgeVector.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
+        if (mObjectsBridgeVector.at(i).CheckInterval(s_check))
+            res=static_cast<int>(i);	//assign it to the result id
         else
             break;	//if not, break;
     }
@@ -1374,330 +1807,1527 @@ int Road::CheckLaneOffsetInterval(double s_check)
 
 }
 //-----------
-void  Road::FillLaneSectionSample(double s_check, LaneSectionSample& laneSectionSample)
+int Road::CheckRoadObjectsTunnelInterval(double s_check)
 {
-	int index=CheckLaneSectionInterval(s_check);
-	if (index>=0)
-		mLaneSectionsVector.at(index).FillLaneSectionSample(s_check,laneSectionSample);
-}
-
-//-------------------------------------------------
+    int res=-1;
+    //Go through all the tunnel records
+    for (unsigned int i=0;i<mObjectsTunnelVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mObjectsTunnelVector.at(i).CheckInterval(s_check))
+            res=static_cast<int>(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
 
-/**
- * Destructor
- */
-Road::~Road()
+}
+//-----------
+int Road::CheckRoadObjectsObjectReferenceInterval(double s_check)
 {
-	delete mPredecessor;
-	delete mSuccessor;
-	delete mNeighbor1;
-	delete mNeighbor2;
+    int res=-1;
+    //Go through all the tunnel records
+    for (unsigned int i=0;i<mObjectsObjectReferenceVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mObjectsObjectReferenceVector.at(i).CheckInterval(s_check))
+            res=static_cast<int>(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 Road::CheckSignalInterval(double s_check)
+{
+    int res=-1;
+    //Go through all the tunnel records
+    for (unsigned int i=0;i<mSignalsVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mSignalsVector.at(i).CheckInterval(s_check))
+            res=static_cast<int>(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 Road::CheckSignalReferencInterval(double s_check)
+{
+    int res=-1;
+    //Go through all the tunnel records
+    for (unsigned int i=0;i<mSignalReferenceVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mSignalReferenceVector.at(i).CheckInterval(s_check))
+            res=static_cast<int>(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 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
+
+}
+//-----------
+int Road::CheckSurfaceCRGInterval(double s_check)
+{
+    int res=-1;
+    //Go through all the lane section records
+    for (unsigned int i=0;i<mSurfaceCRGVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mSurfaceCRGVector.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);
+	if (index>=0)
+		mLaneSectionsVector.at(index).FillLaneSectionSample(s_check,laneSectionSample);
+}
+
+//-------------------------------------------------
+
+std::vector<double> Road::GetDrivingLaneWidthVector(double s_check,int nlr)
+{
+    std::vector<double> xrtn;
+    xrtn.clear();
+
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  xrtn;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return xrtn;
+
+    if(nlr == 1)
+    {
+        unsigned int nlanecount = pLS->GetLeftLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+
+
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+            if(pLane->GetType() != "driving")
+            {
+                break;
+            }
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+    else
+    {
+        unsigned int nlanecount = pLS->GetRightLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetRightLaneAt(i+1);
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+            if(pLane->GetType() != "driving")
+            {
+                break;
+            }
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+
+
+
+    return  xrtn;
+}
+//-------------------------------------------------
+
+int Road::GetLeftDrivingLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return -1;
+
+    int nrtn;
+
+    unsigned int nlanecount = pLS->GetLeftLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+
+
+        if(pLane == NULL)
+        {
+            return nrtn;
+        }
+        if(pLane->GetType() != "driving")
+        {
+            break;
+        }
+        nrtn++;
+
+
+    }
+
+    return nrtn;
+}
+
+//-------------------------------------------------
+
+int Road::GetRightDrivingLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return -1;
+
+    int nrtn;
+
+    unsigned int nlanecount = pLS->GetRightLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetRightLaneAt(i+1);
+
+
+
+        if(pLane == NULL)
+        {
+            return nrtn;
+        }
+        if(pLane->GetType() != "driving")
+        {
+            break;
+        }
+        nrtn++;
+
+
+    }
+
+    return nrtn;
+}
+
+//-------------------------------------------------
+
+std::vector<double> Road::GetLaneWidthVector(double s_check,int nlr) //Lane Width, From Refline to side. nlr 1 left 2 right
+{
+    std::vector<double> xrtn;
+    xrtn.clear();
+
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  xrtn;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return xrtn;
+
+    if(nlr == 1)
+    {
+        unsigned int nlanecount = pLS->GetLeftLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+    else
+    {
+        unsigned int nlanecount = pLS->GetRightLaneCount();
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLS->GetRightLaneAt(i+1);
+            if(pLane == NULL)
+            {
+                std::cout<<" Road::GetLaneWidthVector Fail."<<" s_check: "<<s_check<<" nlr: "<<nlr<<std::endl;
+                return xrtn;
+            }
+
+           double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+           xrtn.push_back(fwidth);
+
+
+        }
+    }
+
+
+
+    return  xrtn;
+}
+
+//-------------------------------------------------
+
+double Road::GetLeftRoadWidth(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    double xrtn;
+
+    unsigned int nlanecount = pLS->GetLeftLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLeftLaneAt(i+1);
+
+        if(pLane == NULL)
+        {
+            std::cout<<" Road::GetLeftRoadWidth Fail."<<" s_check: "<<s_check<<std::endl;
+            return 0;
+        }
+       double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+       xrtn = xrtn + fwidth;
+
+
+    }
+    return xrtn;
+}
+
+//-------------------------------------------------
+
+
+double Road::GetRightRoadWidth(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    double xrtn;
+
+    unsigned int nlanecount = pLS->GetRightLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetRightLaneAt(i+1);
+        if(pLane == NULL)
+        {
+            std::cout<<" Road::GetRightRoadWidth Fail."<<" s_check: "<<s_check<<std::endl;
+            return 0;
+        }
+
+       double fwidth = pLane->GetWidthValue(s_check - pLS->GetS());
+
+       xrtn = xrtn + fwidth;
+
+
+    }
+
+    return  xrtn;
+
+
+}
+
+//-------------------------------------------------
+
+int Road::GetLeftLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return 0;
+
+    return static_cast<int>(pLS->GetLeftLaneCount());
+}
+
+//-------------------------------------------------
+
+
+
+int Road::GetRightLaneCount(double s_check)
+{
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  0;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return -1;
+
+    return static_cast<int>(pLS->GetRightLaneCount());
+}
+
+
+//-------------------------------------------------
+
+LaneRoadMark  Road::GetLaneRoadMarkValue(double s_check,int nlane)
+{
+    LaneRoadMark xRoadMark;
+    LaneSection * pLS = NULL;
+
+    unsigned int nsecCount = GetLaneSectionCount();
+    unsigned int i;
+    if(nsecCount == 0)return  xRoadMark;
+    for(i=0;i<nsecCount;i++)
+    {
+        if(GetLaneSection(i)->GetS()>s_check)break;
+        pLS = GetLaneSection(i);
+    }
+
+    if(pLS == NULL)return xRoadMark;
+
+    Lane * pLane = NULL;
+
+    if(nlane == 0)pLane = pLS->GetCenterLane();
+    else
+    {
+        if(nlane<0)pLane = pLS->GetRightLaneAt(static_cast<unsigned int>(abs(nlane)));
+        else
+            pLane = pLS->GetLeftLaneAt(static_cast<unsigned int>(nlane));
+    }
+
+    if(pLane == NULL)
+    {
+        std::cout<<"Road::GetLaneRoadMark "<<"Can't Find lane s:"<<s_check<<" lane: "<<nlane<<std::endl;
+        return xRoadMark;
+    }
+
+    return pLane->GetRoadMarkValue(s_check - pLS->GetS());
+
+
+}
+
+
+
+//-------------------------------------------------
+
+void Road::SetRoadPriority(int nPriority)
+{
+    if(mRoadPriorityVector.size()==0)
+        mRoadPriorityVector.push_back(RoadPriority(nPriority));
+    else
+        mRoadPriorityVector[0].SetPriority(nPriority);
+}
+
+//-------------------------------------------------
+
+int Road::GetRoadPriority(int & nPriority)
+{
+    if(mRoadPriorityVector.size() == 0)return 0;
+    nPriority = mRoadPriorityVector[0].GetPriority();
+    return 1;
+}
+
+//-------------------------------------------------
+
+void Road::ResetPriority()
+{
+    if(mRoadPriorityVector.size()>0)mRoadPriorityVector.clear();
+}
+
+//-------------------------------------------------
+
+double Road::GetDis(const double x,const double y, const double hdg, double & fRefDis, double & fHdgDiff, int & nlane,double & s,double & refx,double & refy,double & refhdg)
+{
+    unsigned int ngbsize = static_cast<unsigned int >(mGeometryBlockVector.size());
+    unsigned int i;
+    double fdismin = 1000000.0;
+    for(i=0;i<ngbsize;i++)
+    {
+        GeometryBlock * pgb = GetGeometryBlock(i);
+        RoadGeometry * pg;
+        pg = pgb->GetGeometryAt(0);
+        double xstart,ystart;
+        xstart = pg->GetX();
+        ystart = pg->GetY();
+        double fdisstart = sqrt(pow(xstart - x,2) + pow(ystart - y,2));
+        double sstart = pg->GetS();
+        double geolen = pg->GetLength();
+
+        if(fdismin > fdisstart)
+        {
+            fdismin = fdisstart;
+            fRefDis = fdismin;
+            nlane = 1000;
+            s = sstart;
+            refx = xstart;
+            refy = ystart;
+            refhdg = pg->GetHdg();
+            fHdgDiff = hdg - refhdg;
+            if(fHdgDiff<(-M_PI))fHdgDiff = fHdgDiff + 2.0*M_PI;
+            if(fHdgDiff>M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+        }
+
+        if(fdisstart<(100.0 + geolen))
+        {
+            double fstep = 0.1;
+            double fsnow = 0.1;
+            while(fsnow<=geolen)
+            {
+                double xnow,ynow,hdgnow;
+                GetGeometryCoords(fsnow,xnow,ynow,hdgnow);
+                fsnow = fsnow + fstep;
+                double fdisnow = sqrt(pow(xnow - x,2)+pow(ynow - y,2));
+                if(fdisnow < fdismin)
+                {
+                    fdismin = fdisnow;;
+                    fRefDis = fdismin;
+                    nlane = 1000;
+                    s = fsnow;
+                    refx = xnow;
+                    refy = ynow;
+                    refhdg = hdgnow;
+                    fHdgDiff = hdg - hdgnow;
+                    if(fHdgDiff<(-M_PI))fHdgDiff = fHdgDiff + 2.0*M_PI;
+                    if(fHdgDiff>M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+                }
+            }
+        }
+    }
+
+    double frealdis = fRefDis;
+
+    if(fdismin < 100.0)
+    {
+        double fOffset = GetLaneOffsetValue(s);
+        double realx,realy,realhdg;
+        realhdg = refhdg;
+        if(fabs(fOffset)<0.00001)
+        {
+            realx = refx;
+            realy = refy;
+        }
+        else
+        {
+            realx = refx + fOffset * cos(refhdg + M_PI/2.0);
+            realy = refy + fOffset * sin(refhdg + M_PI/2.0);
+        }
+
+        if((fabs(realx - x)<0.001)&&(fabs(realy - y)<0.001))
+        {
+            if(GetRightDrivingLaneCount(s)>0)
+            {
+                nlane = -1;
+            }
+            else
+            {
+                if(GetLeftDrivingLaneCount(s)>0)
+                {
+                    nlane = 1;
+                }
+                else
+                {
+                    nlane = 0;
+                }
+            }
+        }
+        else
+        {
+            double x0,y0,x1,y1;
+            x0 = realx;
+            y0 = realy;
+            x1 = x;
+            y1 = y;
+            double hdgpoint;
+            if(x0 == x1)
+            {
+                if(y0 < y1)
+                {
+                    hdgpoint =  M_PI/2.0;
+                }
+                else
+                    hdgpoint =  M_PI*3.0/2.0;
+            }
+            else
+            {
+                double ratio = (y1-y0)/(x1-x0);
+                double hdgpoint = atan(ratio);
+                if(ratio > 0)
+                {
+                    if(y1 > y0)
+                    {
+
+                    }
+                    else
+                    {
+                        hdgpoint = hdg + M_PI;
+                    }
+                }
+                else
+                {
+                    if(y1 > y0)
+                    {
+                        hdgpoint = hdg + M_PI;
+                    }
+                    else
+                    {
+                        hdgpoint = hdg + 2.0*M_PI;
+                    }
+                }
+
+            }
+
+
+            hdgpoint = hdgpoint - refhdg;
+
+            while(hdgpoint>=M_PI)hdgpoint = hdgpoint -2.0*M_PI;
+            while(hdgpoint <(-M_PI))hdgpoint = hdgpoint +2.0*M_PI;
+
+            frealdis = sqrt(pow(realx - x,2)+pow(realy - y,2));
+
+            if(hdgpoint > 0)
+            {
+                if(GetLeftDrivingLaneCount(s)>0)
+                {
+                    std::vector<double> fvectorwidth =  GetDrivingLaneWidthVector(s,1);
+                    if(fvectorwidth.size()>0)
+                    {
+                        unsigned int i;
+                        unsigned int nwidthsize = fvectorwidth.size();
+                        for(i=0;i<nwidthsize;i++)
+                        {
+                            frealdis = frealdis - fvectorwidth[i];
+                            if(frealdis<=0)
+                            {
+                                nlane = static_cast<int>(i+1);
+                                frealdis = 0;
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+            else
+            {
+                if(GetRightDrivingLaneCount(s)>0)
+                {
+                    std::vector<double> fvectorwidth =  GetDrivingLaneWidthVector(s,2);
+                    if(fvectorwidth.size()>0)
+                    {
+                        unsigned int i;
+                        unsigned int nwidthsize = fvectorwidth.size();
+                        for(i=0;i<nwidthsize;i++)
+                        {
+                            frealdis = frealdis - fvectorwidth[i];
+                            if(frealdis<=0)
+                            {
+                                nlane = (static_cast<int>(i+1))*(-1);
+                                frealdis = 0;
+                                break;
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+    return frealdis;
+}
+
+
+short int Road::GetLaneCoords(double s_check, double t_check,double &retX, double &retY, double &retZ,double &retHDG)
+{
+    short int nrtn;
+    double fRefX,fRefY,fRefHDG;
+    nrtn = GetGeometryCoords(s_check,fRefX,fRefY,fRefHDG);
+    if(nrtn<0)
+    {
+        return nrtn;
+    }
+
+    double fOffValue = GetLaneOffsetValue(s_check);
+
+    double fSuperElevationValue = GetSuperElevationValue(s_check);
+
+    retHDG = fRefHDG;
+
+    retX = fRefX + t_check*cos(fSuperElevationValue)*cos(retHDG + M_PI/2.0) + fOffValue*cos(retHDG+M_PI/2.0);
+    retY = fRefY + t_check*cos(fSuperElevationValue)*sin(retHDG + M_PI/2.0) + fOffValue*sin(retHDG+M_PI/2.0);
+
+    double fElevation = GetElevationValue(s_check);
+
+    retZ = fElevation + t_check*sin(fSuperElevationValue) + GetShapeValue(s_check,t_check);
+
+    return nrtn;
+}
+
+//-------------------------------------------------
+
+/**
+ * Destructor
+ */
+Road::~Road()
+{
+	delete mPredecessor;
+	delete mSuccessor;
+	delete mNeighbor1;
+	delete mNeighbor2;
+
+	// DELETING ROAD TYPES
+	mRoadTypeVector.clear();
+
+	// DELETING GEOMETRY BLOKS
+	mGeometryBlockVector.clear();
+
+	// DELETING ELEVATIONS
+	mElevationVector.clear();
+
+	// DELETING SUPERELEVATION
+	mSuperElevationVector.clear();
+
+	// DELETING CROSSFALL
+	mCrossfallVector.clear();
+
+	// DELETING LANE SECTIONS
+	mLaneSectionsVector.clear();
+
+	// DELETING OBJECTS
+	mObjectsVector.clear();
+
+	// DELETING SIGNALS
+	mSignalsVector.clear();
+}
+
+
+
+
+//***********************************************************************************
+//Road Link Record
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+RoadLink::RoadLink(string elementType, string elementId, string contactPoint)
+{
+	mElementType=elementType;
+	mElementId=elementId;
+	mContactPoint=contactPoint;
+}
+
+/**
+ * Getters for the basic properties
+ */
+string RoadLink::GetElementType()
+{	
+	return mElementType;	
+}
+string RoadLink::GetElementId()
+{	
+	return mElementId;	
+}
+string RoadLink::GetContactPoint()
+{	
+	return mContactPoint;	
+}
+double RoadLink::GetElementS()
+{
+    return mElementS;
+}
+string RoadLink::GetElementDir()
+{
+    return mElementDir;
+}
+
+/**
+ * Setters for the basic properties
+ */
+void RoadLink::SetElementType(string elementType)
+{	
+	mElementType=elementType;	
+}
+void RoadLink::SetElementId(string elementId)
+{	
+	mElementId=elementId;
+}
+void RoadLink::SetContactPoint(string contactPoint)
+{	
+	mContactPoint=contactPoint;	
+}
+void RoadLink::SetElementS(double value)
+{
+    mElementS = value;
+}
+void RoadLink::SetELementDir(std::string elementdir)
+{
+    mElementDir = elementdir;
+}
+
+
+//***********************************************************************************
+//Road Neighbor Record
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+RoadNeighbor::RoadNeighbor(string side, string elementId, string direction)
+{
+	mSide=side;
+	mElementId=elementId;
+	mDirection=direction;
+}
+/**
+ * Getters for the basic properties
+ */
+string RoadNeighbor::GetSide()
+{	
+	return mSide;	
+}
+string RoadNeighbor::GetElementId()
+{	
+	return mElementId;	
+}
+string RoadNeighbor::GetDirection()
+{	
+	return mDirection;	
+}
+
+/**
+ * Setters for the basic properties
+ */
+void RoadNeighbor::SetSide(string side)
+{	
+	mSide=side;	
+}
+void RoadNeighbor::SetElementId(string elementId)
+{	
+	mElementId=elementId;	
+}
+void RoadNeighbor::SetDirection(string direction)
+{	
+	mDirection=direction;	
+}
 
-	// DELETING ROAD TYPES
-	mRoadTypeVector.clear();
 
-	// DELETING GEOMETRY BLOKS
-	mGeometryBlockVector.clear();
+//***********************************************************************************
+//Road Type
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+RoadType::RoadType (double s, string type,string country)
+{	
+    mS=s; mType=type;mCountry = country;
+}
 
-	// DELETING ELEVATIONS
-	mElevationVector.clear();
+/**
+ * Setters for the basic properties
+ */
+void RoadType::SetS(double value)
+{
+	mS=value;
+}
+void RoadType::SetType(string type)
+{
+	mType=type;
+}
+void RoadType::SetCountry(std::string country)
+{
+    mCountry = country;
+}
 
-	// DELETING SUPERELEVATION
-	mSuperElevationVector.clear();
+/**
+ * Getters for the basic properties
+ */
+double RoadType::GetS()
+{
+	return mS;
+}
+string RoadType::GetType()
+{
+	return mType;
+}
+string RoadType::GetCountry()
+{
+    return mCountry;
+}
 
-	// DELETING CROSSFALL
-	mCrossfallVector.clear();
+vector<RoadTypeSpeed> * RoadType::GetRoadTypeSpeedVector()
+{
+    return &mRoadTypeSpeedVector;
+}
 
-	// DELETING LANE SECTIONS
-	mLaneSectionsVector.clear();
+unsigned int RoadType::GetRoadTypeSpeedCount()
+{
+    return mRoadTypeSpeedVector.size();
+}
 
-	// DELETING OBJECTS
-	mObjectsVector.clear();
+RoadTypeSpeed * RoadType::GetRoadTypeSpeed(unsigned int i)
+{
+    if ((mRoadTypeSpeedVector.size()>0)&&(i<mRoadTypeSpeedVector.size()))
+        return &mRoadTypeSpeedVector.at(i);
+    else
+        return NULL;
+}
 
-	// DELETING SIGNALS
-	mSignalsVector.clear();
+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 Link Record
+//Road Type Speed
 //***********************************************************************************
 /**
  * Constructor which intializes the basic properties
  */
-RoadLink::RoadLink(string elementType, string elementId, string contactPoint)
+
+RoadTypeSpeed::RoadTypeSpeed(double maxSpeed, std::string unit)
 {
-	mElementType=elementType;
-	mElementId=elementId;
-	mContactPoint=contactPoint;
+    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
  */
-string RoadLink::GetElementType()
+double RoadTypeSpeed::GetmaxSpeed()
+{
+    return mmaxSpeed;
+}
+string RoadTypeSpeed::Getunit()
+{
+    return munit;
+}
+
+
+
+//***********************************************************************************
+//Elevation record
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+Elevation::Elevation(double s, double a, double b, double c, double d): ThirdOrderPolynom(s,a,b,c,d)
+{}
+
+
+
+
+
+//***********************************************************************************
+//Elevation record
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+SuperElevation::SuperElevation(double s, double a, double b, double c, double d): ThirdOrderPolynom(s,a,b,c,d)
+{}
+
+
+//***********************************************************************************
+//Shape record
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+Shape::Shape(double s, double t, double a, double b, double c, double d): Third2DOrderPolynom(s,t,a,b,c,d)
+{}
+
+
+
+
+
+//***********************************************************************************
+//Crossfall Record
+//***********************************************************************************
+/**
+ * Constructor which intializes the basic properties
+ */
+Crossfall::Crossfall (string side, double s, double a, double b, double c, double d):ThirdOrderPolynom(s,a,b,c,d)
 {	
-	return mElementType;	
+	mSide=side;	
+}
+
+/**
+ * Getters for the crossfall side
+ */
+string Crossfall::GetSide()
+{
+	return mSide;
+}
+
+/**
+ * Setters for the crossfall side
+ */
+void Crossfall::SetSide(string side)
+{
+	mSide=side;
+}
+
+
+//***********************************************************************************
+//Surface Record
+//***********************************************************************************
+
+surface_CRG::surface_CRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode)
+{
+    mfile = file;
+    msStart = sStart;
+    msEnd = sEnd;
+    morientation = orientation;
+    mmode = mode;
+}
+
+std::string surface_CRG::Getfile()
+{
+    return mfile;
+}
+
+double surface_CRG::GetsStart()
+{
+    return msStart;
+}
+
+double surface_CRG::GetsEnd()
+{
+    return msEnd;
+}
+
+std::string surface_CRG::Getorientation()
+{
+    return morientation;
+}
+
+std::string surface_CRG::Getmode()
+{
+    return mmode;
+}
+
+int surface_CRG::Getpurpose(std::string & purpose)
+{
+    if(mpurpose.size() == 0)return  0;
+    purpose = mpurpose[0];
+    return 1;
+}
+
+int surface_CRG::GetsOffset(double & sOffset)
+{
+    if(msOffset.size() == 0)return  0;
+    sOffset = msOffset[0];
+    return 1;
+}
+
+int surface_CRG::GettOffset(double & tOffset)
+{
+    if(mtOffset.size() == 0)return 0;
+    tOffset = mtOffset[0];
+    return  1;
+}
+
+int surface_CRG::GetzOffset(double & zOffset)
+{
+    if(mzOffset.size() == 0)return  0;
+    zOffset = mzOffset[0];
+    return 1;
+}
+
+int surface_CRG::GetzScale(double & zScale)
+{
+    if(mzScale.size() == 0)return  0;
+    zScale = mzScale[0];
+    return 1;
+}
+
+int surface_CRG::GethOffset(double & hOffset)
+{
+    if(mhOffset.size() == 0)return 0;
+    hOffset = mhOffset[0];
+    return  1;
+}
+
+void surface_CRG::Setfile(std::string file)
+{
+    mfile = file;
+}
+
+void surface_CRG::SetsStart(double sStart)
+{
+    msStart = sStart;
+}
+
+void surface_CRG::SetsEnd(double sEnd)
+{
+    msEnd = sEnd;
+}
+
+void surface_CRG::Setorientation(std::string orientation)
+{
+    morientation = orientation;
+}
+
+void surface_CRG::Setmode(std::string mode)
+{
+    mmode = mode;
+}
+
+void surface_CRG::Setpurpose(std::string purpose)
+{
+    if(mpurpose.size() > 0)mpurpose.clear();
+    mpurpose.push_back(purpose);
+}
+
+void surface_CRG::SetsOffset(double sOffset)
+{
+    if(msOffset.size()>0)msOffset.clear();
+    msOffset.push_back(sOffset);
+}
+
+void surface_CRG::SettOffset(double tOffset)
+{
+    if(mtOffset.size()>0)mtOffset.clear();
+    mtOffset.push_back(tOffset);
+}
+
+void surface_CRG::SetzOffset(double zOffset)
+{
+    if(mzOffset.size()>0)mzOffset.clear();
+    mzOffset.push_back(zOffset);
+}
+
+void surface_CRG::SetzScale(double zScale)
+{
+    if(mzScale.size()>0)mzScale.clear();
+    mzScale.push_back(zScale);
+}
+
+void surface_CRG::SethOffset(double hOffset)
+{
+    if(mhOffset.size()>0)mhOffset.clear();
+    mhOffset.push_back(hOffset);
+}
+
+void surface_CRG::Resetpurpose()
+{
+    if(mpurpose.size()>0)mpurpose.clear();
 }
-string RoadLink::GetElementId()
-{	
-	return mElementId;	
+
+void surface_CRG::ResetsOffset()
+{
+    if(msOffset.size()>0)msOffset.clear();
 }
-string RoadLink::GetContactPoint()
-{	
-	return mContactPoint;	
+
+void surface_CRG::ResettOffset()
+{
+    if(mtOffset.size()>0)mtOffset.clear();
 }
-double RoadLink::GetElementS()
+
+void surface_CRG::ResetzOffset()
 {
-    return mElementS;
+    if(mzOffset.size()>0)mzOffset.clear();
 }
-string RoadLink::GetElementDir()
+
+void surface_CRG::ResetzScale()
 {
-    return mElementDir;
+    if(mzScale.size()>0)mzScale.clear();
 }
 
-/**
- * Setters for the basic properties
- */
-void RoadLink::SetElementType(string elementType)
-{	
-	mElementType=elementType;	
+void surface_CRG::ResethOffset()
+{
+    if(mhOffset.size()>0)mhOffset.clear();
 }
-void RoadLink::SetElementId(string elementId)
-{	
-	mElementId=elementId;
+
+bool surface_CRG::CheckInterval(double s_check)
+{
+    if (s_check>=msStart)
+        return true;
+    else
+        return false;
 }
-void RoadLink::SetContactPoint(string contactPoint)
-{	
-	mContactPoint=contactPoint;	
+
+
+
+surface::surface()
+{
+
 }
-void RoadLink::SetElementS(double value)
+
+vector<surface_CRG> * surface::GetCRGVector()
 {
-    mElementS = value;
+    return &mCRG;
 }
-void RoadLink::SetELementDir(std::string elementdir)
+
+surface_CRG* surface::GetCRG(unsigned int i)
 {
-    mElementDir = elementdir;
+    if ((mCRG.size()>0)&&(i<(mCRG.size())))
+        return &(mCRG.at(i));
+    else
+        return NULL;
 }
 
+unsigned int surface::GetCRGCount()
+{
+    return static_cast<unsigned int >(mCRG.size()) ;
+}
 
-//***********************************************************************************
-//Road Neighbor Record
-//***********************************************************************************
-/**
- * Constructor which intializes the basic properties
- */
-RoadNeighbor::RoadNeighbor(string side, string elementId, string direction)
+surface_CRG*			surface::GetLastCRG()
 {
-	mSide=side;
-	mElementId=elementId;
-	mDirection=direction;
+    if (mCRG.size()>0)
+        return &mCRG.at(mCRG.size()-1);
+    else
+        return NULL;
 }
-/**
- * Getters for the basic properties
- */
-string RoadNeighbor::GetSide()
-{	
-	return mSide;	
+
+surface_CRG*			surface::GetLastAddedCRG()
+{
+    if(mLastAddedCRG<mCRG.size())
+        return &mCRG.at(mLastAddedCRG);
+    else
+        return NULL;
 }
-string RoadNeighbor::GetElementId()
-{	
-	return mElementId;	
+
+unsigned int surface::AddCRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode)
+{
+
+    // Check the first method in the group for details
+
+    unsigned int index = static_cast<unsigned int>(CheckCRGInterval(sStart)+1) ;
+    if(index>=GetCRGCount()) mCRG.push_back(surface_CRG(file,sStart,sEnd,orientation,mode));
+    else mCRG.insert(mCRG.begin()+index, surface_CRG(file,sStart,sEnd,orientation,mode));
+    mLastAddedCRG=index;
+    return index;
 }
-string RoadNeighbor::GetDirection()
-{	
-	return mDirection;	
+
+unsigned int surface::CloneCRG(unsigned int index)
+{
+    if(index<(mCRG.size()-1))
+        mCRG.insert(mCRG.begin()+index+1, mCRG[index]);
+    else if(index==mCRG.size()-1)
+        mCRG.push_back(mCRG[index]);
+    mLastAddedCRG=index+1;
+    return mLastAddedCRG;
 }
 
-/**
- * Setters for the basic properties
- */
-void RoadNeighbor::SetSide(string side)
-{	
-	mSide=side;	
+void surface::DeleteCRG(unsigned int index)
+{
+    mCRG.erase(mCRG.begin()+index);
 }
-void RoadNeighbor::SetElementId(string elementId)
-{	
-	mElementId=elementId;	
+
+int surface::CheckCRGInterval(double s_check)
+{
+    int res=-1;
+    //Go through all the tunnel records
+    for (unsigned int i=0;i<mCRG.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mCRG.at(i).CheckInterval(s_check))
+            res=static_cast<int>(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 RoadNeighbor::SetDirection(string direction)
-{	
-	mDirection=direction;	
+
+
+Railroad_Switch_MainTrack::Railroad_Switch_MainTrack(std::string id, double s, std::string dir)
+{
+    mid = id;
+    ms = s;
+    mdir = dir;
 }
 
+Railroad_Switch_MainTrack::Railroad_Switch_MainTrack()
+{
 
-//***********************************************************************************
-//Road Type
-//***********************************************************************************
-/**
- * Constructor which intializes the basic properties
- */
-RoadType::RoadType (double s, string type,string country)
-{	
-    mS=s; mType=type;mCountry = country;
 }
 
-/**
- * Setters for the basic properties
- */
-void RoadType::SetS(double value)
+std::string  Railroad_Switch_MainTrack::Getid()
 {
-	mS=value;
+    return mid;
 }
-void RoadType::SetType(string type)
+
+double Railroad_Switch_MainTrack::Gets()
 {
-	mType=type;
+    return ms;
 }
-void RoadType::SetCountry(std::string country)
+
+std::string Railroad_Switch_MainTrack::Getdir()
 {
-    mCountry = country;
+    return  mdir;
 }
 
-/**
- * Getters for the basic properties
- */
-double RoadType::GetS()
+void Railroad_Switch_MainTrack::Setid(std::string id)
 {
-	return mS;
+    mid = id;
 }
-string RoadType::GetType()
+
+void Railroad_Switch_MainTrack::Sets(double s)
 {
-	return mType;
+    ms = s;
 }
-string RoadType::GetCountry()
+
+void Railroad_Switch_MainTrack::Setdir(std::string dir)
 {
-    return mCountry;
+    mdir = dir;
 }
 
-vector<RoadTypeSpeed> * RoadType::GetRoadTypeSpeedVector()
+
+
+Railroad_Switch_SideTrack::Railroad_Switch_SideTrack(std::string id, double s, std::string dir)
 {
-    return &mRoadTypeSpeedVector;
+    mid = id;
+    ms = s;
+    mdir = dir;
 }
 
-unsigned int RoadType::GetRoadTypeSpeedCount()
+Railroad_Switch_SideTrack::Railroad_Switch_SideTrack()
 {
-    return mRoadTypeSpeedVector.size();
+
 }
 
-RoadTypeSpeed * RoadType::GetRoadTypeSpeed(unsigned int i)
+std::string  Railroad_Switch_SideTrack::Getid()
 {
-    if ((mRoadTypeSpeedVector.size()>0)&&(i<mRoadTypeSpeedVector.size()))
-        return &mRoadTypeSpeedVector.at(i);
-    else
-        return NULL;
+    return mid;
 }
 
-unsigned int RoadType::AddRoadTypeSpeed(double maxSpeed, std::string unit)
+double Railroad_Switch_SideTrack::Gets()
 {
-    RoadTypeSpeed rts(maxSpeed,unit);
-    if(mRoadTypeSpeedVector.size()>0)mRoadTypeSpeedVector.clear();
-    mRoadTypeSpeedVector.push_back(rts);
-    return mRoadTypeSpeedVector.size()-1;
+    return ms;
 }
 
-void RoadType::DeleteRoadTypeSpeed(unsigned int index)
+std::string Railroad_Switch_SideTrack::Getdir()
 {
-    if(index >= 1)
-    {
-        return;
-    }
-    mRoadTypeSpeedVector.clear();
+    return  mdir;
 }
 
+void Railroad_Switch_SideTrack::Setid(std::string id)
+{
+    mid = id;
+}
 
+void Railroad_Switch_SideTrack::Sets(double s)
+{
+    ms = s;
+}
 
+void Railroad_Switch_SideTrack::Setdir(std::string dir)
+{
+    mdir = dir;
+}
 
-//***********************************************************************************
-//Road Type Speed
-//***********************************************************************************
-/**
- * Constructor which intializes the basic properties
- */
 
-RoadTypeSpeed::RoadTypeSpeed(double maxSpeed, std::string unit)
+Railroad_Switch_Partner::Railroad_Switch_Partner(std::string id)
 {
-    mmaxSpeed = maxSpeed;
-    munit = unit;
+    mid = id;
 }
 
-/**
- * Setters for the basic properties
- */
-void RoadTypeSpeed::SetmaxSpeed(double value)
+Railroad_Switch_Partner::Railroad_Switch_Partner()
 {
-    mmaxSpeed = value;
+
 }
-void RoadTypeSpeed::Setunit(std::string unit)
+
+int Railroad_Switch_Partner::Getname(std::string & name)
 {
-    munit = unit;
+    if(mname.size() == 0)return  0;
+    name = mname[0];
+    return 1;
 }
 
-/**
- * Getters for the basic properties
- */
-double RoadTypeSpeed::GetmaxSpeed()
+std::string Railroad_Switch_Partner::Getid()
 {
-    return mmaxSpeed;
+    return  mid;
 }
-string RoadTypeSpeed::Getunit()
+
+void Railroad_Switch_Partner::Setname(std::string name)
 {
-    return munit;
+    if(mname.size()>0)mname.clear();
+    mname.push_back(name);
 }
 
+void Railroad_Switch_Partner::Setid(std::string id)
+{
+    mid = id;
+}
 
+void Railroad_Switch_Partner::Resetname()
+{
+    if(mname.size()>0)mname.clear();
+}
 
-//***********************************************************************************
-//Elevation record
-//***********************************************************************************
-/**
- * Constructor which intializes the basic properties
- */
-Elevation::Elevation(double s, double a, double b, double c, double d): ThirdOrderPolynom(s,a,b,c,d)
-{}
 
 
+Railroad_Switch::Railroad_Switch(std::string name, std::string id, std::string position,Railroad_Switch_MainTrack  mainTrack,Railroad_Switch_SideTrack  sideTrack)
+{
+    mname = name;
+    mid = id;
+    mposition = position;
+    mMainTrack = mainTrack;
+    mSideTrack = sideTrack;
+}
 
+std::string Railroad_Switch::Getname()
+{
+    return mname;
+}
 
+std::string Railroad_Switch::Getid()
+{
+    return mid;
+}
 
-//***********************************************************************************
-//Elevation record
-//***********************************************************************************
-/**
- * Constructor which intializes the basic properties
- */
-SuperElevation::SuperElevation(double s, double a, double b, double c, double d): ThirdOrderPolynom(s,a,b,c,d)
-{}
+std::string Railroad_Switch::Getposition()
+{
+    return mposition;
+}
 
+void Railroad_Switch::Setname(std::string name)
+{
+    mname = name;
+}
 
+void Railroad_Switch::Setid(std::string id)
+{
+    mid = id;
+}
 
+void Railroad_Switch::Setposition(std::string position)
+{
+    mposition = position;
+}
 
+void Railroad_Switch::SetPartner(Railroad_Switch_Partner & partner)
+{
+    if(mPartner.size() > 0)mPartner.clear();
+    mPartner.push_back(partner);
+}
 
-//***********************************************************************************
-//Crossfall Record
-//***********************************************************************************
-/**
- * Constructor which intializes the basic properties
- */
-Crossfall::Crossfall (string side, double s, double a, double b, double c, double d):ThirdOrderPolynom(s,a,b,c,d)
-{	
-	mSide=side;	
+int Railroad_Switch::GetPartner(Railroad_Switch_Partner & partner)
+{
+    if(mPartner.size() == 0)return  0;
+    partner = mPartner[0];
+    return 1;
 }
 
-/**
- * Getters for the crossfall side
- */
-string Crossfall::GetSide()
+void Railroad_Switch::ResetPartner()
 {
-	return mSide;
+    if(mPartner.size()>0)mPartner.clear();
 }
 
-/**
- * Setters for the crossfall side
- */
-void Crossfall::SetSide(string side)
+Railroad_Switch_MainTrack * Railroad_Switch::GetMainTrack()
 {
-	mSide=side;
+    return &mMainTrack;
 }
 
+Railroad_Switch_SideTrack * Railroad_Switch::GetSideTrack()
+{
+    return &mSideTrack;
+}
+
+

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

@@ -22,6 +22,11 @@ class GeometryBlock;
 class Elevation;
 class SuperElevation;
 class Crossfall;
+class surface;
+class surface_CRG;
+class Railroad_Switch;
+class Railroad_Switch_MainTrack;
+class Railroad_Switch_SideTrack;
 //lanes
 class LaneSection;
 class LaneSectionSample;
@@ -29,6 +34,8 @@ class LaneOffset;
 //objects, signals
 class Object;
 class Signal;
+class Shape;
+
 //--------------
 
 using std::vector;
@@ -69,23 +76,40 @@ private:
 	vector<Elevation> mElevationVector;
 	// Superelevation vector
 	vector<SuperElevation> mSuperElevationVector;
+    // Shape vector
+    vector<Shape> mShapeVector;
 	// Crossfall vector
 	vector<Crossfall> mCrossfallVector;
 	// Lane Section vector
 	vector<LaneSection> mLaneSectionsVector;
     // Lane offset vector
     vector<LaneOffset> mLaneOffsetVector;
+    //bridge vector
+    vector<Objects_bridge> mObjectsBridgeVector;
+    //tunnel vector
+    vector<Objects_tunnel> mObjectsTunnelVector;
+    //objectReference vector
+    vector<Objects_objectReference> mObjectsObjectReferenceVector;
 	// Objects vectors
 	vector<Object> mObjectsVector;
 	// Signal vector
 	vector<Signal> mSignalsVector;
 
+    //Signal Reference
+    vector<signals_signalReference> mSignalReferenceVector;
+
     vector<string> muserDataVector;
 
     vector<RoadBorrow> mRoadBorrowVector;
 
     vector<RoadNoavoid> mRoadNoavoidVector;
 
+    vector<RoadPriority> mRoadPriorityVector;
+
+    vector<surface_CRG> mSurfaceCRGVector;
+
+    vector<Railroad_Switch> mRailroadSwitchVector;
+
 	/**
 	 * Indices of the last added child records
 	 */
@@ -96,10 +120,17 @@ private:
 	unsigned int mLastAddedCrossfall;
 	unsigned int mLastAddedLaneSection;
     unsigned int mLastAddedLaneOffset;
+    unsigned int mLastAddedObjectsBridge;
+    unsigned int mLastAddedObjectsTunnel;
+    unsigned int mLastAddedObjectsObjectReference;
 	unsigned int mLastAddedObject;
 	unsigned int mLastAddedSignal;
+    unsigned int mLastAddedSignalReference;
     unsigned int mLastAddedRoadBorrow;
     unsigned int mLastAddedRoadNoavoid;
+    unsigned int mLastAddedSurfaceCRG;
+    unsigned int mLastAddedRailroadSwitch;
+    unsigned int mLastAddedShape;
 
 public:
 	/**
@@ -160,6 +191,10 @@ public:
 	vector<Elevation> *GetElevationVector();
 	Elevation*	GetElevation(unsigned int i);
 	unsigned int GetElevationCount();
+    // Road Shape records
+    vector<Shape> *GetShapeVector();
+    Shape*	GetShape(unsigned int i);
+    unsigned int GetShapeCount();
 	// Road superelevation records
 	vector<SuperElevation> *GetSuperElevationVector();
 	SuperElevation*	GetSuperElevation(unsigned int i);
@@ -180,11 +215,28 @@ public:
 	vector<Object> *GetObjectVector();
 	Object*	GetObject(unsigned int i);
 	unsigned int GetObjectCount();
+    // Road bridge records
+    vector<Objects_bridge> *GetObjectsBridgeVector();
+    Objects_bridge*	GetObjectsBridge(unsigned int i);
+    unsigned int GetObjectsBridgeCount();
+    // Road tunnel records
+    vector<Objects_tunnel> *GetObjectsTunnelVector();
+    Objects_tunnel*	GetObjectsTunnel(unsigned int i);
+    unsigned int GetObjectsTunnelCount();
+    // Road objectReference records
+    vector<Objects_objectReference> *GetObjectsObjectReferenceVector();
+    Objects_objectReference*	GetObjectsObjectReference(unsigned int i);
+    unsigned int GetObjectsObjectReferenceCount();
 	// Road signal records
 	vector<Signal> *GetSignalVector();
 	Signal*	GetSignal(unsigned int i);
 	unsigned int GetSignalCount();
 
+    // Road signal Reference records
+    vector<signals_signalReference> *GetSignalReferenceVector();
+    signals_signalReference*	GetSignalReference(unsigned int i);
+    unsigned int GetSignalReferencCount();
+
     vector<RoadBorrow> *GetRoadBorrowVector();
     RoadBorrow*	GetRoadBorrow(unsigned int i);
     unsigned int GetRoadBorrowCount();
@@ -193,6 +245,14 @@ public:
     RoadNoavoid*	GetRoadNoavoid(unsigned int i);
     unsigned int GetRoadNoavoidCount();
 
+    vector<surface_CRG> *GetSurfaceCRGVector();
+    surface_CRG*	GetSurfaceCRG(unsigned int i);
+    unsigned int GetSurfaceCRGCount();
+
+    vector<Railroad_Switch> *GetRailroadSwitchVector();
+    Railroad_Switch*	GetRailroadSwitch(unsigned int i);
+    unsigned int GetRailroadSwitchCount();
+
 
     vector<string> * GetUserData();
 	//-------------------------------------------------
@@ -203,13 +263,20 @@ public:
 	RoadType*		GetLastRoadType();
 	GeometryBlock*	GetLastGeometryBlock();
 	Elevation*		GetLastElevation();
-	SuperElevation*	GetLastSuperElevation();
+    Shape*          GetLastShape();
+    SuperElevation*	GetLastSuperElevation();
 	Crossfall*		GetLastCrossfall();
 	LaneSection*	GetLastLaneSection();
+    Objects_bridge* GetLastObjectsBridge();
+    Objects_tunnel* GetLastObjectsTunnel();
+    Objects_objectReference* GetLastObjectsObjectReference();
 	Object*			GetLastObject();
 	Signal*			GetLastSignal();
+    signals_signalReference* GetLastSignalReference();
     RoadBorrow *    GetLastRoadBorrow();
     RoadNoavoid *   GetLastRoadNoavoid();
+    surface_CRG *       GetLastSurfaceCRG();
+    Railroad_Switch * GetLastRailroadSwitch();
 
 	/**
 	 * Getters for the last added child records in their respective vectors
@@ -217,13 +284,20 @@ public:
 	RoadType*		GetLastAddedRoadType();
 	GeometryBlock*	GetLastAddedGeometryBlock();
 	Elevation*		GetLastAddedElevation();
+    Shape*          GetLastAddedShape();
 	SuperElevation*	GetLastAddedSuperElevation();
 	Crossfall*		GetLastAddedCrossfall();
 	LaneSection*	GetLastAddedLaneSection();
+    Objects_bridge* GetLastAddedObjectsBridge();
+    Objects_tunnel* GetLastAddedObjectsTunnel();
+    Objects_objectReference* GetLastAddedObjectsObjectReference();
 	Object*			GetLastAddedObject();
 	Signal*			GetLastAddedSignal();
+    signals_signalReference* GetLastAddedSignalReference();
     RoadBorrow*     GetLastAddedRoadBorrow();
     RoadNoavoid*    GetLastAddedRoadNoavoid();
+    surface_CRG*        GetLastAddedSurfaceCRG();
+    Railroad_Switch * GetLastAddedRailroadSwitch();
 
 	//-------------------------------------------------
 
@@ -261,29 +335,43 @@ public:
     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 AddShape(double s, double t, 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 AddLaneOffset(double s,double a,double b,double c,double d);
+    unsigned int AddObjectsBridge(double s,double length,string id,string type);
+    unsigned int AddObjectsTunnel(double s,double length,string id,string type);
+    unsigned int AddObjectsObjectReference(double s,double t,string id,string orientation);
     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);
+    unsigned int AddSignalReference(double s,double t,std::string id,std::string orientation);
 
     unsigned int AddRoadBorrow(double s,double length,string mode);
     unsigned int AddRoadNoavoid(double s,double length);
+    unsigned int AddSurfaceCRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode);
+    unsigned int AddRailroadSwitch(std::string name, std::string id,std::string position, Railroad_Switch_MainTrack mainTrack, Railroad_Switch_SideTrack sideTrack);
 	/**
 	 * Methods used to clone child records in the respective vectors
 	 */
 	unsigned int CloneRoadType(unsigned int index);
 	unsigned int CloneElevation(unsigned int index);
+    unsigned int CloneShape(unsigned int index);
 	unsigned int CloneSuperElevation(unsigned int index);
 	unsigned int CloneCrossfall(unsigned int index);
 	unsigned int CloneLaneSection(unsigned int index);
 	unsigned int CloneLaneSectionEnd(unsigned int index);
+    unsigned int CloneObjectsBridge(unsigned int index);
+    unsigned int CloneObjectsTunnel(unsigned int index);
+    unsigned int CloneObjectsObjectReference(unsigned int index);
 	unsigned int CloneObject(unsigned int index);
 	unsigned int CloneSignal(unsigned int index);
+    unsigned int CloneSignalReference(unsigned int index);
     unsigned int CloneRoadBorrow(unsigned int index);
     unsigned int CloneRoadNoavoid(unsigned int index);
+    unsigned int CloneSurfaceCRG(unsigned int index);
+    unsigned int CloneRailroadSwitch(unsigned int index);
 
 	/**
 	 * Methods used to delete child records from the respective vectors
@@ -291,14 +379,21 @@ public:
 	void DeleteRoadType(unsigned int index);
 	void DeleteGeometryBlock(unsigned int index);
 	void DeleteElevation(unsigned int index);
+    void DeleteShape(unsigned int index);
 	void DeleteSuperElevation(unsigned int index);
 	void DeleteCrossfall(unsigned int index);
 	void DeleteLaneSection(unsigned int index);
     void DeleteLaneOffset(unsigned int index);
+    void DeleteObjectsBridge(unsigned int index);
+    void DeleteObjectsTunnel(unsigned int index);
+    void DeleteObjectsObjectReference(unsigned int index);
 	void DeleteObject(unsigned int index);
 	void DeleteSignal(unsigned int index);
+    void DeleteSignalReference(unsigned int index);
     void DeleteRoadBorrow(unsigned int index);
     void DeleteRoadNoavoid(unsigned int index);
+    void DeleteSurfaceCRG(unsigned int index);
+    void DeleteRailroadSwitch(unsigned int index);
 	
 	//-------------------------------------------------
 
@@ -336,6 +431,9 @@ public:
 	int CheckElevationInterval(double s_check);
 	double GetElevationValue (double s_check);
 
+    int CheckShapeInterval(double s_check,double t_check);
+    double GetShapeValue(double s_check,double t_check);
+
 	int CheckSuperElevationInterval(double s_check);
 	double GetSuperElevationValue (double s_check);
 
@@ -350,6 +448,51 @@ public:
     int CheckRoadBorrowInterval(double s_check);
 
     int CheckRoadNoavoidInterval(double s_check);
+
+    int CheckRoadObjectsBridgeInterval(double s_check);
+
+    int CheckRoadObjectsTunnelInterval(double s_check);
+
+    int CheckRoadObjectsObjectReferenceInterval(double s_check);
+
+    int CheckSignalReferencInterval(double s_check);
+
+    int CheckSignalInterval(double s_check);
+
+    int CheckSurfaceCRGInterval(double s_check);
+
+
+    std::vector<double> GetDrivingLaneWidthVector(double s_check,int nlr); //Lane Width, From Refline to side. nlr 1 left 2 right
+
+
+    /**
+     * @brief GetLeftDrivingLaneCount
+     * @param s_check
+     * @return if No LaneSection return -1
+     */
+    int GetLeftDrivingLaneCount(double s_check);
+    int GetRightDrivingLaneCount(double s_check);
+
+    std::vector<double> GetLaneWidthVector(double s_check,int nlr); //Lane Width, From Refline to side. nlr 1 left 2 right
+
+    double GetLeftRoadWidth(double s_check);
+    double GetRightRoadWidth(double s_check);
+
+    int GetLeftLaneCount(double s_check);
+    int GetRightLaneCount(double s_check);
+
+    LaneRoadMark GetLaneRoadMarkValue(double s_check,int nlane);  //nlane=0 center lane    nlane<0 left lane     nlane>0 right lane
+
+    void SetRoadPriority(int nPriority);
+    int GetRoadPriority(int & nPriority);
+    void ResetPriority();
+
+    double GetDis(const double x,const double y, const double hdg, double & fRefDis, double & fHdgDiff, int & nlane,double & s,double & refx,double & refy,double & refhdg); //fRefDis distance to reference line,  nlane if dis is 0, nlane which lane is distance is 0
+
+    short int GetLaneCoords(double s_check, double t_check,double &retX, double &retY, double &retZ,double &retHDG);
+
+
+
 	
 	//-------------------------------------------------
 
@@ -571,6 +714,20 @@ public:
 };
 //----------------------------------------------------------------------------------
 
+
+/**
+ * Shape Class is used to store information about a road shape record
+ * It inherits the Polynom class and has no additional properties
+ *
+ *
+ */
+class Shape : public  Third2DOrderPolynom
+{
+public:
+    Shape(double s,double t,double a,double b,double c,double d);
+};
+
+//----------------------------------------------------------------------------------
 /**
  * Crossfall class is used to store information about a road superelevation record
  * It inherits the Polynom class and has one additional properties
@@ -606,4 +763,180 @@ public:
 //----------------------------------------------------------------------------------
 
 
+class surface_CRG
+{
+private:
+    std::string mfile;
+    double msStart;
+    double msEnd;
+    std::string morientation;
+    std::string mmode;
+    std::vector<std::string> mpurpose;
+    std::vector<double> msOffset;
+    std::vector<double> mtOffset;
+    std::vector<double> mzOffset;
+    std::vector<double> mzScale;
+    std::vector<double> mhOffset;
+
+public:
+    surface_CRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode);
+
+    std::string Getfile();
+    double GetsStart();
+    double GetsEnd();
+    std::string Getorientation();
+    std::string Getmode();
+    int Getpurpose(std::string & purpose);
+    int GetsOffset(double & sOffset);
+    int GettOffset(double & tOffset);
+    int GetzOffset(double & zOffset);
+    int GetzScale(double & zScale);
+    int GethOffset(double & hOffset);
+
+    void Setfile(std::string file);
+    void SetsStart(double sStart);
+    void SetsEnd(double sEnd);
+    void Setorientation(std::string orientation);
+    void Setmode(std::string mode);
+    void Setpurpose(std::string purpose);
+    void SetsOffset(double sOffset);
+    void SettOffset(double tOffset);
+    void SetzOffset(double zOffset);
+    void SetzScale(double zScale);
+    void SethOffset(double hOffset);
+
+    void Resetpurpose();
+    void ResetsOffset();
+    void ResettOffset();
+    void ResetzOffset();
+    void ResetzScale();
+    void ResethOffset();
+
+    bool CheckInterval(double s_check);
+
+};
+
+//----------------------------------------------------------------------------------
+
+class surface
+{
+private:
+    vector<surface_CRG> mCRG;
+    unsigned int mLastAddedCRG;
+
+public:
+    surface();
+
+    vector<surface_CRG> * GetCRGVector();
+    surface_CRG* GetCRG(unsigned int i);
+    unsigned int GetCRGCount();
+    surface_CRG*			GetLastCRG();
+    surface_CRG*			GetLastAddedCRG();
+    unsigned int AddCRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode);
+    unsigned int CloneCRG(unsigned int index);
+    void DeleteCRG(unsigned int index);
+
+    int CheckCRGInterval(double s_check);
+
+
+
+
+};
+
+//----------------------------------------------------------------------------------
+
+class Railroad_Switch_MainTrack
+{
+private:
+    std::string mid;
+    double ms;
+    std::string mdir;
+
+public:
+    Railroad_Switch_MainTrack(std::string id, double s, std::string dir);
+    Railroad_Switch_MainTrack();
+    std::string  Getid();
+    double Gets();
+    std::string Getdir();
+    void Setid(std::string id);
+    void Sets(double s);
+    void Setdir(std::string dir);
+};
+
+//----------------------------------------------------------------------------------
+
+class Railroad_Switch_SideTrack
+{
+private:
+    std::string mid;
+    double ms;
+    std::string mdir;
+
+public:
+    Railroad_Switch_SideTrack(std::string id, double s, std::string dir);
+    Railroad_Switch_SideTrack();
+    std::string  Getid();
+    double Gets();
+    std::string Getdir();
+    void Setid(std::string id);
+    void Sets(double s);
+    void Setdir(std::string dir);
+};
+
+//----------------------------------------------------------------------------------
+
+class Railroad_Switch_Partner
+{
+private:
+    std::vector<std::string> mname;
+    std::string mid;
+
+public:
+    Railroad_Switch_Partner(std::string id);
+    Railroad_Switch_Partner();
+
+    int Getname(std::string & name);
+    std::string Getid();
+
+    void Setname(std::string name);
+    void Setid(std::string id);
+
+    void Resetname();
+
+};
+
+//----------------------------------------------------------------------------------
+
+class Railroad_Switch
+{
+private:
+    std::string mname;
+    std::string mid;
+    std::string mposition;
+
+    Railroad_Switch_MainTrack mMainTrack;
+    Railroad_Switch_SideTrack mSideTrack;
+    std::vector<Railroad_Switch_Partner> mPartner;
+
+public:
+    Railroad_Switch(std::string name, std::string id, std::string position,Railroad_Switch_MainTrack  mainTrack,Railroad_Switch_SideTrack sideTrack);
+    std::string Getname();
+    std::string Getid();
+    std::string Getposition();
+
+    void Setname(std::string name);
+    void Setid(std::string id);
+    void Setposition(std::string position);
+
+    void SetPartner(Railroad_Switch_Partner & partner);
+    int GetPartner(Railroad_Switch_Partner & partner);
+    void ResetPartner();
+
+    Railroad_Switch_MainTrack * GetMainTrack();
+    Railroad_Switch_SideTrack * GetSideTrack();
+
+};
+
+
+//----------------------------------------------------------------------------------
 #endif

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

@@ -380,13 +380,51 @@ void GeometrySpiral::ComputeVars()
 {
 	mA=0;
 
+    mLengthBase = 0;
+    mX0 = 0;
+    mY0 = 0;
+    mHDG0 = 0;
+
+    mHDG0Cos = 1.0;
+    mHDG0Sin = 0.0;
+
 	//if the curvatureEnd is the non-zero curvature, then the motion is in normal direction along the spiral
-	if ((fabs(mCurvatureEnd)>1.00e-15)&&(fabs(mCurvatureStart)<=1.00e-15))
+ //   if ((fabs(mCurvatureEnd)>1.00e-15)&&(fabs(mCurvatureStart)<=1.00e-15))
+    if ((fabs(mCurvatureEnd))>(fabs(mCurvatureStart)))
 	{
 		mNormalDir=true;
-		mCurvature=mCurvatureEnd;
+        mCurvature=mCurvatureEnd;
 		//Calculate the normalization term : a = 1.0/sqrt(2*End_Radius*Total_Curve_Length) 
-		mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*mLength);			
+        mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*mLength);
+
+        double xLength = mLength;
+        if(fabs(mCurvatureStart)>0.00001)
+        {
+            mLengthBase = mLength * fabs(mCurvatureStart)/(fabs(mCurvatureEnd) - fabs(mCurvatureStart));
+            xLength = mLength  + mLengthBase;
+            mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*xLength);
+
+
+            double l_start =mLengthBase *mA/sqrtPiO2;
+
+            //Solve the Fresnel Integrals
+            fresnl(l_start,&mY0,&mX0);
+
+            if (mCurvature<0)
+                mY0=-mY0;
+
+           l_start =mLengthBase*mA;
+            double tangentAngle = l_start*l_start;
+            if (mCurvature<0)
+                tangentAngle=-tangentAngle;
+
+            mHDG0 = tangentAngle;
+
+            mHDG0Cos = cos(-mHDG0);
+            mHDG0Sin = sin(-mHDG0);
+
+        }
+
 		//Denormalization Factor
 		mDenormalizeFactor=1.0/mA;	
 
@@ -395,30 +433,74 @@ void GeometrySpiral::ComputeVars()
 		mRotSin=sin(mHdg);
 	}
 	//else the motion is in the inverse direction along the spiral
-	else
+    else
 	{
+
 		mNormalDir=false;
-		mCurvature=mCurvatureStart;
+        mCurvature=mCurvatureStart;
+
 		//Calculate the normalization term : a = 1.0/sqrt(2*End_Radius*Total_Curve_Length) 
-		mA=1.0/sqrt(2*1.0/fabs(mCurvature)*mLength);			
+        mA=1.0/sqrt(2*1.0/fabs(mCurvature)*mLength);
+
+
+        if(fabs(mCurvatureEnd)>0.00001)
+        {
+            mLengthBase = mLength * fabs(mCurvatureEnd)/(fabs(mCurvatureStart) - fabs(mCurvatureEnd));
+            double xLength = mLength  + mLengthBase;
+            mA=1.0/sqrt(2*1.0/fabs(double(mCurvature))*xLength);
+            //not complete, when have this type spiral, change this code.
+
+            double l_start =mLengthBase *mA/sqrtPiO2;
+
+            //Solve the Fresnel Integrals
+            fresnl(l_start,&mY0,&mX0);
+
+            if (mCurvature<0)
+                mY0=-mY0;
+
+           l_start =mLengthBase*mA;
+            double tangentAngle = l_start*l_start;
+            if (mCurvature<0)
+                tangentAngle=-tangentAngle;
+
+            mHDG0 = tangentAngle;
+
+            mHDG0Cos = cos(-mHDG0);
+            mHDG0Sin = sin(-mHDG0);
+
+        }
+
 
 		//Because we move in the inverse direction, we need to rotate the curve according to the heading
 		//around the last point of the normalized spiral
 		//Calculate the total length, normalize it and divide by sqrtPiO2, then, calculate the position of the final point.
-		double L=(mS2-mS)*mA/sqrtPiO2;							
+        double L=(mS2-mS + mLengthBase)*mA/sqrtPiO2;
 		fresnl(L,&mEndY,&mEndX);
 		//Invert the curve if the curvature is negative
 		if (mCurvature<0)
 			mEndY=-mEndY;
 
+        mEndX = mEndX - mX0;
+        mEndY = mEndY - mY0;
+
+        double tx,ty;
+        tx = mEndX;
+        ty = mEndY;
+
+        mEndX=tx*mHDG0Cos -ty*mHDG0Sin;
+        mEndY=ty*mHDG0Cos+tx*mHDG0Sin;
+
 		//Denormalization factor
-		mDenormalizeFactor=1.0/mA;									
+
+        mDenormalizeFactor=1.0/mA;
+
 		//Find the x,y coords of the final point of the curve in local curve coordinates
 		mEndX*=mDenormalizeFactor*sqrtPiO2;						
 		mEndY*=mDenormalizeFactor*sqrtPiO2;
 
+        double L2=(mLengthBase)*mA/sqrtPiO2;
 		//Calculate the tangent angle
-		differenceAngle=L*L*(sqrtPiO2*sqrtPiO2);
+        differenceAngle=L*L*(sqrtPiO2*sqrtPiO2) - L2*L2*(sqrtPiO2*sqrtPiO2);
 		double diffAngle;
 		//Calculate the tangent and heading angle difference that will be used to rotate the spiral
 		if (mCurvature<0)
@@ -517,35 +599,50 @@ void GeometrySpiral::GetCoords(double s_check, double &retX, double &retY, doubl
 	double l=0.0;
 	double tmpX=0.0, tmpY=0.0;
 
+
+
 	//Depending on the moving direction, calculate the length of the curve from its beginning to the current point and normalize
 	//it by multiplying with the "a" normalization term
 	//Cephes lib for solving Fresnel Integrals, uses cos/sin (PI/2 * X^2) format in its function.
 	//So, in order to use the function, transform the argument (which is just L) by dividing it by the sqrt(PI/2) factor and multiply the results by it.
 	if (mNormalDir)
 	{ 
-		l=(s_check-mS)*mA/sqrtPiO2;
+        l=(s_check-mS + mLengthBase)*mA/sqrtPiO2;
 	}
 	else
 	{
-		l=(mS2-s_check)*mA/sqrtPiO2;		
+        l=(mS2-s_check + mLengthBase)*mA/sqrtPiO2;
 	}
 
 	//Solve the Fresnel Integrals
 	fresnl(l,&tmpY,&tmpX);
+
+
 	//If the curvature is negative, invert the curve on the Y axis
 	if (mCurvature<0)
 		tmpY=-tmpY;
 
+
+    tmpX = tmpX - mX0;
+    tmpY = tmpY - mY0;
+
+    double tx,ty;
+    tx = tmpX;
+    ty = tmpY;
+
+    tmpX=tx*mHDG0Cos -ty*mHDG0Sin;
+    tmpY=ty*mHDG0Cos+tx*mHDG0Sin;
+
 	//Denormalize the results and multiply by the sqrt(PI/2) term
 	tmpX*=mDenormalizeFactor*sqrtPiO2;	
 	tmpY*=mDenormalizeFactor*sqrtPiO2;
 
 	//Calculate the heading at the found position. Kill the sqrt(PI/2) term that was added to the L
-	l=(s_check-mS)*mA;
+    l=(s_check-mS+mLengthBase)*mA;
 	double tangentAngle = l*l;
 	if (mCurvature<0)
 		tangentAngle=-tangentAngle;
-	retHDG=mHdg+tangentAngle;
+    retHDG=mHdg+tangentAngle -mHDG0;
 
 
 	if (!mNormalDir)
@@ -664,7 +761,7 @@ void GeometryPoly3::UpdateSamplePoint()
             gsp.y = xvectorgeosample[ipos1].y;
             if(ipos1 == 0)
             {
-                gsp.fHdg = mHdg;
+                gsp.fHdg = 0;//mHdg;
             }
             else
             {
@@ -689,11 +786,11 @@ void GeometryPoly3::UpdateSamplePoint()
             gsp.y = y1 + fratio * (y2 - y1);
             if(mvectorgeosample.size() == 0)
             {
-                gsp.fHdg = mHdg;
+                gsp.fHdg = 0;//mHdg;
             }
             else
             {
-                gsp.fHdg = (mvectorgeosample[mvectorgeosample.size() -1].x,mvectorgeosample[mvectorgeosample.size() -1].y,
+                gsp.fHdg = CalcHdg(mvectorgeosample[mvectorgeosample.size() -1].x,mvectorgeosample[mvectorgeosample.size() -1].y,
                         gsp.x,gsp.y);
             }
         }
@@ -702,7 +799,8 @@ void GeometryPoly3::UpdateSamplePoint()
         s = s+ ds;
     }
 
-//    vector<geosamplepoint> * pxvectorgeosample = &mvectorgeosample;
+
+    vector<geosamplepoint> * pxvectorgeosample = &mvectorgeosample;
     mbHaveSample = true;
 
 }
@@ -762,9 +860,9 @@ void GeometryPoly3::GetCoords(double s_check, double &retX, double &retY, double
         double temX,temY,temHDG;
         if(ipos<=0)
         {
-            temX = mvectorgeosample[ipos].x;
-            temY = mvectorgeosample[ipos].y;
-            temHDG = mHdg;
+            temX = mvectorgeosample[0].x;
+            temY = mvectorgeosample[0].y;
+            temHDG = mvectorgeosample[0].fHdg;;
         }
         else
         {

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

@@ -240,6 +240,14 @@ private:
 	double mRotCos;
 	double mRotSin;
 
+    double mLengthBase; //If Start Curvature not zero
+    double mX0;  //If Start Curvature not zero
+    double mY0;
+    double mHDG0; //If Start Curvature not zero
+
+    double mHDG0Cos;
+    double mHDG0Sin;
+
 public:
 	/**
 	 * Constructor that initializes the base properties of the record

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

@@ -0,0 +1,143 @@
+#include "controller.h"
+
+Controller::Controller(std::string id)
+{
+    mid = id;
+}
+
+void Controller::Setid(std::string id)
+{
+    mid = id;
+}
+
+void Controller::Setname(std::string name)
+{
+    if(mname.size()>0)mname.clear();
+    mname.push_back(name);
+}
+
+void Controller::Setsequence(unsigned int sequence)
+{
+    if(msequence.size()>0)msequence.clear();
+    msequence.push_back(sequence);
+}
+
+std::string Controller::Getid()
+{
+    return mid;
+}
+
+int Controller::Getname(std::string & name)
+{
+    if(mname.size() == 0)return 0;
+    name = mname[0];
+    return 1;
+}
+
+int Controller::Getsequence(unsigned int & sequence)
+{
+    if(msequence.size() == 0)return 0;
+    sequence = msequence[0];
+    return 1;
+}
+
+void Controller::Resetname()
+{
+    if(mname.size()>0)mname.clear();
+}
+
+void Controller::Resetsequence()
+{
+    if(msequence.size()>0)msequence.clear();
+}
+
+vector<Controller_control> * Controller::GetControlVector()
+{
+    return  &mcontrol;
+}
+
+Controller_control* Controller::GetControl(unsigned int i)
+{
+    if ((mcontrol.size()>0)&&(i<(mcontrol.size())))
+        return &(mcontrol.at(i));
+    else
+        return NULL;
+}
+
+unsigned int Controller::GetControlCount()
+{
+    return static_cast<unsigned int >(mcontrol.size()) ;
+}
+
+Controller_control*			Controller::GetLastControl()
+{
+    if (mcontrol.size()>0)
+        return &mcontrol.at(mcontrol.size()-1);
+    else
+        return NULL;
+}
+
+Controller_control*			Controller::GetLastAddedControl()
+{
+    if(mLastAddedControl<mcontrol.size())
+        return &mcontrol.at(mLastAddedControl);
+    else
+        return NULL;
+}
+
+unsigned int Controller::AddControl(std::string signalId)
+{
+    mcontrol.push_back(Controller_control(signalId));
+    mLastAddedControl = static_cast<unsigned int>(mcontrol.size()-1) ;
+    return mLastAddedControl;
+}
+
+unsigned int Controller::CloneControl(unsigned int index)
+{
+    if(index<(mcontrol.size()-1))
+        mcontrol.insert(mcontrol.begin()+index+1, mcontrol[index]);
+    else if(index==mcontrol.size()-1)
+        mcontrol.push_back(mcontrol[index]);
+    mLastAddedControl=index+1;
+    return mLastAddedControl;
+}
+
+void Controller::DeleteControl(unsigned int index)
+{
+    mcontrol.erase(mcontrol.begin()+index);
+}
+
+
+
+Controller_control::Controller_control(std::string signalId)
+{
+    msignalId = signalId;
+}
+
+void Controller_control::SetsignalId(std::string signalId)
+{
+    msignalId = signalId;
+}
+
+void Controller_control::Settype(std::string type)
+{
+    if(mtype.size()>0)mtype.clear();
+    mtype.push_back(type);
+}
+
+std::string Controller_control::GetsignalId()
+{
+    return msignalId;
+}
+
+int Controller_control::Gettype(std::string & type)
+{
+    if(mtype.size() == 0)return 0;
+    type = mtype[0];
+    return 1;
+}
+
+void Controller_control::Resettype()
+{
+    if(mtype.size()>0)mtype.clear();
+}

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

@@ -0,0 +1,66 @@
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include <string>
+#include <vector>
+
+
+using std::vector;
+using std::string;
+
+class Controller_control;
+
+class Controller
+{
+private:
+    std::string mid;
+    std::vector<std::string> mname;
+    std::vector<unsigned int> msequence;
+
+    std::vector<Controller_control> mcontrol;
+    unsigned int mLastAddedControl;
+
+
+public:
+    Controller(std::string id);
+
+    void Setid(std::string id);
+    void Setname(std::string name);
+    void Setsequence(unsigned int sequence);
+
+    std::string Getid();
+    int Getname(std::string & name);
+    int Getsequence(unsigned int & sequence);
+
+    void Resetname();
+    void Resetsequence();
+
+    vector<Controller_control> * GetControlVector();
+    Controller_control* GetControl(unsigned int i);
+    unsigned int GetControlCount();
+    Controller_control*			GetLastControl();
+    Controller_control*			GetLastAddedControl();
+    unsigned int AddControl(std::string signalId);
+    unsigned int CloneControl(unsigned int index);
+    void DeleteControl(unsigned int index);
+};
+
+class Controller_control
+{
+private:
+    std::string msignalId;
+    std::vector<std::string> mtype;
+
+public:
+    Controller_control(std::string signalId);
+
+    void SetsignalId(std::string signalId);
+    void Settype(std::string type);
+
+    std::string GetsignalId();
+    int Gettype(std::string & type);
+
+    void Resettype();
+};
+
+#endif // CONTROLLER_H

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

@@ -80,3 +80,18 @@ bool RoadNoavoid::CheckInterval(double s_check)
     else
         return false;
 }
+
+RoadPriority::RoadPriority(int nPriority)
+{
+    mnPriority = nPriority;
+}
+
+void RoadPriority::SetPriority(int nPriority)
+{
+    mnPriority = nPriority;
+}
+
+int RoadPriority::GetPriority()
+{
+    return mnPriority;
+}

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

@@ -43,4 +43,16 @@ public:
     bool CheckInterval(double s_check);
 };
 
+class RoadPriority
+{
+private:
+    int mnPriority;
+
+public:
+    RoadPriority(int nPriority);
+
+    void SetPriority(int nPriority);
+    int GetPriority();
+};
+
 #endif // USERDATA_H

src/common/common/xodr/odaux/const.cpp

@@ -0,0 +1,255 @@
+/*							const.c
+ *
+ *	Globally declared constants
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * extern double nameofconstant;
+ *
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains a number of mathematical constants and
+ * also some needed size parameters of the computer arithmetic.
+ * The values are supplied as arrays of hexadecimal integers
+ * for IEEE arithmetic; arrays of octal constants for DEC
+ * arithmetic; and in a normal decimal scientific notation for
+ * other machines.  The particular notation used is determined
+ * by a symbol (DEC, IBMPC, or UNK) defined in the include file
+ * mconf.h.
+ *
+ * The default size parameters are as follows.
+ *
+ * For DEC and UNK modes:
+ * MACHEP =  1.38777878078144567553E-17       2**-56
+ * MAXLOG =  8.8029691931113054295988E1       log(2**127)
+ * MINLOG = -8.872283911167299960540E1        log(2**-128)
+ * MAXNUM =  1.701411834604692317316873e38    2**127
+ *
+ * For IEEE arithmetic (IBMPC):
+ * MACHEP =  1.11022302462515654042E-16       2**-53
+ * MAXLOG =  7.09782712893383996843E2         log(2**1024)
+ * MINLOG = -7.08396418532264106224E2         log(2**-1022)
+ * MAXNUM =  1.7976931348623158E308           2**1024
+ *
+ * The global symbols for mathematical constants are
+ * PI     =  3.14159265358979323846           pi
+ * PIO2   =  1.57079632679489661923           pi/2
+ * PIO4   =  7.85398163397448309616E-1        pi/4
+ * SQRT2  =  1.41421356237309504880           sqrt(2)
+ * SQRTH  =  7.07106781186547524401E-1        sqrt(2)/2
+ * LOG2E  =  1.4426950408889634073599         1/log(2)
+ * SQ2OPI =  7.9788456080286535587989E-1      sqrt( 2/pi )
+ * LOGE2  =  6.93147180559945309417E-1        log(2)
+ * LOGSQ2 =  3.46573590279972654709E-1        log(2)/2
+ * THPIO4 =  2.35619449019234492885           3*pi/4
+ * TWOOPI =  6.36619772367581343075535E-1     2/pi
+ *
+ * These lists are subject to change.
+ */
+
+/*							const.c */
+
+/*
+Cephes Math Library Release 2.3:  March, 1995
+Copyright 1984, 1995 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef UNK
+#if 1
+double MACHEP =  1.11022302462515654042E-16;   /* 2**-53 */
+#else
+double MACHEP =  1.38777878078144567553E-17;   /* 2**-56 */
+#endif
+double UFLOWTHRESH =  2.22507385850720138309E-308; /* 2**-1022 */
+#ifdef DENORMAL
+double MAXLOG =  7.09782712893383996732E2;     /* log(MAXNUM) */
+/* double MINLOG = -7.44440071921381262314E2; */     /* log(2**-1074) */
+double MINLOG = -7.451332191019412076235E2;     /* log(2**-1075) */
+#else
+double MAXLOG =  7.08396418532264106224E2;     /* log 2**1022 */
+double MINLOG = -7.08396418532264106224E2;     /* log 2**-1022 */
+#endif
+double MAXNUM =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+double PI     =  3.14159265358979323846;       /* pi */
+double PIO2   =  1.57079632679489661923;       /* pi/2 */
+double PIO4   =  7.85398163397448309616E-1;    /* pi/4 */
+double SQRT2  =  1.41421356237309504880;       /* sqrt(2) */
+double SQRTH  =  7.07106781186547524401E-1;    /* sqrt(2)/2 */
+double LOG2E  =  1.4426950408889634073599;     /* 1/log(2) */
+double SQ2OPI =  7.9788456080286535587989E-1;  /* sqrt( 2/pi ) */
+double LOGE2  =  6.93147180559945309417E-1;    /* log(2) */
+double LOGSQ2 =  3.46573590279972654709E-1;    /* log(2)/2 */
+double THPIO4 =  2.35619449019234492885;       /* 3*pi/4 */
+double TWOOPI =  6.36619772367581343075535E-1; /* 2/pi */
+#ifdef INFINITIES
+//double INFINITY = 1.0/0.0;  /* 99e999; */
+double INFINITY = atof("infinity");  /* 99e999; */
+#else
+double INFINITY =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+#endif
+#ifdef NANS
+double NAN =atof("infinity") - atof("infinity");
+#else
+double NAN = 0.0;
+#endif
+#ifdef MINUSZERO
+double NEGZERO = -0.0;
+#else
+double NEGZERO = 0.0;
+#endif
+#endif
+
+#ifdef IBMPC
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x0000,0x0000,0x0000,0x3ca0};
+unsigned short UFLOWTHRESH[4] = {0x0000,0x0000,0x0000,0x0010};
+#ifdef DENORMAL
+			/* log(MAXNUM) =  7.09782712893383996732224E2 */
+unsigned short MAXLOG[4] = {0x39ef,0xfefa,0x2e42,0x4086};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087};*/
+unsigned short MINLOG[4] = {0x3052,0xd52d,0x4910,0xc087};
+#else
+			/* log(2**1022) =   7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0xbcd2,0xdd7a,0x232b,0x4086};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xbcd2,0xdd7a,0x232b,0xc086};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0xffff,0xffff,0xffff,0x7fef};
+unsigned short PI[4]     = {0x2d18,0x5444,0x21fb,0x4009};
+unsigned short PIO2[4]   = {0x2d18,0x5444,0x21fb,0x3ff9};
+unsigned short PIO4[4]   = {0x2d18,0x5444,0x21fb,0x3fe9};
+unsigned short SQRT2[4]  = {0x3bcd,0x667f,0xa09e,0x3ff6};
+unsigned short SQRTH[4]  = {0x3bcd,0x667f,0xa09e,0x3fe6};
+unsigned short LOG2E[4]  = {0x82fe,0x652b,0x1547,0x3ff7};
+unsigned short SQ2OPI[4] = {0x3651,0x33d4,0x8845,0x3fe9};
+unsigned short LOGE2[4]  = {0x39ef,0xfefa,0x2e42,0x3fe6};
+unsigned short LOGSQ2[4] = {0x39ef,0xfefa,0x2e42,0x3fd6};
+unsigned short THPIO4[4] = {0x21d2,0x7f33,0xd97c,0x4002};
+unsigned short TWOOPI[4] = {0xc883,0x6dc9,0x5f30,0x3fe4};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x0000,0x0000,0x0000,0x7ff0};
+#else
+unsigned short INFINITY[4] = {0xffff,0xffff,0xffff,0x7fef};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x7ffc};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x8000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef MIEEE
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x3ca0,0x0000,0x0000,0x0000};
+unsigned short UFLOWTHRESH[4] = {0x0010,0x0000,0x0000,0x0000};
+#ifdef DENORMAL
+			/* log(2**1024) =   7.09782712893383996843E2 */
+unsigned short MAXLOG[4] = {0x4086,0x2e42,0xfefa,0x39ef};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */
+unsigned short MINLOG[4] = {0xc087,0x4910,0xd52d,0x3052};
+#else
+			/* log(2**1022) =  7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0x4086,0x232b,0xdd7a,0xbcd2};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xc086,0x232b,0xdd7a,0xbcd2};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0x7fef,0xffff,0xffff,0xffff};
+unsigned short PI[4]     = {0x4009,0x21fb,0x5444,0x2d18};
+unsigned short PIO2[4]   = {0x3ff9,0x21fb,0x5444,0x2d18};
+unsigned short PIO4[4]   = {0x3fe9,0x21fb,0x5444,0x2d18};
+unsigned short SQRT2[4]  = {0x3ff6,0xa09e,0x667f,0x3bcd};
+unsigned short SQRTH[4]  = {0x3fe6,0xa09e,0x667f,0x3bcd};
+unsigned short LOG2E[4]  = {0x3ff7,0x1547,0x652b,0x82fe};
+unsigned short SQ2OPI[4] = {0x3fe9,0x8845,0x33d4,0x3651};
+unsigned short LOGE2[4]  = {0x3fe6,0x2e42,0xfefa,0x39ef};
+unsigned short LOGSQ2[4] = {0x3fd6,0x2e42,0xfefa,0x39ef};
+unsigned short THPIO4[4] = {0x4002,0xd97c,0x7f33,0x21d2};
+unsigned short TWOOPI[4] = {0x3fe4,0x5f30,0x6dc9,0xc883};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x7ff0,0x0000,0x0000,0x0000};
+#else
+unsigned short INFINITY[4] = {0x7fef,0xffff,0xffff,0xffff};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x7ff8,0x0000,0x0000,0x0000};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x8000,0x0000,0x0000,0x0000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef DEC
+			/* 2**-56 =  1.38777878078144567553E-17 */
+unsigned short MACHEP[4] = {0022200,0000000,0000000,0000000};
+unsigned short UFLOWTHRESH[4] = {0x0080,0x0000,0x0000,0x0000};
+			/* log 2**127 = 88.029691931113054295988 */
+unsigned short MAXLOG[4] = {041660,007463,0143742,025733,};
+			/* log 2**-128 = -88.72283911167299960540 */
+unsigned short MINLOG[4] = {0141661,071027,0173721,0147572,};
+			/* 2**127 = 1.701411834604692317316873e38 */
+unsigned short MAXNUM[4] = {077777,0177777,0177777,0177777,};
+unsigned short PI[4]     = {040511,007732,0121041,064302,};
+unsigned short PIO2[4]   = {040311,007732,0121041,064302,};
+unsigned short PIO4[4]   = {040111,007732,0121041,064302,};
+unsigned short SQRT2[4]  = {040265,002363,031771,0157145,};
+unsigned short SQRTH[4]  = {040065,002363,031771,0157144,};
+unsigned short LOG2E[4]  = {040270,0125073,024534,013761,};
+unsigned short SQ2OPI[4] = {040114,041051,0117241,0131204,};
+unsigned short LOGE2[4]  = {040061,071027,0173721,0147572,};
+unsigned short LOGSQ2[4] = {037661,071027,0173721,0147572,};
+unsigned short THPIO4[4] = {040426,0145743,0174631,007222,};
+unsigned short TWOOPI[4] = {040042,0174603,067116,042025,};
+/* Approximate infinity by MAXNUM.  */
+unsigned short INFINITY[4] = {077777,0177777,0177777,0177777,};
+unsigned short NAN[4] = {0000000,0000000,0000000,0000000};
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0100000};
+#else
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0000000};
+#endif
+#endif
+
+#ifndef UNK
+extern unsigned short MACHEP[];
+extern unsigned short UFLOWTHRESH[];
+extern unsigned short MAXLOG[];
+extern unsigned short UNDLOG[];
+extern unsigned short MINLOG[];
+extern unsigned short MAXNUM[];
+extern unsigned short PI[];
+extern unsigned short PIO2[];
+extern unsigned short PIO4[];
+extern unsigned short SQRT2[];
+extern unsigned short SQRTH[];
+extern unsigned short LOG2E[];
+extern unsigned short SQ2OPI[];
+extern unsigned short LOGE2[];
+extern unsigned short LOGSQ2[];
+extern unsigned short THPIO4[];
+extern unsigned short TWOOPI[];
+extern unsigned short INFINITY[];
+extern unsigned short NAN[];
+extern unsigned short NEGZERO[];
+#endif

src/common/common/xodr/odaux/fresnl.cpp

@@ -0,0 +1,518 @@
+/*							fresnl.c
+ *
+ *	Fresnel integral
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * double x, S, C;
+ * void fresnl();
+ *
+ * fresnl( x, _&S, _&C );
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates the Fresnel integrals
+ *
+ *           x
+ *           -
+ *          | |
+ * C(x) =   |   cos(pi/2 t**2) dt,
+ *        | |
+ *         -
+ *          0
+ *
+ *           x
+ *           -
+ *          | |
+ * S(x) =   |   sin(pi/2 t**2) dt.
+ *        | |
+ *         -
+ *          0
+ *
+ *
+ * The integrals are evaluated by a power series for x < 1.
+ * For x >= 1 auxiliary functions f(x) and g(x) are employed
+ * such that
+ *
+ * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 )
+ * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 )
+ *
+ *
+ *
+ * ACCURACY:
+ *
+ *  Relative error.
+ *
+ * Arithmetic  function   domain     # trials      peak         rms
+ *   IEEE       S(x)      0, 10       10000       2.0e-15     3.2e-16
+ *   IEEE       C(x)      0, 10       10000       1.8e-15     3.3e-16
+ *   DEC        S(x)      0, 10        6000       2.2e-16     3.9e-17
+ *   DEC        C(x)      0, 10        5000       2.3e-16     3.9e-17
+ */
+
+/*
+Cephes Math Library Release 2.8:  June, 2000
+Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+
+
+/* S(x) for small x */
+#ifdef UNK
+static double sn[6] = {
+-2.99181919401019853726E3,
+ 7.08840045257738576863E5,
+-6.29741486205862506537E7,
+ 2.54890880573376359104E9,
+-4.42979518059697779103E10,
+ 3.18016297876567817986E11,
+};
+static double sd[6] = {
+/* 1.00000000000000000000E0,*/
+ 2.81376268889994315696E2,
+ 4.55847810806532581675E4,
+ 5.17343888770096400730E6,
+ 4.19320245898111231129E8,
+ 2.24411795645340920940E10,
+ 6.07366389490084639049E11,
+};
+#endif
+#ifdef DEC
+static unsigned short sn[24] = {
+0143072,0176433,0065455,0127034,
+0045055,0007200,0134540,0026661,
+0146560,0035061,0023667,0127545,
+0050027,0166503,0002673,0153756,
+0151045,0002721,0121737,0102066,
+0051624,0013177,0033451,0021271,
+};
+static unsigned short sd[24] = {
+/*0040200,0000000,0000000,0000000,*/
+0042214,0130051,0112070,0101617,
+0044062,0010307,0172346,0152510,
+0045635,0160575,0143200,0136642,
+0047307,0171215,0127457,0052361,
+0050647,0031447,0032621,0013510,
+0052015,0064733,0117362,0012653,
+};
+#endif
+#ifdef IBMPC
+static unsigned short sn[24] = {
+0xb5c3,0x6d65,0x5fa3,0xc0a7,
+0x05b6,0x172c,0xa1d0,0x4125,
+0xf5ed,0x24f6,0x0746,0xc18e,
+0x7afe,0x60b7,0xfda8,0x41e2,
+0xf087,0x347b,0xa0ba,0xc224,
+0x2457,0xe6e5,0x82cf,0x4252,
+};
+static unsigned short sd[24] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x1072,0x3287,0x9605,0x4071,
+0xdaa9,0xfe9c,0x4218,0x40e6,
+0x17b4,0xb8d0,0xbc2f,0x4153,
+0xea9e,0xb5e5,0xfe51,0x41b8,
+0x22e9,0xe6b2,0xe664,0x4214,
+0x42b5,0x73de,0xad3b,0x4261,
+};
+#endif
+#ifdef MIEEE
+static unsigned short sn[24] = {
+0xc0a7,0x5fa3,0x6d65,0xb5c3,
+0x4125,0xa1d0,0x172c,0x05b6,
+0xc18e,0x0746,0x24f6,0xf5ed,
+0x41e2,0xfda8,0x60b7,0x7afe,
+0xc224,0xa0ba,0x347b,0xf087,
+0x4252,0x82cf,0xe6e5,0x2457,
+};
+static unsigned short sd[24] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x4071,0x9605,0x3287,0x1072,
+0x40e6,0x4218,0xfe9c,0xdaa9,
+0x4153,0xbc2f,0xb8d0,0x17b4,
+0x41b8,0xfe51,0xb5e5,0xea9e,
+0x4214,0xe664,0xe6b2,0x22e9,
+0x4261,0xad3b,0x73de,0x42b5,
+};
+#endif
+
+/* C(x) for small x */
+#ifdef UNK
+static double cn[6] = {
+-4.98843114573573548651E-8,
+ 9.50428062829859605134E-6,
+-6.45191435683965050962E-4,
+ 1.88843319396703850064E-2,
+-2.05525900955013891793E-1,
+ 9.99999999999999998822E-1,
+};
+static double cd[7] = {
+ 3.99982968972495980367E-12,
+ 9.15439215774657478799E-10,
+ 1.25001862479598821474E-7,
+ 1.22262789024179030997E-5,
+ 8.68029542941784300606E-4,
+ 4.12142090722199792936E-2,
+ 1.00000000000000000118E0,
+};
+#endif
+#ifdef DEC
+static unsigned short cn[24] = {
+0132126,0040141,0063733,0013231,
+0034037,0072223,0010200,0075637,
+0135451,0021020,0073264,0036057,
+0036632,0131520,0101316,0060233,
+0137522,0072541,0136124,0132202,
+0040200,0000000,0000000,0000000,
+};
+static unsigned short cd[28] = {
+0026614,0135503,0051776,0032631,
+0030573,0121116,0154033,0126712,
+0032406,0034100,0012442,0106212,
+0034115,0017567,0150520,0164623,
+0035543,0106171,0177336,0146351,
+0037050,0150073,0000607,0171635,
+0040200,0000000,0000000,0000000,
+};
+#endif
+#ifdef IBMPC
+static unsigned short cn[24] = {
+0x62d3,0x2cfb,0xc80c,0xbe6a,
+0x0f74,0x6210,0xee92,0x3ee3,
+0x8786,0x0ed6,0x2442,0xbf45,
+0xcc13,0x1059,0x566a,0x3f93,
+0x9690,0x378a,0x4eac,0xbfca,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+static unsigned short cd[28] = {
+0xc6b3,0x6a7f,0x9768,0x3d91,
+0x75b9,0xdb03,0x7449,0x3e0f,
+0x5191,0x02a4,0xc708,0x3e80,
+0x1d32,0xfa2a,0xa3ee,0x3ee9,
+0xd99d,0x3fdb,0x718f,0x3f4c,
+0xfe74,0x6030,0x1a07,0x3fa5,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+#endif
+#ifdef MIEEE
+static unsigned short cn[24] = {
+0xbe6a,0xc80c,0x2cfb,0x62d3,
+0x3ee3,0xee92,0x6210,0x0f74,
+0xbf45,0x2442,0x0ed6,0x8786,
+0x3f93,0x566a,0x1059,0xcc13,
+0xbfca,0x4eac,0x378a,0x9690,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+static unsigned short cd[28] = {
+0x3d91,0x9768,0x6a7f,0xc6b3,
+0x3e0f,0x7449,0xdb03,0x75b9,
+0x3e80,0xc708,0x02a4,0x5191,
+0x3ee9,0xa3ee,0xfa2a,0x1d32,
+0x3f4c,0x718f,0x3fdb,0xd99d,
+0x3fa5,0x1a07,0x6030,0xfe74,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+#endif
+
+/* Auxiliary function f(x) */
+#ifdef UNK
+static double fn[10] = {
+  4.21543555043677546506E-1,
+  1.43407919780758885261E-1,
+  1.15220955073585758835E-2,
+  3.45017939782574027900E-4,
+  4.63613749287867322088E-6,
+  3.05568983790257605827E-8,
+  1.02304514164907233465E-10,
+  1.72010743268161828879E-13,
+  1.34283276233062758925E-16,
+  3.76329711269987889006E-20,
+};
+static double fd[10] = {
+/*  1.00000000000000000000E0,*/
+  7.51586398353378947175E-1,
+  1.16888925859191382142E-1,
+  6.44051526508858611005E-3,
+  1.55934409164153020873E-4,
+  1.84627567348930545870E-6,
+  1.12699224763999035261E-8,
+  3.60140029589371370404E-11,
+  5.88754533621578410010E-14,
+  4.52001434074129701496E-17,
+  1.25443237090011264384E-20,
+};
+#endif
+#ifdef DEC
+static unsigned short fn[40] = {
+0037727,0152216,0106601,0016214,
+0037422,0154606,0112710,0071355,
+0036474,0143453,0154253,0166545,
+0035264,0161606,0022250,0073743,
+0033633,0110036,0024653,0136246,
+0032003,0036652,0041164,0036413,
+0027740,0174122,0046305,0036726,
+0025501,0125270,0121317,0167667,
+0023032,0150555,0076175,0047443,
+0020061,0133570,0070130,0027657,
+};
+static unsigned short fd[40] = {
+/*0040200,0000000,0000000,0000000,*/
+0040100,0063767,0054413,0151452,
+0037357,0061566,0007243,0065754,
+0036323,0005365,0033552,0133625,
+0035043,0101123,0000275,0165402,
+0033367,0146614,0110623,0023647,
+0031501,0116644,0125222,0144263,
+0027436,0062051,0117235,0001411,
+0025204,0111543,0056370,0036201,
+0022520,0071351,0015227,0122144,
+0017554,0172240,0112713,0005006,
+};
+#endif
+#ifdef IBMPC
+static unsigned short fn[40] = {
+0x2391,0xd1b0,0xfa91,0x3fda,
+0x0e5e,0xd2b9,0x5b30,0x3fc2,
+0x7dad,0x7b15,0x98e5,0x3f87,
+0x0efc,0xc495,0x9c70,0x3f36,
+0x7795,0xc535,0x7203,0x3ed3,
+0x87a1,0x484e,0x67b5,0x3e60,
+0xa7bb,0x4998,0x1f0a,0x3ddc,
+0xfdf7,0x1459,0x3557,0x3d48,
+0xa9e4,0xaf8f,0x5a2d,0x3ca3,
+0x05f6,0x0e0b,0x36ef,0x3be6,
+};
+static unsigned short fd[40] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x7a65,0xeb21,0x0cfe,0x3fe8,
+0x6d7d,0xc1d4,0xec6e,0x3fbd,
+0x56f3,0xa6ed,0x615e,0x3f7a,
+0xbd60,0x6017,0x704a,0x3f24,
+0x64f5,0x9232,0xf9b1,0x3ebe,
+0x5916,0x9552,0x33b4,0x3e48,
+0xa061,0x33d3,0xcc85,0x3dc3,
+0x0790,0x6b9f,0x926c,0x3d30,
+0xf48d,0x2352,0x0e5d,0x3c8a,
+0x6141,0x12b9,0x9e94,0x3bcd,
+};
+#endif
+#ifdef MIEEE
+static unsigned short fn[40] = {
+0x3fda,0xfa91,0xd1b0,0x2391,
+0x3fc2,0x5b30,0xd2b9,0x0e5e,
+0x3f87,0x98e5,0x7b15,0x7dad,
+0x3f36,0x9c70,0xc495,0x0efc,
+0x3ed3,0x7203,0xc535,0x7795,
+0x3e60,0x67b5,0x484e,0x87a1,
+0x3ddc,0x1f0a,0x4998,0xa7bb,
+0x3d48,0x3557,0x1459,0xfdf7,
+0x3ca3,0x5a2d,0xaf8f,0xa9e4,
+0x3be6,0x36ef,0x0e0b,0x05f6,
+};
+static unsigned short fd[40] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3fe8,0x0cfe,0xeb21,0x7a65,
+0x3fbd,0xec6e,0xc1d4,0x6d7d,
+0x3f7a,0x615e,0xa6ed,0x56f3,
+0x3f24,0x704a,0x6017,0xbd60,
+0x3ebe,0xf9b1,0x9232,0x64f5,
+0x3e48,0x33b4,0x9552,0x5916,
+0x3dc3,0xcc85,0x33d3,0xa061,
+0x3d30,0x926c,0x6b9f,0x0790,
+0x3c8a,0x0e5d,0x2352,0xf48d,
+0x3bcd,0x9e94,0x12b9,0x6141,
+};
+#endif
+
+
+/* Auxiliary function g(x) */
+#ifdef UNK
+static double gn[11] = {
+  5.04442073643383265887E-1,
+  1.97102833525523411709E-1,
+  1.87648584092575249293E-2,
+  6.84079380915393090172E-4,
+  1.15138826111884280931E-5,
+  9.82852443688422223854E-8,
+  4.45344415861750144738E-10,
+  1.08268041139020870318E-12,
+  1.37555460633261799868E-15,
+  8.36354435630677421531E-19,
+  1.86958710162783235106E-22,
+};
+static double gd[11] = {
+/*  1.00000000000000000000E0,*/
+  1.47495759925128324529E0,
+  3.37748989120019970451E-1,
+  2.53603741420338795122E-2,
+  8.14679107184306179049E-4,
+  1.27545075667729118702E-5,
+  1.04314589657571990585E-7,
+  4.60680728146520428211E-10,
+  1.10273215066240270757E-12,
+  1.38796531259578871258E-15,
+  8.39158816283118707363E-19,
+  1.86958710162783236342E-22,
+};
+#endif
+#ifdef DEC
+static unsigned short gn[44] = {
+0040001,0021435,0120406,0053123,
+0037511,0152523,0037703,0122011,
+0036631,0134302,0122721,0110235,
+0035463,0051712,0043215,0114732,
+0034101,0025677,0147725,0057630,
+0032323,0010342,0067523,0002206,
+0030364,0152247,0110007,0054107,
+0026230,0057654,0035464,0047124,
+0023706,0036401,0167705,0045440,
+0021166,0154447,0105632,0142461,
+0016142,0002353,0011175,0170530,
+};
+static unsigned short gd[44] = {
+/*0040200,0000000,0000000,0000000,*/
+0040274,0145551,0016742,0127005,
+0037654,0166557,0076416,0015165,
+0036717,0140217,0030675,0050111,
+0035525,0110060,0076405,0070502,
+0034125,0176061,0060120,0031730,
+0032340,0001615,0054343,0120501,
+0030375,0041414,0070747,0107060,
+0026233,0031034,0160757,0074526,
+0023710,0003341,0137100,0144664,
+0021167,0126414,0023774,0015435,
+0016142,0002353,0011175,0170530,
+};
+#endif
+#ifdef IBMPC
+static unsigned short gn[44] = {
+0xcaca,0xb420,0x2463,0x3fe0,
+0x7481,0x67f8,0x3aaa,0x3fc9,
+0x3214,0x54ba,0x3718,0x3f93,
+0xb33b,0x48d1,0x6a79,0x3f46,
+0xabf3,0xf9fa,0x2577,0x3ee8,
+0x6091,0x4dea,0x621c,0x3e7a,
+0xeb09,0xf200,0x9a94,0x3dfe,
+0x89cb,0x8766,0x0bf5,0x3d73,
+0xa964,0x3df8,0xc7a0,0x3cd8,
+0x58a6,0xf173,0xdb24,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+static unsigned short gd[44] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x55c1,0x23bc,0x996d,0x3ff7,
+0xc34f,0xefa1,0x9dad,0x3fd5,
+0xaa09,0xe637,0xf811,0x3f99,
+0xae28,0x0fa0,0xb206,0x3f4a,
+0x067b,0x2c0a,0xbf86,0x3eea,
+0x7428,0xab1c,0x0071,0x3e7c,
+0xf1c6,0x8e3c,0xa861,0x3dff,
+0xef2b,0x9c3d,0x6643,0x3d73,
+0x1936,0x37c8,0x00dc,0x3cd9,
+0x8364,0x84ff,0xf5a1,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+#endif
+#ifdef MIEEE
+static unsigned short gn[44] = {
+0x3fe0,0x2463,0xb420,0xcaca,
+0x3fc9,0x3aaa,0x67f8,0x7481,
+0x3f93,0x3718,0x54ba,0x3214,
+0x3f46,0x6a79,0x48d1,0xb33b,
+0x3ee8,0x2577,0xf9fa,0xabf3,
+0x3e7a,0x621c,0x4dea,0x6091,
+0x3dfe,0x9a94,0xf200,0xeb09,
+0x3d73,0x0bf5,0x8766,0x89cb,
+0x3cd8,0xc7a0,0x3df8,0xa964,
+0x3c2e,0xdb24,0xf173,0x58a6,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+static unsigned short gd[44] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3ff7,0x996d,0x23bc,0x55c1,
+0x3fd5,0x9dad,0xefa1,0xc34f,
+0x3f99,0xf811,0xe637,0xaa09,
+0x3f4a,0xb206,0x0fa0,0xae28,
+0x3eea,0xbf86,0x2c0a,0x067b,
+0x3e7c,0x0071,0xab1c,0x7428,
+0x3dff,0xa861,0x8e3c,0xf1c6,
+0x3d73,0x6643,0x9c3d,0xef2b,
+0x3cd9,0x00dc,0x37c8,0x1936,
+0x3c2e,0xf5a1,0x84ff,0x8364,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+#endif
+
+#ifdef ANSIPROT
+extern "C" {double fabs ( double );}
+extern "C" {double cos ( double );}
+extern "C" {double sin ( double );}
+extern "C" {double polevl ( double, double *, int );}
+extern "C" {double p1evl ( double, double *, int );}
+#else
+double fabs(), cos(), sin(), polevl(), p1evl();
+#endif
+extern "C" {extern double PI, PIO2, MACHEP;}
+
+//int fresnl( xxa, ssa, cca )
+//double xxa, *ssa, *cca;
+
+int fresnl( double xxa, double *ssa, double *cca )
+{
+double f, g, cc, ss, c, s, t, u;
+double x, x2;
+
+x = fabs(xxa);
+x2 = x * x;
+if( x2 < 2.5625 )
+	{
+	t = x2 * x2;
+	ss = x * x2 * polevl( t, sn, 5)/p1evl( t, sd, 6 );
+	cc = x * polevl( t, cn, 5)/polevl(t, cd, 6 );
+	goto done;
+	}
+
+
+
+
+
+
+if( x > 36974.0 )
+	{
+	cc = 0.5;
+	ss = 0.5;
+	goto done;
+	}
+
+
+/*		Asymptotic power series auxiliary functions
+ *		for large argument
+ */
+	x2 = x * x;
+	t = PI * x2;
+	u = 1.0/(t * t);
+	t = 1.0/t;
+	f = 1.0 - u * polevl( u, fn, 9)/p1evl(u, fd, 10);
+	g = t * polevl( u, gn, 10)/p1evl(u, gd, 11);
+
+	t = PIO2 * x2;
+	c = cos(t);
+	s = sin(t);
+	t = PI * x;
+	cc = 0.5  +  (f * s  -  g * c)/t;
+	ss = 0.5  -  (f * c  +  g * s)/t;
+
+done:
+if( xxa < 0.0 )
+	{
+	cc = -cc;
+	ss = -ss;
+	}
+
+*cca = cc;
+*ssa = ss;
+return(0);
+}

src/common/common/xodr/odaux/mconf.h

@@ -0,0 +1,199 @@
+/*							mconf.h
+ *
+ *	Common include file for math routines
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * #include "mconf.h"
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains definitions for error codes that are
+ * passed to the common error handling routine mtherr()
+ * (which see).
+ *
+ * The file also includes a conditional assembly definition
+ * for the type of computer arithmetic (IEEE, DEC, Motorola
+ * IEEE, or UNKnown).
+ * 
+ * For Digital Equipment PDP-11 and VAX computers, certain
+ * IBM systems, and others that use numbers with a 56-bit
+ * significand, the symbol DEC should be defined.  In this
+ * mode, most floating point constants are given as arrays
+ * of octal integers to eliminate decimal to binary conversion
+ * errors that might be introduced by the compiler.
+ *
+ * For little-endian computers, such as IBM PC, that follow the
+ * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE
+ * Std 754-1985), the symbol IBMPC should be defined.  These
+ * numbers have 53-bit significands.  In this mode, constants
+ * are provided as arrays of hexadecimal 16 bit integers.
+ *
+ * Big-endian IEEE format is denoted MIEEE.  On some RISC
+ * systems such as Sun SPARC, double precision constants
+ * must be stored on 8-byte address boundaries.  Since integer
+ * arrays may be aligned differently, the MIEEE configuration
+ * may fail on such machines.
+ *
+ * To accommodate other types of computer arithmetic, all
+ * constants are also provided in a normal decimal radix
+ * which one can hope are correctly converted to a suitable
+ * format by the available C language compiler.  To invoke
+ * this mode, define the symbol UNK.
+ *
+ * An important difference among these modes is a predefined
+ * set of machine arithmetic constants for each.  The numbers
+ * MACHEP (the machine roundoff error), MAXNUM (largest number
+ * represented), and several other parameters are preset by
+ * the configuration symbol.  Check the file const.c to
+ * ensure that these values are correct for your computer.
+ *
+ * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL
+ * may fail on many systems.  Verify that they are supposed
+ * to work on your computer.
+ */
+/*
+Cephes Math Library Release 2.3:  June, 1995
+Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier
+*/
+
+
+/* Define if the `long double' type works.  */
+#define HAVE_LONG_DOUBLE 1
+
+/* Define as the return type of signal handlers (int or void).  */
+#define RETSIGTYPE void
+
+/* Define if you have the ANSI C header files.  */
+#define STDC_HEADERS 1
+
+/* Define if your processor stores words with the most significant
+   byte first (like Motorola and SPARC, unlike Intel and VAX).  */
+/* #undef WORDS_BIGENDIAN */
+
+/* Define if floating point words are bigendian.  */
+/* #undef FLOAT_WORDS_BIGENDIAN */
+
+/* The number of bytes in a int.  */
+#define SIZEOF_INT 4
+
+/* Define if you have the <string.h> header file.  */
+#define HAVE_STRING_H 1
+
+/* Name of package */
+#define PACKAGE "cephes"
+
+/* Version number of package */
+#define VERSION "2.7"
+
+/* Constant definitions for math error conditions
+ */
+
+//#define DOMAIN		1	/* argument domain error */
+//#define SING		2	/* argument singularity */
+//#define OVERFLOW	3	/* overflow range error */
+//#define UNDERFLOW	4	/* underflow range error */
+//#define TLOSS		5	/* total loss of precision */
+//#define PLOSS		6	/* partial loss of precision */
+
+#define EDOM		33
+#define ERANGE		34
+/* Complex numeral.  */
+typedef struct
+	{
+	double r;
+	double i;
+	} cmplx;
+
+#ifdef HAVE_LONG_DOUBLE
+/* Long double complex numeral.  */
+typedef struct
+	{
+	long double r;
+	long double i;
+	} cmplxl;
+#endif
+
+
+/* Type of computer arithmetic */
+
+/* PDP-11, Pro350, VAX:
+ */
+/* #define DEC 1 */
+
+/* Intel IEEE, low order words come first:
+ */
+/* #define IBMPC 1 */
+
+/* Motorola IEEE, high order words come first
+ * (Sun 680x0 workstation):
+ */
+/* #define MIEEE 1 */
+
+/* UNKnown arithmetic, invokes coefficients given in
+ * normal decimal format.  Beware of range boundary
+ * problems (MACHEP, MAXLOG, etc. in const.c) and
+ * roundoff problems in pow.c:
+ * (Sun SPARCstation)
+ */
+#define UNK 1
+
+/* If you define UNK, then be sure to set BIGENDIAN properly. */
+#ifdef FLOAT_WORDS_BIGENDIAN
+#define BIGENDIAN 1
+#else
+#define BIGENDIAN 0
+#endif
+/* Define this `volatile' if your compiler thinks
+ * that floating point arithmetic obeys the associative
+ * and distributive laws.  It will defeat some optimizations
+ * (but probably not enough of them).
+ *
+ * #define VOLATILE volatile
+ */
+#define VOLATILE
+
+/* For 12-byte long doubles on an i386, pad a 16-bit short 0
+ * to the end of real constants initialized by integer arrays.
+ *
+ * #define XPD 0,
+ *
+ * Otherwise, the type is 10 bytes long and XPD should be
+ * defined blank (e.g., Microsoft C).
+ *
+ * #define XPD
+ */
+#define XPD 0,
+
+/* Define to support tiny denormal numbers, else undefine. */
+#define DENORMAL 1
+
+/* Define to ask for infinity support, else undefine. */
+#define INFINITIES 1
+
+/* Define to ask for support of numbers that are Not-a-Number,
+   else undefine.  This may automatically define INFINITIES in some files. */
+#define NANS 1
+
+/* Define to distinguish between -0.0 and +0.0.  */
+#define MINUSZERO 1
+
+/* Define 1 for ANSI C atan2() function
+   See atan.c and clog.c. */
+#define ANSIC 1
+
+/* Get ANSI function prototypes, if you want them. */
+#if 1
+/* #ifdef __STDC__ */
+#define ANSIPROT 1
+int mtherr ( char *, int );
+#else
+int mtherr();
+#endif
+
+/* Variable for error reporting.  See mtherr.c.  */
+extern int merror;

src/common/common/xodr/odaux/odaux.pri

@@ -0,0 +1,7 @@
+HEADERS += \
+    $$PWD/mconf.h 
+
+SOURCES += \
+    $$PWD/const.cpp \
+    $$PWD/fresnl.cpp \
+    $$PWD/polevl.c

src/common/common/xodr/odaux/polevl.c

@@ -0,0 +1,97 @@
+/*							polevl.c
+ *							p1evl.c
+ *
+ *	Evaluate polynomial
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * int N;
+ * double x, y, coef[N+1], polevl[];
+ *
+ * y = polevl( x, coef, N );
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates polynomial of degree N:
+ *
+ *                     2          N
+ * y  =  C  + C x + C x  +...+ C x
+ *        0    1     2          N
+ *
+ * Coefficients are stored in reverse order:
+ *
+ * coef[0] = C  , ..., coef[N] = C  .
+ *            N                   0
+ *
+ *  The function p1evl() assumes that coef[N] = 1.0 and is
+ * omitted from the array.  Its calling arguments are
+ * otherwise the same as polevl().
+ *
+ *
+ * SPEED:
+ *
+ * In the interest of speed, there are no checks for out
+ * of bounds arithmetic.  This routine is used by most of
+ * the functions in the library.  Depending on available
+ * equipment features, the user may wish to rewrite the
+ * program in microcode or assembly language.
+ *
+ */
+
+
+/*
+Cephes Math Library Release 2.1:  December, 1988
+Copyright 1984, 1987, 1988 by Stephen L. Moshier
+Direct inquiries to 30 Frost Street, Cambridge, MA 02140
+*/
+
+
+double polevl( double x,  double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+int i;
+double *p;
+
+p = coef;
+ans = *p++;
+i = N;
+
+do
+	ans = ans * x  +  *p++;
+while( --i );
+
+return( ans );
+}
+
+/*							p1evl()	*/
+/*                                          N
+ * Evaluate polynomial when coefficient of x  is 1.0.
+ * Otherwise same as polevl.
+ */
+
+double p1evl( double x, double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+double *p;
+int i;
+
+p = coef;
+ans = x + *p++;
+i = N-1;
+
+do
+	ans = ans * x  + *p++;
+while( --i );
+
+return( ans );
+}

src/common/common/xodr/xodrfunc/roadsample.cpp

@@ -0,0 +1,756 @@
+#include "roadsample.h"
+
+#include <math.h>
+
+#include <iostream>
+
+namespace iv {
+
+
+RoadSample::RoadSample(Road * pRoad)
+{
+    mfRefX_max = 0;
+    mfRefY_max = 0;
+    mfRefX_min = 0;
+    mfRefY_min = 0;
+    SampleRoad(pRoad);
+
+    std::vector<RoadPoint> xvectorRoadPoint = mvectorRoadPoint;
+
+}
+
+int RoadSample::SampleRoad(Road * pRoad)
+{
+    mfRoadLen = pRoad->GetRoadLength();
+    mstrroadid = pRoad->GetRoadId();
+    mstrroadname = pRoad->GetRoadName();
+
+    bool bMaxMinInit = false;
+    bool bRealMaxMinInit = false;
+
+    double fS = 0;
+    const double fStep = 0.1;
+    LaneSection * pLS;
+    unsigned int nLSCount = pRoad->GetLaneSectionCount();
+    unsigned int i;
+    int nSec = 0;
+
+//#ifdef ODRTOLANELET
+//    const double fSampleMark = 10.0;
+//#else
+     const double fSampleMark = 2.5;
+//#endif
+
+
+
+    bool bMarkBrokenVisable = false;
+
+    for(i=0;i<nLSCount;i++)
+    {
+        pLS = pRoad->GetLaneSection(i);
+
+        if(pLS == NULL)
+        {
+            break;
+        }
+
+
+        bMarkBrokenVisable = false;
+        std::vector<double> xvectorFeaturePoint = GetFeaturePoint(pLS);
+
+        double fLastRefX,fLastRefY,fLastRefZ,fLastRefHdg,fLastOffsetValue,fLastS;
+        bool bHaveLast = false;
+        std::vector<LaneSamplePoint> xvectorLastLanePoint;
+
+        double sstart_Section = pLS->GetS();
+
+        double send_Section = pLS->GetS();
+        if(i == (nLSCount -1))
+        {
+            send_Section = pRoad->GetRoadLength();
+        }
+        else
+        {
+            LaneSection * pLSNext = pRoad->GetLaneSection(i+1);
+            if(pLSNext != NULL)
+            {
+                send_Section = pLSNext->GetS();
+            }
+        }
+        nSec++;
+
+        fS = sstart_Section;
+        while(fS<send_Section)
+        {
+            bool bInserPoint = false;
+            if(fabs(send_Section - fS)<=fStep)
+            {
+                fS = send_Section;  //If Section End,Same Point
+                if(i !=(nLSCount -1))
+                {
+                    fS = send_Section - 0.0000001; //if not the last,set a value small to sample;
+                }
+                bInserPoint = true;
+            }
+            double fRefX,fRefY,fRefZ,fRefHdg,fOffsetValue;
+            pRoad->GetGeometryCoords(fS,fRefX,fRefY,fRefHdg);
+            fRefZ = pRoad->GetElevationValue(fS);
+            fOffsetValue = pRoad->GetLaneOffsetValue(fS);
+
+            if(fabs(fOffsetValue) > mfMaxLaneOff)mfMaxLaneOff = fabs(fOffsetValue);
+
+
+
+
+
+            bool bIsCrossFeature = false;
+
+            if((fS - sstart_Section)>fStep*0.5)
+                bIsCrossFeature = IsCrossFeaturePoint(xvectorFeaturePoint,fS,fLastS);
+
+            if(bIsCrossFeature)
+            {
+                if(fS> send_Section)fS = send_Section;
+            }
+
+            RoadPoint xRP;
+            xRP.ms = fS;
+            xRP.mfRefX = fRefX;
+            xRP.mfRefY = fRefY;
+            xRP.mfRefZ = fRefZ;
+            xRP.mfHdg = fRefHdg;
+            xRP.mfLaneOffValue = fOffsetValue;
+            xRP.nSecNum = nSec;
+
+            if(bMaxMinInit)
+            {
+                if(mfRefX_max<fRefX)mfRefX_max = fRefX;
+                if(mfRefX_min>fRefX)mfRefX_min = fRefX;
+                if(mfRefY_max<fRefY)mfRefY_max = fRefY;
+                if(mfRefY_min>fRefY)mfRefY_min = fRefY;
+            }
+            else
+            {
+                mfRefX_max = fRefX;
+                mfRefX_min = fRefX;
+                mfRefY_max = fRefY;
+                mfRefY_min = fRefY;
+                bMaxMinInit = true;
+            }
+
+            std::vector<LaneSamplePoint> xvectorLanePoint;
+ //           SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
+
+
+
+
+            if(bHaveLast == false)
+            {
+                bInserPoint = true;
+            }
+            else
+            {
+                if(fabs((fOffsetValue - fLastOffsetValue)/(fS - fLastS))>0.1)
+                {
+                    bInserPoint = true;
+                }
+                else
+                {
+                    double fHdgDiff = fRefHdg - fLastRefHdg;
+                    if(fHdgDiff>M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
+                    if(fHdgDiff<-M_PI)fHdgDiff = fHdgDiff + 2.0*M_PI;
+                    if((fabs((fHdgDiff)/(1.0))>0.05) ||((fS - fLastS)>=fSampleMark))
+                    {
+                        bInserPoint = true;
+                    }
+                }
+            }
+
+
+     //       if((bInserPoint == false)&&(bHaveLast))
+            if((bHaveLast))
+            {
+
+                if(bIsCrossFeature)
+                {
+                    SampleLanePoint(pRoad,pLS,fS-0.0000001,xvectorLanePoint);
+                    xRP.mvectorLanePoint = xvectorLanePoint;
+                    unsigned int k;
+                    unsigned int ksize = static_cast<unsigned int >(xRP.mvectorLanePoint.size());
+                    for(k=0;k<ksize;k++)
+                    {
+    //                    std::vector<LaneSamplePoint> *pvectorLastLanePoint = &mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint;;
+                        xRP.mvectorLanePoint[k].mstrroadmarkcolor = mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mstrroadmarkcolor;
+                        xRP.mvectorLanePoint[k].mstrroadmarktype = mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mstrroadmarktype;
+                    }
+                    mvectorRoadPoint.push_back(xRP);
+                    xvectorLanePoint.clear();
+                    SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
+                    bInserPoint = true;
+                    nSec++;
+                    xRP.nSecNum = nSec;
+                }
+
+//                if(IsMarkChange(xvectorLanePoint,xvectorLastLanePoint))
+//                {
+//                    bInserPoint = true;
+//                    nSec++;
+//                    xRP.nSecNum = nSec;
+//                    mvectorRoadPoint.push_back(mvectorRoadPoint[mvectorRoadPoint.size() -1]);
+//                    mvectorRoadPoint[mvectorRoadPoint.size()-1].nSecNum = nSec;
+//                }
+            }
+
+            if((bInserPoint == false)&&(bHaveLast))
+            {
+
+                if((fS-fLastS)>1.0)
+                {
+                    double fGradientMax = GetMaxWidthRatio(pLS,fS);
+                    if(fabs(fGradientMax*(fS-fLastS))>0.1)
+                    {
+                        bInserPoint = true;
+                    }
+                }
+
+
+//                xRP.mvectorLanePoint = xvectorLanePoint;
+//                unsigned int k;
+//                unsigned int ksize = static_cast<unsigned int >(xRP.mvectorLanePoint.size());
+//                for(k=0;k<ksize;k++)
+//                {
+//                    if(fabs(xRP.mvectorLanePoint[k].mfLaneWidth - mvectorRoadPoint[mvectorRoadPoint.size()-1].mvectorLanePoint[k].mfLaneWidth )>0.1)
+//                    {
+//                        bInserPoint = true;
+//                        break;
+//                    }
+//                }
+            }
+
+            if((bInserPoint == false)&&(bHaveLast))
+            {
+                double fcalmod = fmod(fS-sstart_Section,10.0);
+                double fcalmod2 = fmod(fLastS-sstart_Section,10.0);
+                if((fcalmod>=2.5)&&(fcalmod2<2.5))
+                {
+                    bInserPoint = true;
+                }
+                if((fcalmod>=7.5)&&(fcalmod2<7.5))
+                {
+                    bInserPoint = true;
+                }
+
+
+            }
+
+            if(bHaveLast)
+            {
+                double fcalmod = fmod(fS-sstart_Section,10.0);
+                if(fcalmod<2.5)
+                {
+                    bMarkBrokenVisable = false;
+                }
+                else
+                {
+                    if(fcalmod<7.5)
+                    {
+                        bMarkBrokenVisable = true;
+                    }
+                    else
+                        bMarkBrokenVisable = false;
+                }
+            }
+
+
+
+            if((i == (nLSCount -1))&&(fabs(fS - send_Section)<0.001))
+            {
+                bInserPoint = true;
+            }
+
+            if((bIsCrossFeature== false)&&bInserPoint)
+            {
+                SampleLanePoint(pRoad,pLS,fS,xvectorLanePoint);
+            }
+
+
+            if(bInserPoint)
+            {
+                xRP.mvectorLanePoint = xvectorLanePoint;
+//                if(xvectorLanePoint.size() == 0)
+//                {
+//                    int a = 1;
+//                    a++;
+//                }
+                xRP.mbMarkBrokenVisable = bMarkBrokenVisable;
+                if(fabs(xRP.mfLaneOffValue)<0.01)
+                {
+                    xRP.mfCenterX = xRP.mfRefX;
+                    xRP.mfCenterY = xRP.mfRefY;
+                }
+                else
+                {
+                    xRP.mfCenterX = xRP.mfRefX + xRP.mfLaneOffValue * cos(xRP.mfHdg + M_PI/2.0);
+                    xRP.mfCenterY = xRP.mfRefY + xRP.mfLaneOffValue * sin(xRP.mfHdg + M_PI/2.0);
+                }
+
+                xRP.mfRoadWidthLeft = pRoad->GetRoadLeftWidth(fS);
+                xRP.mfRoadWidthRight = pRoad->GetRoadRightWidth(fS);
+
+                xRP.mfBorderX_Left = xRP.mfCenterX + xRP.mfRoadWidthLeft * cos(xRP.mfHdg + M_PI/2.0);
+                xRP.mfBorderY_Left = xRP.mfCenterY + xRP.mfRoadWidthLeft * sin(xRP.mfHdg + M_PI/2.0);
+
+                xRP.mfBorderX_Right = xRP.mfCenterX + xRP.mfRoadWidthRight * cos(xRP.mfHdg - M_PI/2.0);
+                xRP.mfBorderY_Right = xRP.mfCenterY + xRP.mfRoadWidthRight * sin(xRP.mfHdg - M_PI/2.0);
+
+
+                if(bRealMaxMinInit == false)
+                {
+                    mfX_max = xRP.mfBorderX_Left;
+                    mfX_min = xRP.mfBorderX_Left;
+                    mfY_max = xRP.mfBorderY_Left;
+                    mfY_min = xRP.mfBorderY_Left;
+                    bRealMaxMinInit = true;
+                }
+
+                if(bRealMaxMinInit)
+                {
+                    if(mfX_max<xRP.mfBorderX_Left)mfX_max = xRP.mfBorderX_Left;
+                    if(mfX_min>xRP.mfBorderX_Left)mfX_min = xRP.mfBorderX_Left;
+                    if(mfY_max<xRP.mfBorderY_Left)mfY_max = xRP.mfBorderY_Left;
+                    if(mfY_min>xRP.mfBorderY_Left)mfY_min = xRP.mfBorderY_Left;
+                    if(mfX_max<xRP.mfBorderX_Right)mfX_max = xRP.mfBorderX_Right;
+                    if(mfX_min>xRP.mfBorderX_Right)mfX_min = xRP.mfBorderX_Right;
+                    if(mfY_max<xRP.mfBorderY_Right)mfY_max = xRP.mfBorderY_Right;
+                    if(mfY_min>xRP.mfBorderY_Right)mfY_min = xRP.mfBorderY_Right;
+                }
+
+
+                mvectorRoadPoint.push_back(xRP);
+
+                fLastS = fS;
+                fLastRefX = fRefX;
+                fLastRefY = fRefY;
+                fLastRefZ = fRefZ;
+                fLastRefHdg = fRefHdg;
+                fLastOffsetValue = fOffsetValue;
+                xvectorLastLanePoint = xvectorLanePoint;
+                bHaveLast = true;
+            }
+
+
+            fS = fS + fStep;
+        }
+    }
+
+
+    if(pRoad->GetRoadId() == "16")
+    {
+        int a = 1;
+        a++;
+    }
+    std::vector<RoadPoint> * pvectorRoadPoint = &mvectorRoadPoint;
+
+    std::cout<<"Road: "<<mstrroadid<<" max "<<mfRefX_max<<" "<<mfRefY_max<<"  min "<<mfRefX_min<<" "<<mfRefY_min<<std::endl;
+
+    return 0;
+}
+
+int RoadSample::SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::vector<LaneSamplePoint> & xvectorLanePoint)
+{
+
+    unsigned int nLeftLaneCount = pLS->GetLeftLaneCount();
+    unsigned int nRightLaneCount = pLS->GetRightLaneCount();
+
+
+    std::vector<double> xLeftWidth = pRoad->GetLaneWidthVector(fS,1);
+    std::vector<double> xRightWidth = pRoad->GetLaneWidthVector(fS,2);
+
+    if(static_cast<unsigned int>(xLeftWidth.size()) != nLeftLaneCount )
+    {
+        std::cout<<"RoadSample::SampleLanePoint xLeftWidth not equal nLeftLaneCount. "<<" left width size: "<<xLeftWidth.size()
+                <<" nLeftLaneCount: "<<nLeftLaneCount<<std::endl;
+        return -1;
+    }
+
+    if(static_cast<unsigned int>(xRightWidth.size()) != nRightLaneCount )
+    {
+        std::cout<<"RoadSample::SampleLanePoint xRightWidth not equal nRIghtLaneCount. "<<" right width size: "<<xRightWidth.size()
+                <<" nRightLaneCount: "<<nRightLaneCount<<std::endl;
+        return -2;
+    }
+
+    std::vector<double> xvectorLeftLaneT;
+    std::vector<double> xvectorRightLaneT;
+
+    unsigned int i;
+    for(i=0;i<nLeftLaneCount;i++)
+    {
+        unsigned int j;
+        double fValue = xLeftWidth[nLeftLaneCount-i-1];
+        for(j=(nLeftLaneCount-i-1);j>0;j--)
+        {
+            fValue = fValue + xLeftWidth[j-1];
+        }
+        xvectorLeftLaneT.push_back(fValue);
+    }
+
+    for(i=0;i<nRightLaneCount;i++)
+    {
+        unsigned int j;
+        double fValue = 0;
+        for(j=0;j<=i;j++)
+        {
+            fValue = fValue - xRightWidth[j];
+        }
+        xvectorRightLaneT.push_back(fValue);
+    }
+
+
+    iv::LaneSamplePoint xLanePoint;
+    for(i=0;i<nLeftLaneCount;i++)
+    {      
+        pRoad->GetLaneCoords(fS,xvectorLeftLaneT[i],xLanePoint.mx,xLanePoint.my,xLanePoint.mz,xLanePoint.mhdg);
+        LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,static_cast<int>(nLeftLaneCount -i));
+        xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
+        xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
+        xLanePoint.mstrroadmarktype = xRoadMark.GetType();
+        xLanePoint.nLane = static_cast<int>(nLeftLaneCount -i);
+        xLanePoint.mstrLaneType = pLS->GetLeftLaneAt(nLeftLaneCount -i)->GetType();
+        xLanePoint.mfLaneWidth = xLeftWidth[nLeftLaneCount-1 -i];
+        xvectorLanePoint.push_back(xLanePoint);
+    }
+
+    pRoad->GetLaneCoords(fS,0,xLanePoint.mx,xLanePoint.my,xLanePoint.mz,xLanePoint.mhdg);
+    LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,0);
+    xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
+    xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
+    xLanePoint.mstrroadmarktype = xRoadMark.GetType();
+    xLanePoint.nLane = static_cast<int>(0);
+    xLanePoint.mstrLaneType = pLS->GetCenterLane()->GetType();
+    xLanePoint.mfLaneWidth = 0.0;
+    xvectorLanePoint.push_back(xLanePoint);
+
+    for(i=0;i<nRightLaneCount;i++)
+    {
+        pRoad->GetLaneCoords(fS,xvectorRightLaneT[i],xLanePoint.mx,xLanePoint.my,xLanePoint.mz,xLanePoint.mhdg);
+        LaneRoadMark xRoadMark = pRoad->GetLaneRoadMarkValue(fS,static_cast<int>(i+1)*(-1));
+        xLanePoint.mfmarkwidth = xRoadMark.GetWidth();
+        xLanePoint.mstrroadmarkcolor = xRoadMark.GetColor();
+        xLanePoint.mstrroadmarktype = xRoadMark.GetType();
+        xLanePoint.mfLaneWidth = xRightWidth[i];
+        xLanePoint.nLane = static_cast<int>(i+1) *(-1);
+        xLanePoint.mstrLaneType = pLS->GetRightLaneAt(i+1)->GetType();
+        xvectorLanePoint.push_back(xLanePoint);
+    }
+
+
+
+
+
+    return 0;
+}
+
+bool RoadSample::IsMarkChange(std::vector<LaneSamplePoint> & xvectorLastLanePoint,std::vector<LaneSamplePoint> & xvectorLanePoint)
+{
+    if(xvectorLanePoint.size() != xvectorLastLanePoint.size())
+    {
+        return true;
+    }
+
+    unsigned int i;
+    unsigned int nSize = static_cast<unsigned int >(xvectorLanePoint.size());
+
+    for(i=0;i<nSize;i++)
+    {
+        if(xvectorLanePoint[i].mstrroadmarkcolor != xvectorLastLanePoint[i].mstrroadmarkcolor)
+        {
+            return true;
+        }
+        if(xvectorLanePoint[i].mstrroadmarktype != xvectorLastLanePoint[i].mstrroadmarktype)
+        {
+            return true;
+        }
+    }
+
+    return false;
+
+
+}
+
+std::vector<RoadPoint> * RoadSample::GetRoadPointVector()
+{
+    return  &mvectorRoadPoint;
+}
+
+std::vector<double> RoadSample::GetFeaturePoint(LaneSection * pLS)
+{
+    unsigned int i;
+    unsigned int nlanecount = pLS->GetLaneCount();
+    std::vector<double> xvectorS;
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLane(i);
+        if(pLane != NULL)
+        {
+            unsigned int nMarkCount = pLane->GetLaneRoadMarkCount();
+            unsigned int j;
+            for(j=0;j<nMarkCount;j++)
+            {
+                LaneRoadMark * pMark = pLane->GetLaneRoadMark(j);
+                if(fabs(pMark->GetS())>0.001)
+                {
+                    xvectorS.push_back(pMark->GetS());
+                }
+            }
+        }
+    }
+
+    std::vector<double> xvectorFeature;
+    if(xvectorS.size() == 0)return xvectorFeature;
+    while(xvectorS.size()>0)
+    {
+        unsigned int indexmin = 0;
+        unsigned int nsize = static_cast<unsigned int >(xvectorS.size());
+        for(i=1;i<nsize;i++)
+        {
+            if(xvectorS[i] < xvectorS[indexmin])
+            {
+                indexmin = i;
+            }
+        }
+        if(xvectorFeature.size() == 0)
+        {
+            xvectorFeature.push_back(xvectorS[indexmin]);
+        }
+        else
+        {
+            if(fabs(xvectorS[indexmin] - xvectorFeature[xvectorFeature.size()-1]) > 0.001)
+            {
+                xvectorFeature.push_back(xvectorS[indexmin]);
+            }
+        }
+        xvectorS.erase(xvectorS.begin() + indexmin);
+    }
+
+    return xvectorFeature;
+}
+
+bool RoadSample::IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double & fS,double fSLast)
+{
+    unsigned int nSize = static_cast<unsigned int>(xvectorFeature.size());
+    unsigned int i;
+    for(i=0;i<nSize;i++)
+    {
+        if(((fSLast - xvectorFeature[i])<-0.001)&&((fS+0.1-xvectorFeature[i])>0))
+        {
+            fS = xvectorFeature[i];
+            return true;
+        }
+    }
+
+    return false;
+}
+
+double RoadSample::GetMaxWidthRatio(LaneSection * pLS, double fS)
+{
+    unsigned int nlanecount = static_cast<unsigned int>(pLS->GetLaneCount());
+    unsigned int i;
+    double fRtn = 0.0;
+    for(i=0;i<nlanecount;i++)
+    {
+        Lane * pLane = pLS->GetLane(i);
+        if(pLane != NULL)
+        {
+            if(pLane->GetId() != 0)
+            {
+               double fGradient =  pLane->GetWidthValueGradient(fS-pLS->GetS());
+               if(fabs(fGradient)>fabs(fRtn))
+               {
+                   fRtn = fGradient;
+               }
+            }
+        }
+    }
+    return fRtn;
+}
+
+std::string RoadSample::GetRoadID()
+{
+    return mstrroadid;
+}
+
+std::string RoadSample::GetRoadName()
+{
+    return mstrroadname;
+}
+
+double RoadSample::GetRefXMax()
+{
+    return  mfRefX_max;
+}
+
+double RoadSample::GetRefXMin()
+{
+    return  mfRefX_min;
+}
+
+double RoadSample::GetRefYMax()
+{
+    return mfRefY_max;
+}
+
+double RoadSample::GetRefYMin()
+{
+    return  mfRefY_min;
+}
+
+double RoadSample::GetXMax()
+{
+    return  mfX_max;
+}
+
+double RoadSample::GetXMin()
+{
+    return  mfX_min;
+}
+
+double RoadSample::GetYMax()
+{
+    return mfY_max;
+}
+
+double RoadSample::GetYMin()
+{
+    return  mfY_min;
+}
+
+bool RoadSample::PointInRoad(const double x, const double y,  double &s,double & t,int & nlane, std::string & strlanetype)
+{
+    if((x<mfX_min)||(x>mfX_max))
+    {
+        return false;
+    }
+    if((y<mfY_min)||(y>mfY_max))
+    {
+        return false;
+    }
+
+    unsigned int i;
+    unsigned int nRPCount = static_cast<unsigned int >(mvectorRoadPoint.size());
+
+    for(i=0;i<(nRPCount -1);i++)
+    {
+        if(mvectorRoadPoint[i].nSecNum != mvectorRoadPoint[i+1].nSecNum)  //s same
+        {
+            continue;
+        }
+        if(PointInQuadrilateral(x,y,PointRS(mvectorRoadPoint[i].mfBorderX_Left,mvectorRoadPoint[i].mfBorderY_Left),
+                                PointRS(mvectorRoadPoint[i+1].mfBorderX_Left,mvectorRoadPoint[i+1].mfBorderY_Left),
+                                PointRS(mvectorRoadPoint[i+1].mfBorderX_Right,mvectorRoadPoint[i+1].mfBorderY_Right),
+                                PointRS(mvectorRoadPoint[i].mfBorderX_Right,mvectorRoadPoint[i].mfBorderY_Right)))
+        {
+            unsigned int j;
+            unsigned int nlanecount = static_cast<unsigned int>(mvectorRoadPoint[i].mvectorLanePoint.size()) ;
+
+            for(j=0;j<(nlanecount -1);j++)
+            {
+                iv::RoadPoint p1 = mvectorRoadPoint[i];
+                iv::RoadPoint p2 = mvectorRoadPoint[i+1];
+                if(PointInQuadrilateral(x,y,PointRS(p1.mvectorLanePoint[j].mx,p1.mvectorLanePoint[j].my),
+                                        PointRS(p2.mvectorLanePoint[j].mx,p2.mvectorLanePoint[j].my),
+                                        PointRS(p2.mvectorLanePoint[j+1].mx,p2.mvectorLanePoint[j+1].my),
+                                        PointRS(p1.mvectorLanePoint[j+1].mx,p1.mvectorLanePoint[j+1].my)))
+                {
+                    double t_off = pldis(PointRS(x,y),
+                                         PointRS(p1.mfCenterX,p1.mfCenterY),
+                                         PointRS(p2.mfCenterX,p2.mfCenterY));
+                    if(p1.mvectorLanePoint[j].nLane<=0)t_off = t_off*(-1.0);
+                    double s_off = pldis(PointRS(x,y),
+                                         PointRS(p1.mvectorLanePoint[j].mx,p1.mvectorLanePoint[j].my),
+                                         PointRS(p1.mvectorLanePoint[j+1].mx,p1.mvectorLanePoint[j+1].my));
+                    s = p1.ms + s_off;
+                    t = t_off;
+                    if(p1.mvectorLanePoint[j].nLane<=0)
+                    {
+                        nlane = p1.mvectorLanePoint[j+1].nLane;
+                        strlanetype =  p1.mvectorLanePoint[j+1].mstrLaneType;
+                    }
+                    else
+                    {
+                        nlane = p1.mvectorLanePoint[j].nLane;
+                        strlanetype =  p1.mvectorLanePoint[j].mstrLaneType;
+                    }
+
+                    return true;
+                }
+            }
+
+            std::cout<<"RoadSample::PointInRoad Warning."<<std::endl;
+        }
+    }
+
+    return false;
+}
+
+bool RoadSample::PointInQuadrilateral(double x, double y, PointRS A,PointRS B,PointRS C, PointRS D)
+{
+    double a = (B.x - A.x)*(y - A.y) - (B.y - A.y)*(x - A.x);
+    double b = (C.x - B.x)*(y - B.y) - (C.y - B.y)*(x - B.x);
+    double c = (D.x - C.x)*(y - C.y) - (D.y - C.y)*(x - C.x);
+    double d = (A.x - D.x)*(y - D.y) - (A.y - D.y)*(x - D.x);
+    if((a >= 0 && b >= 0 && c >= 0 && d >= 0) || (a <= 0 && b <= 0 && c <= 0 && d <= 0)) {
+        return true;
+    }
+    return  false;
+}
+
+
+
+//参考https://blog.csdn.net/Mr_robot_strange/article/details/123495084
+double RoadSample::pldis(iv::PointRS a,iv::PointRS b,iv::PointRS c)
+{
+
+    iv::PointRS s1(c.x-b.x,c.y-b.y);
+    iv::PointRS s2(a.x-b.x,a.y-b.y);
+    iv::PointRS s3(a.x-c.x,a.y-c.y);
+
+    double fdis_ab = sqrt(pow(a.x-b.x,2) + pow(a.y - b.y,2));
+    double fdis_ac = sqrt(pow(a.x-c.x,2) + pow(a.y - c.y,2));
+    double fdis_bc = sqrt(pow(b.x-c.x,2) + pow(b.y - c.y,2));
+
+    double cmult_bac = (a.x - b.x) * (c.y-b.y) - (c.x - b.x) * (a.y - b.y);
+
+    double pmults1s2 = s1.x*s2.x + s1.y*s2.y;
+    double pmults1s3 = s1.x*s3.x + s1.y*s3.y;
+
+    if((b.x == c.x) && (b.y == c.y))
+    {
+        return fdis_ab;
+    }
+
+    if(pmults1s2<-0.0000001)
+    {
+        return  fdis_ab;
+    }
+    else
+    {
+        if(pmults1s3>0.000001)
+        {
+            return fdis_ac;
+        }
+        else
+        {
+            return fabs(cmult_bac)/fdis_bc;
+        }
+    }
+
+    return 0;
+
+
+}
+
+}
+
+
+

src/common/common/xodr/xodrfunc/roadsample.h

@@ -0,0 +1,136 @@
+#ifndef ROADSAMPLE_H
+#define ROADSAMPLE_H
+
+#include <string>
+#include <vector>
+
+#include "OpenDrive/OpenDrive.h"
+
+namespace iv
+{
+
+struct PointRS
+{
+public:
+    double x;
+    double y;
+    PointRS(double xset,double yset)
+    {
+        x = xset;
+        y = yset;
+    }
+};
+
+struct LaneSamplePoint
+{
+public:
+
+    //
+    int nLane; // 1 left 1   0 center lane   -1 right lane1
+    double mx;
+    double my;
+    double mz;
+    double mhdg;
+    std::string mstrroadmarktype = "none";
+    std::string mstrroadmarkcolor = "standard";
+    //IF solid all is visialbe,  5-5 in view. if broken mark type  6-9   2-4 3-6  if motor way is 6-9 or 3-6 other 2-4
+    double mfroadmarkwidth;
+
+    double mfLaneWidth;
+    double mfmarkwidth;
+    std::string mstrLaneType;  //if nlane = 0 center lane, lanetype is not valid.
+
+};
+
+struct RoadPoint
+{
+public:
+    double ms;
+    std::vector<LaneSamplePoint> mvectorLanePoint;  //From Left to Right
+
+    double mfRefX;
+    double mfRefY;
+    double mfRefZ;
+    double mfHdg;
+    double mfLaneOffValue;
+
+    double mfCenterX;   //Because LaneOffset, the Centerline or Refline is not the real center, so add CenterX CenterY define the real.
+    double mfCenterY;
+
+    double mfBorderX_Left;  //Left Border, Right Border, use to describe the road border
+    double mfBorderY_Left;
+
+    double mfBorderX_Right;
+    double mfBorderY_Right;
+
+    double mfRoadWidthLeft;
+    double mfRoadWidthRight;
+
+
+    bool mbMarkBrokenVisable = false;
+
+    int nSecNum;  //If lane count change or lane roadmarkchange is new section.
+
+};
+
+
+
+class RoadSample
+{
+public:
+    RoadSample(Road * pRoad);
+
+    std::vector<RoadPoint> * GetRoadPointVector();
+
+    std::string GetRoadID();
+    std::string GetRoadName();
+    double GetRefXMax();
+    double GetRefXMin();
+    double GetRefYMax();
+    double GetRefYMin();
+
+    double GetXMax();
+    double GetXMin();
+    double GetYMax();
+    double GetYMin();
+
+    bool PointInRoad(const double x, const double y,  double & s, double & t, int & nlane, std::string & strlanetype);
+
+private:
+    std::string mstrroadid;
+    std::string mstrroadname;
+    double mfRoadLen;
+    std::vector<RoadPoint> mvectorRoadPoint;
+    double mfRefX_min,mfRefX_max,mfRefY_min,mfRefY_max;
+    double mfX_min,mfX_max,mfY_min,mfY_max;
+    double mfMaxLaneOff = 0.0;
+    double mfMaxRoadWidth = 0.0;
+
+private:
+    /**
+     * @brief Samle OpenDrive Road
+     * @param pRoad
+     * @return
+     */
+    int SampleRoad(Road * pRoad);
+
+
+    int SampleLanePoint(Road * pRoad,LaneSection * pLS, double fS,std::vector<LaneSamplePoint> & xvectorLanePoint);
+
+    bool IsMarkChange(std::vector<LaneSamplePoint> & xvectorLastLanePoint,std::vector<LaneSamplePoint> & xvectorLanePoint);
+
+    std::vector<double> GetFeaturePoint(LaneSection * pLS);
+
+    bool IsCrossFeaturePoint(std::vector<double> & xvectorFeature,double & fS,double fSLast);
+
+    inline double GetMaxWidthRatio(LaneSection * pLS, double fS);
+
+    bool PointInQuadrilateral(double x, double y, PointRS A,PointRS B,PointRS C, PointRS D);
+
+    double pldis(iv::PointRS a,iv::PointRS b,iv::PointRS c);
+
+};
+
+}
+
+#endif // ROADSAMPLE_H

src/common/common/xodr/xodrfunc/xodrdijkstra.cpp

@@ -1709,7 +1709,12 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
         pathsection xps;
         Road * pRoad = mroadedge[xvectorpath[i]].mpx;
         int nsuggest = nSel;
+        if(i == (nsize-1))
+        {
+            nsuggest = 100;//最右侧
+        }
         if(mroadedge[xvectorpath[i]].mnleftright == 2)nsuggest  = nsuggest *(-1);
+
         int nlane = xodrfunc::GetDrivingLane(pRoad,mroadedge[xvectorpath[i]].mnsectionid,nsuggest);
         xps.mainsel = nlane;
         xps.mnStartLaneSel = nlane;
@@ -1988,9 +1993,9 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
                         {
                             if(pLS->GetLane(k)->IsPredecessorSet())
                             {
-                                if(pLS->GetLane(k)->GetPredecessor() == pLane2->GetSuccessor())
+                                if(pLS->GetLane(k)->GetPredecessor() == pLane2->GetId())
                                 {
-                                    xpathsection[i].secondsel = k;
+                                    xpathsection[i].secondsel = pLS->GetLane(k)->GetId();
                                     break;
                                 }
                             }
@@ -1999,9 +2004,9 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
                 }
                 else
                 {
-                    if(pLane2->IsSuccessorSet())
+                    if(pLane2->IsPredecessorSet())
                     {
-                        xpathsection[i].secondsel = pLane2->GetSuccessor();
+                        xpathsection[i].secondsel = pLane2->GetPredecessor();
                     }
 //                    if(pLane2->IsPredecessorSet())
 //                    {
@@ -2015,9 +2020,9 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
                         {
                             if(pLS->GetLane(k)->IsSuccessorSet())
                             {
-                                if(pLS->GetLane(k)->GetSuccessor() == pLane2->GetPredecessor())
+                                if(pLS->GetLane(k)->GetSuccessor() == pLane2->GetId())
                                 {
-                                    xpathsection[i].secondsel = k;
+                                    xpathsection[i].secondsel = pLS->GetLane(k)->GetId();
                                     break;
                                 }
                             }

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -655,6 +655,74 @@ int xodrfunc::GetNearPointAtRoad(const double x, const double y, Road *pRoad, Ge
 }
 
 
+int xodrfunc::GetNearPointWithHide2(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad,
+                                    double &fdis, double &nearx, double &neary,
+                                   double &nearhead, const double nearthresh,
+                                   std::vector<int> &xvectorhideroad, double *pfs,
+                                   int *pnlane, bool bnotuselane)
+{
+    double dismin = std::numeric_limits<double>::infinity();
+    fdis = dismin;
+    unsigned int i;
+    *pObjRoad = 0;
+    std::vector<iv::nearoption> xvectornearopt;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        unsigned int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh,frels;
+
+        if(xvectorhideroad.size() > 0)
+        {
+            int nroadid = atoi(proad->GetRoadId().data());
+            unsigned int k;
+            bool bIsHiden = false;
+            for(k = 0;k<xvectorhideroad.size();k++)
+            {
+                if(xvectorhideroad[k] == nroadid)
+                {
+                    bIsHiden = true;
+                    break;
+                }
+            }
+            if(bIsHiden)continue;
+        }
+
+        double fRefDis,fHdgDiff,s,refx,refy,refhdg;
+        int nlane = 1000;
+        double fdisroad = proad->GetDis(x,y,0,fRefDis,fHdgDiff,nlane,s,refx,refy,refhdg);
+
+        if(nlane != 1000)
+        {
+            fdis = fdisroad;
+            nearx = refx;
+            neary = refy;
+            nearhead = refhdg;
+            *pObjRoad = proad;
+            if(pfs != 0)*pfs = s;
+            if(pnlane != 0)*pnlane = nlane;
+             return 0;
+        }
+
+        if(fdisroad < fdis)
+        {
+            fdis = fdisroad;
+            nearx = refx;
+            neary = refy;
+            nearhead = refhdg;
+            *pObjRoad = proad;
+            if(pfs != 0)*pfs = s;
+            if(pnlane != 0)*pnlane = nlane;
+        }
+    }
+
+
+    if(fdis > nearthresh)return -1;
+    return 0;
+}
+
+
+
 int xodrfunc::GetNearPointWithHide(const double x, const double y, OpenDrive *pxodr, Road **pObjRoad,
                                    GeometryBlock **pgeo, double &fdis, double &nearx, double &neary,
                                    double &nearhead, const double nearthresh,
@@ -1347,6 +1415,7 @@ int xodrfunc::GetDrivingLane(Road *pRoad, const int nLS, const int nsuggestlane)
     }
 
     nrtn = 1000;
+    if(nsuggestlane<0)nrtn = nrtn * (-1);
     int ndiff;
 
     for(i=0;i<nLaneCount;i++)
@@ -1354,16 +1423,34 @@ int xodrfunc::GetDrivingLane(Road *pRoad, const int nLS, const int nsuggestlane)
         Lane * pLane = pLS->GetLane(i);
         if((pLane->GetId()*nsuggestlane>0)&&(pLane->GetType() == "driving"))
         {
-            ndiff = pLane->GetId() - nrtn;
-            int xdiff = pLane->GetId() - nsuggestlane;
-            if(abs(xdiff)<abs(ndiff))
+            //Select Max Lane ID.
+            if(abs(nsuggestlane)>10)
             {
-                nrtn = pLane->GetId();
+                if(abs(nrtn) == 1000)
+                {
+                    nrtn = pLane->GetId();
+                }
+                else
+                {
+                    if(abs(nrtn)<abs(pLane->GetId()))
+                    {
+                        nrtn = pLane->GetId();
+                    }
+                }
+            }
+            else
+            {
+                ndiff = pLane->GetId() - nrtn;
+                int xdiff = pLane->GetId() - nsuggestlane;
+                if(abs(xdiff)<abs(ndiff))
+                {
+                    nrtn = pLane->GetId();
+                }
             }
         }
     }
 
-    if(nrtn == 1000)
+    if(abs(nrtn) == 1000)
     {
         std::cout<<"Waring. Maybe no driving road in this side."<<std::endl;
         for(i=0;i<nLaneCount;i++)

src/common/common/xodr/xodrfunc/xodrfunc.h

@@ -53,6 +53,10 @@ public:
                      double & neary,double & nearhead,const double nearthresh,std::vector<int> & xvectorhideroad,
                                     double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
 
+    static int GetNearPointWithHide2(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,double & fdis,double & nearx,
+                     double & neary,double & nearhead,const double nearthresh,std::vector<int> & xvectorhideroad,
+                                    double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
+
     static int GetNearPointAtRoad(const double x,const double y,Road *pRoad,GeometryBlock ** pgeo, double & fdis,double & nearx,
                      double & neary,double & nearhead,const double nearthresh,double * pfs=0,int * pnlane= 0,bool bnotuselane = false);
 

src/common/modulecomm/android/gradlew

@@ -15,8 +15,10 @@ unix:system("./../../../include/linuxsystemtest.sh ")
 
 unix:include(./../../../include/systemdef.pri)
 win32: DEFINES += SYSTEM_WIN
+
 DEFINES += MODULECOMM_NO_FASTRTPS
-#DEFINES += USE_GROUPUDP
+
+DEFINES += USE_GROUPUDP
 
 if(contains(DEFINES,USE_GROUPUDP)){
 QT += network

src/common/modulecomm/modulecomm.xml

@@ -34,24 +34,24 @@
 //#define BOLDCYAN    "\033[1m\033[36m"      /* Bold Cyan */
 //#define BOLDWHITE   "\033[1m\033[37m"      /* Bold White */
 
-class AttachThread : public QThread
-{
-  public:
-    AttachThread(QSharedMemory * pa,bool & bAttach)
-    {
-       mbAttach = bAttach;
-       mpa = pa;
-       mbrun = true;
-    }
-    QSharedMemory * mpa;
-    bool mbAttach = false;
-    bool mbrun = true;
-    void run()
-    {
-        mbAttach = mpa->attach();
-        mbrun = false;
-    }
-};
+//class AttachThread : public QThread
+//{
+//  public:
+//    AttachThread(QSharedMemory * pa,bool & bAttach)
+//    {
+//       mbAttach = bAttach;
+//       mpa = pa;
+//       mbrun = true;
+//    }
+//    QSharedMemory * mpa;
+//    bool mbAttach = false;
+//    bool mbrun = true;
+//    void run()
+//    {
+//        mbAttach = mpa->attach();
+//        mbrun = false;
+//    }
+//};
 
 void procsm::threadAttachMon()
 {
@@ -127,6 +127,7 @@ procsm::procsm(const char * strsmname,const unsigned int nBufSize,const unsigned
     char strasmname[300];
 
 
+    mnMode = nMode;
     if(nMode == ModeWrite)
     {
         int nrtn = CreateASMPTR(strasmname,nBufSize,nMaxPacCount);
@@ -314,6 +315,7 @@ int procsm::CreateAndAttachASM(char * strasmname,const unsigned int nBufSize,con
                 mpinfo->mNext = 0;
                 mpinfo->mLock = 0;
                 mpASM->unlock();
+                std::cout<<"recreate successfully."<<std::endl;
             }
             else
             {
@@ -345,9 +347,10 @@ int procsm::CreateAndAttachASM(char * strasmname,const unsigned int nBufSize,con
     }
 
 
+    std::cout<<strsmname<<"  is attached."<<std::endl;
     if(mpASM->isAttached())
     {
-
+        std::cout<<strsmname<<" attach succesfully."<<std::endl;
         mbAttach = true;
         char * p = (char *)mpASM->data();
         mpinfo = (procsm_info *)p;
@@ -413,31 +416,17 @@ void procsm::recreateasm(int nbufsize)
     char strout[300];
     snprintf(strout,300,"new asm name is %s,buffer size is %d ",mASM_State.mstrshmname,mnBufSize);
     ivstdcolorout(strout,iv::STDCOLOR_GREEN);
+
+
+
+
 //    qDebug("new asm name is %s,buffer size is %d ",mASM_State.mstrshmname,mnBufSize);
     mpASM = new QSharedMemory(mASM_State.mstrshmname);
 
-    bool bAttach = false;
-    AttachThread AT(mpASM,bAttach);
-    AT.start();
-    QTime xTime;
-    xTime.start();
-    while(xTime.elapsed()<100)
-    {
-        if(AT.mbrun == false)
-        {
-            bAttach = AT.mbAttach;
-            break;
-        }
-    }
-    //       qDebug("time is %d",xTime.elapsed());
-    if(xTime.elapsed()>= 1000)
-    {
-        qDebug("in 1000ms Attach fail.terminate it .");
-        AT.terminate();
-        bAttach = false;
-    }
+    mpASM->attach();
+
+    bool bAttach = mpASM->isAttached();
 
-    //       if(!mpASM->attach())
     if(!bAttach)
     {
 
@@ -448,9 +437,6 @@ void procsm::recreateasm(int nbufsize)
         mphead = (procsm_head *)(p+sizeof(procsm_info));
         mpinfo->mCap = mnMaxPacCount;
         mpinfo->mnBufSize = mnBufSize;
-//        mpinfo->mFirst = nfirst;
-//        mpinfo->mNext = nnext;
-//        mpinfo->mLock = 0;
     }
 
 
@@ -465,8 +451,6 @@ void procsm::recreateasm(int nbufsize)
         mphead = (procsm_head *)(p+sizeof(procsm_info));
         mnMaxPacCount = mpinfo->mCap;
         mnBufSize = mpinfo->mnBufSize;
-        //        qDebug("attach successful");
- //       mstrtem = new char[mnBufSize];
     }
     else
     {
@@ -524,34 +508,51 @@ bool procsm::AttachMem()
             mphead = (procsm_head *)(p+sizeof(procsm_info));
             mnMaxPacCount = mpinfo->mCap;
             mnBufSize = mpinfo->mnBufSize;
+
+            ivstdcolorout("AttachMem Successfully.");
             return true;
         }
         else
         {
+//            std::cout<<" mode: "<<mnMode<<" asm name: "<<mASM_State.mstrshmname<<std::endl;
+//            ivstdcolorout(" AttachMem: ASM Attach Fail. ",iv::STDCOLOR_BOLDYELLOW);
+            if(mnMode == ModeWrite)
+            {
+
+
+                int nrtn = CreateAndAttachASM(pasmptr->mstrshmname,mmodulemsg_type.mnBufSize,
+                                              mmodulemsg_type.mnMsgBufCount,mmodulemsg_type.mstrmsgidname);
+                if(nrtn <0 )
+                {
+                    char strerr[256];
+                    snprintf(strerr,256,"AttachMem  CreateAndAttachASMFail. error code: %d",nrtn);
+                    ivstdcolorout(strerr,iv::STDCOLOR_BOLDRED);
+                    return false;
+                }
+                else
+                {
+                    return true;
+                }
+
+            }
+
+
             return false;
         }
     }
     else
     {
+        if(mnMode == ModeWrite)
+        {
+            char strout[256];
+            snprintf(strout,256,"%s  Attach ASMPtr Fail.",mstrsmname);
+            ivstdcolorout(" AttachMem: Attach Fail. ",iv::STDCOLOR_BOLDYELLOW);
+        }
         return false;
     }
 
     return false;
-    mpASM->attach();
-    if(mpASM->isAttached())
-    {
-        mbAttach = true;
-        char * p = (char *)mpASM->data();
-        mpinfo = (procsm_info *)p;
-        mphead = (procsm_head *)(p+sizeof(procsm_info));
-        mnMaxPacCount = mpinfo->mCap;
-        mnBufSize = mpinfo->mnBufSize;
-        return true;
-    }
-    else
-    {
-        return false;
-    }
+
 }
 
 int procsm::MoveMem(const unsigned int nSize)
@@ -616,10 +617,19 @@ int  procsm::checkasm()
     if((pASM_PTR->mnUpdateTime == mASM_State.mnUpdateTime) && (mbAttach == true) )
     {
 
+
         mpASMPtr->unlock();
         return 0;
     }
     qDebug("reattch mem.");
+    if(pASM_PTR->mnUpdateTime == mASM_State.mnUpdateTime)
+    {
+        std::cout<<" checkasm: mbAttach is false";
+    }
+    else
+    {
+        std::cout<<" updateTime not equal."<<std::endl;
+    }
     mbAttach = false;
     AttachMem();
     mpASMPtr->unlock();

src/common/modulecomm/shm/procsm.h

@@ -115,6 +115,8 @@ public:
 
     iv::modulemsg_type mmodulemsg_type;
 
+    int mnMode;
+
 #ifndef USE_GROUPUDP
 #ifdef USEDBUS
 private slots:

src/common/modulecomm/shm/procsm_if.cpp

@@ -198,6 +198,7 @@ void procsm_if_readthread::run()
 }
 
 
+
 #ifdef USE_GROUPUDP
     void procsm_if_readthread::WakeRead()
     {
@@ -285,6 +286,7 @@ int procsm_if::writemsg(const char *str, const unsigned int nSize)
 #ifdef USELCM
     int nres = mlcm.publish(mstrsmname,str,nSize);
     qDebug("publish message. res = %d",nres);
+    mmutex.unlock();
     return 0;
 #endif
     if(mnType == procsm::ModeRead)return -1; //this is listen.

src/common/modulecomm/testmodulecomm.pro

@@ -9,7 +9,7 @@ unix:system("./../../../include/linuxsystemtest.sh ")
 unix:include(./../../../include/systemdef.pri)
 win32: DEFINES += SYSTEM_WIN
 
-#DEFINES += USE_GROUPUDP
+DEFINES += USE_GROUPUDP
 
 if(contains(DEFINES,USE_GROUPUDP)){
 QT += network

src/common/modulecomm/testmodulecomm_android.pro

@@ -1,4 +1,4 @@
-QT       += core gui
+QT       += core gui xml
 
 #QT += dbus
 
@@ -12,7 +12,7 @@ CONFIG += c++11
 # deprecated API in order to know how to port your code away from it.
 DEFINES += QT_DEPRECATED_WARNINGS
 
-DEFINES += USE_FASTRTPS
+#DEFINES += USE_FASTRTPS
 #DEFINES += USEDBUS
 
 DEFINES += Module1
@@ -77,7 +77,7 @@ contains(ANDROID_TARGET_ARCH,arm64-v8a) {
     ANDROID_PACKAGE_SOURCE_DIR = \
         $$PWD/android
 
-    ANDROID_EXTRA_LIBS = \
-        $$PWD/androidlib/libfastcdr.so \
-        $$PWD/androidlib/libfastrtps.so
+#    ANDROID_EXTRA_LIBS = \
+#        $$PWD/androidlib/libfastcdr.so \
+#        $$PWD/androidlib/libfastrtps.so
 }

src/controller/controller_Geely_xingyueL/controller_Geely_xingyueL.pro

@@ -2,7 +2,7 @@ QT -= gui
 
 QT += network xml dbus
 
-CONFIG += c++11 console
+CONFIG += c++11 #console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use

src/controller/controller_Geely_xingyueL/include/glog_config.h

@@ -0,0 +1,12 @@
+#pragma once
+#include <glog/logging.h>
+/*************************************************
+函数名称:    google_glog_config
+函数描述:    检查并配置日志存储目录
+作者：       seahu dong
+输入参数:    char *argv
+输出参数:    无
+返回说明:    0:ok ;-1:failed
+其它说明:    无
+*************************************************/
+int google_glog_config(char *argv);

src/controller/controller_Geely_xingyueL/main.cpp

@@ -171,6 +171,7 @@ void executeDecition(const iv::brain::decition decition)
     car_control_module.set_target_acc_mps2(speedSetVal);
     car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);
 
+
     if(decition.has_leftlamp() && decition.leftlamp()==true)
         car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
     else if(decition.has_rightlamp() && decition.rightlamp()==true)
@@ -363,6 +364,7 @@ void sendthread()
         switch (app_ctrl_car_sm_)
         {
             case AppCtrlSm::kStandby://ctrl_car节点有消息输入，否则退出与汽车的控制
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" kStandby"<<std::endl;
                 if (communicate_state!=0)
                 {
                     app_ctrl_car_sm_ = AppCtrlSm::kStartup;
@@ -376,6 +378,7 @@ void sendthread()
                 }
                 break;
             case AppCtrlSm::kStartup:
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" kStartup"<<std::endl;
                 if (car_control_module.is_lat_lgt_ctrl_active()) //底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
                 {
                     app_ctrl_car_sm_ = AppCtrlSm::kActive;
@@ -383,7 +386,7 @@ void sendthread()
                 }
                 else {
                     if (!communicate_state){//通讯失联
-                        app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
+                        app_ctrl_car_sm_ = AppCtrlSm::kStandby;
                         case_state=11;
                     }
                     else {
@@ -395,9 +398,10 @@ void sendthread()
                 }
                 break;
             case AppCtrlSm::kActive:
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" kActive"<<std::endl;
                 if (!(communicate_state && car_control_module.is_lat_lgt_ctrl_active()))//通讯失联或底盘不可控
                 {
-                    app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
+                    app_ctrl_car_sm_ = AppCtrlSm::kStandby;
                     case_state=30;
                     if(!car_control_module.is_lat_lgt_ctrl_active())
                     {
@@ -414,6 +418,7 @@ void sendthread()
                 }
                 break;
             case AppCtrlSm::kShutDown:
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" kShutdown"<<std::endl;
                 if (car_control_module.get_chassis_statemachine_sts() != StsMach::kApaActive &&
                     car_control_module.get_chassis_statemachine_sts() != StsMach::kHwaActive)
                 {

src/decition/common/common/boost.h

@@ -45,7 +45,7 @@ namespace iv {
         std::int16_t wheel_angle;		//方向盘转角
         std::uint8_t braking_pressure;	//刹车压力
          float torque_st=0;
-        bool mbRunPause = false;
+        bool mbRunPause = true;
         bool mbBrainCtring = false;
         bool mbdoor=false,has_mbdoor=false;//false: door close    true:door open
         bool mbjinguang=false,has_mbjinguang=false;//false: off    true:on

src/decition/common/common/common.pri

@@ -78,9 +78,9 @@ int main(int argc, char *argv[])
     gstrmemchassis = xp.GetParam("chassismsgname","chassis");
     gstrmembraincarstate = xp.GetParam("carstate","carstate");
 
-    iv::decition::BrainDecition brain;
-    brain.inialize();
-    brain.start();
+    iv::decition::BrainDecition *pbrain = new iv::decition::BrainDecition();
+    pbrain->inialize();
+    pbrain->start();
 
     iv::ivexit::RegIVExitCall(ExitFunc);
 
@@ -88,7 +88,7 @@ int main(int argc, char *argv[])
 
     int nrc =  a.exec();
 
-    brain.stop();
+    pbrain->stop();
 
     return nrc;
 }

src/decition/common/perception/eyes.cpp

@@ -99,6 +99,14 @@ void iv::perception::Eyes::stopAllSensor() {
     sensor_gps->stop();
 }
 
+//void iv::perception::Eyes::SetGPS(iv::GPSData gpsdata)
+//{
+//    mmutexgps.lock();
+//    gps_ins_data_ = gpsdata;
+//    mgpsindex++;
+//    mmutexgps.unlock();
+//}
+
 void iv::perception::Eyes::getSensorObstacle(iv::ObsRadar& brain_obs_radar, iv::CameraInfoPtr& brain_obs_camera, iv::GPSData& brain_gps_data, iv::LidarGridPtr& brain_obs_lidar_gridptr) {
     //brain_obs_camera = NULL;
     brain_gps_data = NULL;

src/decition/common/perception_sf/eyes.h

@@ -33,6 +33,9 @@ namespace iv {
             iv::CameraInfoPtr obs_camera_;
             iv::GPSData gps_ins_data_;
 
+ //           void SetGPS(iv::GPSData gpsdata);
+ //           std::mutex mmutexgps;
+
         private:
 
             void cb_lidar(iv::ObsLiDAR obs);

src/decition/common/perception_sf/impl_gps.cpp

@@ -333,7 +333,9 @@ void iv::perception::GPSSensor::UpdateGPSIMU(iv::gps::gpsimu xgpsimu)
     ServiceCarStatus.speed = data->speed;
 //       qDebug("speed x is %g",ServiceCarStatus.speed);
     if(data->ins_status == 4)
+    {
         signal_gps_data->operator()(data);	//传输数据
+    }
 
     ServiceCarStatus.location->gps_lat = data->gps_lat;
     ServiceCarStatus.location->gps_lng = data->gps_lng;

src/decition/common/platform/dataformat.h

@@ -116,7 +116,9 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
 //        {
 //           controlSpeed=max(controlSpeed,-0.5f);
 //        }
-        controlSpeed=min(controlSpeed,0.5f);
+        if(dSpeed<3.0)
+            controlSpeed=min(controlSpeed,0.5f);
+
     }
 
     //controlSpeed=0.1;
@@ -191,9 +193,9 @@ float iv::decition::Jeely_xingyueL_adapter::limitBrake(float realSpeed, float co
     }else{
         BrakeIntMax = 10;
     }
-    if(obsDistance>0 && obsDistance<10){
-        BrakeIntMax=max(BrakeIntMax,3);
-    }
+//    if(obsDistance>0 && obsDistance<10){
+//        BrakeIntMax=max(BrakeIntMax,3);
+//    }
 
     if (controlBrake > BrakeIntMax) controlBrake = BrakeIntMax;
 
@@ -209,6 +211,12 @@ float iv::decition::Jeely_xingyueL_adapter::limitBrake(float realSpeed, float co
 float iv::decition::Jeely_xingyueL_adapter::limitSpeed(float realSpeed, float controlBrake,float dSpeed,float controlSpeed ){
 //    controlSpeed=min((float)5.0,controlSpeed);
 //    controlSpeed=max((float)-7.0,controlSpeed);
+
+    controlSpeed=min((float)12.0,controlSpeed);
+    controlSpeed=max((float)-12.0,controlSpeed);
+    return controlSpeed;
+
+
     if(realSpeed >= 0.0 && realSpeed <= 6.0){
         controlSpeed=min((float)1.27,controlSpeed);
         controlSpeed=max((float)-1.28,controlSpeed);

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_controller/pid_controller.cpp

@@ -12,6 +12,7 @@ using namespace std;
 iv::decition::PIDController::PIDController(){
     controller_id = 0;
     controller_name="pid";
+ //   std::cout<<" Init.Init.Init.Init.Init.Init."<<std::endl;
 }
 iv::decition::PIDController::~PIDController(){
 
@@ -48,7 +49,27 @@ iv::decition::Decition iv::decition::PIDController::getControlDecition(GPS_INS n
     return *decition;
 }
 
+double iv::decition::PIDController::CalcKappa(std::vector<Point2D> trace,int preindex)
+{
+    if(trace.size() == 0)return 0;
+    if(preindex>=trace.size())return 0;
+    if(preindex == 0)return 0;
+    double denominator = 2 * trace[preindex].x *(-1);
+    double numerator = pow(trace[preindex].x,2) + pow(trace[preindex].y,2);
+    double fRadius = 1e9;
+    if(fabs(denominator)>0)
+    {
+        fRadius = numerator/denominator;
+    }
+    else
+    {
+        fRadius = 1e9;
+    }
+    if(fRadius == 0)return  0;
+    assert(fRadius != 0);
+    return 1.0/fRadius;
 
+}
 
 
 float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Point2D> trace, int roadMode){
@@ -56,6 +77,11 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     double EPos = 0, EAng = 0;
 
     double KEang = 14, KEPos = 10, DEang = 0, DEPos = 0,IEpos=0,IEang=0;
+    if(realSpeed > 50)
+    {
+        KEang = 5;
+        KEPos = 3;
+    }
     if(ServiceCarStatus.msysparam.mvehtype=="zhongche"){
         KEang = 14, KEPos = 100,IEpos=3,IEang=0.5;
     }
@@ -90,6 +116,12 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
     std::vector<Point2D> farTrace;
 
     int nsize = trace.size();
+
+    if(nsize == 0)
+    {
+        std::cout<<" no plan trace."<<std::endl;
+        return 0;
+    }
     for (int i = 1; i < nsize-1; i++)
     {
         sumdis += GetDistance(trace[i - 1], trace[i]);
@@ -99,6 +131,18 @@ float iv::decition::PIDController::getPidAngle(float realSpeed, std::vector<Poin
             break;
         }
     }
+
+    if((sumdis>=1.0)&&(gpsIndex == 0))gpsIndex = nsize -1;
+    bool bUseAutowareKappa = true;
+    double wheel_base = 2.8;
+    double wheelratio = 13.6;
+
+    if(bUseAutowareKappa)
+    {
+        double kappa = CalcKappa(trace,gpsIndex);
+        return (-1.0)*kappa * wheel_base * wheelratio * 180.0/M_PI;
+    }
+
     if(gpsIndex == 0)   gpsIndex = nsize -1;
 
     EPos = trace[gpsIndex].x;
@@ -225,7 +269,7 @@ float iv::decition::PIDController::getPidAcc (GPS_INS gpsIns,float dSpeed, float
     }
 
     vt=min((float)vt,dSecSpeed);
-    std::cout << "\nvt:%f\n" << vt << std::endl;
+//    std::cout << "\nvt:%f\n" << vt << std::endl;
 
     //计算加速度
     float u =computeU(obsDistance,ds,vs,vt,vh,vl,ttc,ttcr);
@@ -236,6 +280,7 @@ float iv::decition::PIDController::getPidAcc (GPS_INS gpsIns,float dSpeed, float
     lastVt = vt;
     lastU = u;
     float acc=0-u;
+//    std::cout<<" acc: "<<acc<<" lastu:"<<lastU<<std::endl;
     return acc;
 }
 
@@ -249,7 +294,7 @@ float iv::decition::PIDController::computeU(float obsDistance, float ds,float vs
     double u = 0;
     if (ttc>10)
     {
-        double kp = 0.5;        //double kp = 0.5;
+        double kp =1.0;// 0.5;        //double kp = 0.5;
         double kd = 0.5;       //kd = 0.03
         //      double k11 = 0.025;
         //      double k12 = 0.000125;
@@ -262,6 +307,12 @@ float iv::decition::PIDController::computeU(float obsDistance, float ds,float vs
         }
         //u = kp * ev + kd * dev + k11 * Iv + k12 * Id;
         u = kp * ev + kd * dev;
+
+//        if((vt<30.0)&&((vt-vs)>0.5)&&(vt>2.0))
+//        {
+//            u = -1.0;
+//        }
+//        std::cout<<" u: "<<u<<std::endl;
     }
     else if (ttc < 1.6 || (ttc <= 10 && obsDistance <= 0.75 * ds))
     {
@@ -303,6 +354,7 @@ float iv::decition::PIDController::computeU(float obsDistance, float ds,float vs
 
 
 float iv::decition::PIDController::limiteU(float u,float ttc){
+//    std::cout<<"u: "<<u<<" last u: "<<lastU<<std::endl;
     if(ttc<3 && u<-0.2){
         u=-0.2;
     }
@@ -324,7 +376,9 @@ float iv::decition::PIDController::limiteU(float u,float ttc){
     }
     else if (u <= 0 && lastU <= 0)
     {
-        if (u < lastU - 0.04) u = lastU - 0.04;
+
+        if (u < (lastU - 0.04)) u = lastU - 0.04;
+
     }
     return u;
 }

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_controller/pid_controller.h

@@ -19,7 +19,7 @@ namespace iv {
                         float lastEP;
                         float lastAng;
                         float angleLimit=600;
-                        float lastU ;
+
                         float lastEv ;
                         float lastVt ;
 
@@ -54,8 +54,11 @@ namespace iv {
                         float  computeU(float obsDistance, float ds,float vs,float vt,float vh,float vl,float ttc,float ttcr);
                         float  limiteU(float u ,float ttc);
 
+                        double CalcKappa(std::vector<Point2D> trace,int preindex);
+
 
                     private:
+                        float lastU ;
 
                     };
 

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_tools/PolynomialRegression.h

@@ -47,12 +47,9 @@ iv::GPS_INS  iv::decition::Compute00::dTpoint2;
 iv::GPS_INS  iv::decition::Compute00::dTpoint3;
 double  iv::decition::Compute00::dBocheAngle;
 
-
 std::vector<int> lastEsrIdVector;
 std::vector<int> lastEsrCountVector;
 
-
-
 int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
 {
     int startIndex = 0;     // startIndex = 0 则每一次都是遍历整条地图路线
@@ -194,6 +191,46 @@ int iv::decition::Compute00::getDesiredSpeed(std::vector<GPSData> gpsMap)
 }
 
 
+//int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
+//    double distance,deltaphi;
+//    //x,y,phi in rad
+//    doubledata.clear();
+//    for (int i = 0; i < MapTrace.size(); i++) {   //                                       1225/14:25
+//                 std::vector<double> temp;
+//                 temp.clear();
+//                 temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push四个空量，然后就可以给量赋值了
+//                 doubledata.push_back(temp);
+//                 doubledata[i][0] = MapTrace.at(i)->gps_x;
+//                 doubledata[i][1] = MapTrace.at(i)->gps_y;
+//                 doubledata[i][2] = (MapTrace.at(i)->ins_heading_angle) / 180 * PI;
+//                 doubledata[i][3]=0;
+//                 doubledata[i][4]=0;
+//    }
+//    // compute delta///////////////////////////////////////////////////////////////
+//    for (int i = 0; i < doubledata.size()-10; i++)
+//    {
+//                deltaphi=doubledata[i+10][2]-doubledata[i][2];
+//                if (deltaphi>PI)
+//                        {deltaphi=deltaphi-2*PI; }
+//                if (deltaphi<-PI)
+//                        {deltaphi=deltaphi+2*PI;}
+//                distance=sqrt( pow((doubledata[i+10][0]-doubledata[i][0]),2)+pow((doubledata[i+10][1]-doubledata[i][1]),2));
+//                doubledata[i][3]=-atan( 4.0* (deltaphi) / distance  );//车辆坐标系和惯导坐标系方向相反，delta变号
+//    }
+//                doubledata[doubledata.size()-1][3] =  doubledata[doubledata.size()-2][3];
+
+//    //delta filter
+//    for(int j=10;j<doubledata.size()-10;j++)
+//    {
+//        double delta_sum=0;
+//        for(int k=j-10;k<j+10;k++)
+//        {
+//            delta_sum+= doubledata[k][3];
+//        }
+//        doubledata[j][3]=delta_sum/20;
+//    }
+//}
+
 int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
     double distance,deltaphi;
     //x,y,phi in rad
@@ -232,9 +269,28 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
         }
         doubledata[j][3]=delta_sum/20;
     }
-}
 
+    ///计算全局地图S
+    bool flag=false;
+    if(fabs((*MapTrace[MapTrace.size()-1]).frenet_s)<1.0e-6){
+        double sum_s=0.0;
+        for(int i=0;i<MapTrace.size();i++)
+        {
+            if ((i==0)&&(flag==false))
+            {
+                sum_s=0.0;
+                (*MapTrace[i]).frenet_s=0;
+                flag=true;
+            }
+            else
+            {
 
+                sum_s = sum_s+sqrt(pow(((*MapTrace[i]).gps_x-(*MapTrace[i-1]).gps_x),2) + pow(((*MapTrace[i]).gps_y-(*MapTrace[i-1]).gps_y),2));
+               (*MapTrace[i]).frenet_s=sum_s;
+            }
+        }
+    }
+}
 /*int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
                 int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
@@ -356,38 +412,323 @@ int iv::decition::Compute00::getMapDelta(std::vector<GPSData> MapTrace){
                 }
 
 }*/
+ //12.27
+
+
+namespace iv {
+namespace decition {
+extern double gplanbrakeacc;
+extern double gplanacc;
+}
+}
+//extern double iv::decition::gplanbrakeacc;
+
+void iv::decition::Compute00::SmoothACC(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata)
+{
+    double acca = iv::decition::gplanacc;
+    int nsize = static_cast<int>(vdoubledata.size());
+    int i;
+    for(i=1;i<(nsize-1);i++)
+    {
+        if((vdoubledata[i+1][4] - vdoubledata[i][4])>1.0)
+        {
+            int j;
+            for(j=i;j<(nsize -1);j++)
+            {
+                double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+                double vpre = vdoubledata[j][4]/3.6;
+                double fspeed = sqrt((fdis + 0.5*vpre*vpre/acca)*acca/0.5) *3.6;
+                if(fspeed>vdoubledata[j+1][4])
+                {
+                    break;
+                }
+                vdoubledata[j+1][4] = fspeed;
+            }
+            i = j+1;
+        }
+    }
+
+}
+
+
+
+void iv::decition::Compute00::SmoothHighData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
+{
+    if(i==0)return;
+
+//    0.5*v1*v1/a - 0.5*v2*v2/a = dis;
+//    v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
+
+    int j;
+    double brakea = 3.0*iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
+    if(brakea>5.0)brakea = 5.0;
+    int startvalue = static_cast<int>(i-1);
+
+    double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
+    if(fdispoint < 0.1)fdispoint = 0.1;
+    int nprebrake = static_cast<int>(10.0/fdispoint);
+    if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
+    {
+        nprebrake = static_cast<int>(5.0/fdispoint);
+    }
+    else
+    {
+        nprebrake = static_cast<int>(3.0/fdispoint);
+    }
+    for(j=startvalue;j>=0;j--)
+    {
+        if(vdoubledata[j][4] < vdoubledata[j+1][4])
+        {
+            break;
+        }
+        if((startvalue - j) < nprebrake)
+        {
+            vdoubledata[j][4] = vdoubledata[j+1][4];
+            continue;
+        }
+        double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+        double vpre = vdoubledata[j+1][4]/3.6;
+        double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
+        if(fspeed>vdoubledata[j][4])
+        {
+            break;
+        }
+        vdoubledata[j][4] = fspeed;
+    }
+
+}
+
+void iv::decition::Compute00::SmoothData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i)
+{
+    if(i==0)return;
+
+//    0.5*v1*v1/a - 0.5*v2*v2/a = dis;
+//    v1*v1 = (dis + 0.5*v2*v2/a)*a/0.5
+
+    int j;
+    double brakea = iv::decition::gplanbrakeacc;//iv::decition::gplanbrakeacc;
+    int startvalue = static_cast<int>(i-1);
+
+    double fdispoint = sqrt(pow(MapTrace[i]->gps_x - MapTrace[i-1]->gps_x,2)+pow(MapTrace[i]->gps_y - MapTrace[i-1]->gps_y,2));
+    if(fdispoint < 0.1)fdispoint = 0.1;
+    int nprebrake = static_cast<int>(10.0/fdispoint);
+    if((MapTrace[i]->roadMode == 14) || (MapTrace[i]->roadMode == 15))
+    {
+        nprebrake = static_cast<int>(5.0/fdispoint);
+    }
+    else
+    {
+        nprebrake = static_cast<int>(3.0/fdispoint);
+    }
+    for(j=startvalue;j>=0;j--)
+    {
+        if(vdoubledata[j][4] < vdoubledata[j+1][4])
+        {
+            break;
+        }
+        if((startvalue - j) < nprebrake)
+        {
+            vdoubledata[j][4] = vdoubledata[j+1][4];
+            continue;
+        }
+        double fdis = sqrt(pow(MapTrace[j]->gps_x - MapTrace[j+1]->gps_x,2)+pow(MapTrace[j]->gps_y - MapTrace[j+1]->gps_y,2));
+        double vpre = vdoubledata[j+1][4]/3.6;
+        double fspeed = sqrt((fdis + 0.5*vpre*vpre/brakea)*brakea/0.5) *3.6;
+        if(fspeed>vdoubledata[j][4])
+        {
+            break;
+        }
+        vdoubledata[j][4] = fspeed;
+    }
+
+}
 
 int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 {
+    int nSmoothMode = 1;
                 int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
                 double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
                 double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
                 double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
-                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,8.0);
+                double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,15.0); //20221229,8->15
                 getMapDelta(MapTrace);
                 for(int i=0;i<doubledata.size();i++)
                 {
-                    if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
-                        MapTrace[i]->roadMode=5;
-                    }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
-                        MapTrace[i]->roadMode=18;
-                    }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
-                        MapTrace[i]->roadMode=14;
+
+                    if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15))
+                    {
+                       int  a = 1;
+                    }
+                    else
+                    {
+                        if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
+                            MapTrace[i]->roadMode=5;
+                        }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
+                            MapTrace[i]->roadMode=18;
+                        }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
+                            MapTrace[i]->roadMode=14;
+                        }
                     }
                    /* else{
                               MapTrace[i]->roadMode=18;
                         }*/
                 }
+                // pinghua 1.0
+//                for(unsigned int i=1;i<MapTrace.size()-1;i++)
+//                {
+//                    if((MapTrace[i-1]->roadMode==5)&&(MapTrace[i+1]->roadMode==5))
+//                    {
+//                        if(MapTrace[i]->roadMode!=5)
+//                            MapTrace[i]->roadMode=5;
+
+//                    }
+//                    else if ((MapTrace[i-1]->roadMode==18)&&(MapTrace[i+1]->roadMode==18))
+//                    {
+//                        if(MapTrace[i]->roadMode==5)
+//                            MapTrace[i]->roadMode=18;
+//                    }
+//                    else if ((MapTrace[i-1]->roadMode==14)&&(MapTrace[i+1]->roadMode==14))
+//                    {
+//                        if(MapTrace[i]->roadMode==5)
+//                            MapTrace[i]->roadMode=14;
+//                    }
+//                    else
+//                    {
+
+//                    }
+
+//                }
+                for(int i=1;i<MapTrace.size()-1;i++)
+                {
+                 if((MapTrace[i-1]->roadMode==5)&&(MapTrace[i+1]->roadMode==5))
+                 {
+                  if(MapTrace[i]->roadMode!=5)
+                   MapTrace[i]->roadMode=5;
+
+                 }
+                 else if ((MapTrace[i-1]->roadMode==18)&&(MapTrace[i+1]->roadMode==18))
+                 {
+                  if(MapTrace[i]->roadMode==5)
+                   MapTrace[i]->roadMode=18;
+                  if(MapTrace[i]->roadMode==14)
+                   MapTrace[i]->roadMode=18;
+                 }
+                    else if ((MapTrace[i-1]->roadMode==14)&&(MapTrace[i+1]->roadMode==14))
+                 {
+                  if(MapTrace[i]->roadMode==5)
+                   MapTrace[i]->roadMode=14;
+                         if(MapTrace[i]->roadMode==18)
+                   MapTrace[i]->roadMode=14;
+                 }
+                 else
+                 {
+
+                 }
+
+                }
+                //20230106,begin zai zengjiayici lvbo
+//                int cntmode5=0;
+//                //int cntmode5sum=0;
+//                int cntmode18=0;
+//                //int cntmode18sum=0;
+//                int cntmode14=0;
+//                //int cntmode14sum=0;
+//                int lvbo_cnt5=8;
+//                int lvbo_cnt18=6;
+//                int lvbo_cnt14=3;
+//                for(unsigned int i=0;i<MapTrace.size()-10;i++)
+//                {
+//                    if(MapTrace[i]->roadMode==5)
+//                    {
+//                        cntmode14=0;
+//                        cntmode18=0;
+//                        for(unsigned int j=i;j<i+lvbo_cnt5;j++)
+//                        {
+//                            if(MapTrace[j]->roadMode==5)
+//                                cntmode5++;
+//                        }
+//                        if(cntmode5<lvbo_cnt5)
+//                        {
+//                            for(unsigned int j=i;j<i+cntmode5+1;j++)
+//                                MapTrace[j]->roadMode=MapTrace[i-1]->roadMode;
+////                            MapTrace[i+1].roadMode=MapTrace[i-1].roadMode;
+////                            MapTrace[i+2].roadMode=MapTrace[i-1].roadMode;
+//                        }
+//                    }
+//                    else if (MapTrace[i]->roadMode==18)
+//                    {
+//                        cntmode14=0;
+//                        cntmode5=0;
+//                        for(unsigned int j=i;j<i+lvbo_cnt18;j++)
+//                        {
+//                            if(MapTrace[j]->roadMode==18)
+//                                cntmode18++;
+//                        }
+//                        if(cntmode18<lvbo_cnt18)
+//                        {
+//                            for(unsigned int j=i;j<i+cntmode18+1;j++)
+//                            {
+//                                if(MapTrace[i-1]->roadMode!=5)
+//                                     MapTrace[j]->roadMode=MapTrace[i-1]->roadMode;
+//                                else
+//                                    MapTrace[j]->roadMode=14;
+////                            MapTrace[i].roadMode=MapTrace[i-1].roadMode;
+////                            MapTrace[i+1].roadMode=MapTrace[i-1].roadMode;
+////                            MapTrace[i+2].roadMode=MapTrace[i-1].roadMode;
+//                            }
+//                        }
+
+//                    }
+//                    else if (MapTrace[i]->roadMode==14)
+//                    {
+//                        cntmode5=0;
+//                        cntmode18=0;
+//                        for(unsigned int j=i;j<i+lvbo_cnt14;j++)
+//                        {
+//                            if(MapTrace[j]->roadMode==14)
+//                                cntmode14++;
+//                        }
+//                        if(cntmode14<lvbo_cnt14)
+//                        {
+//                            for(unsigned int j=i;j<i+cntmode14+1;j++)
+//                            {
+//                                if(MapTrace[i-1]->roadMode!=5)
+//                                     MapTrace[j]->roadMode=MapTrace[i-1]->roadMode;
+//                                else
+//                                    MapTrace[j]->roadMode=18;
+//                            }
+////                            MapTrace[i].roadMode=MapTrace[i-1].roadMode;
+////                            MapTrace[i+1].roadMode=MapTrace[i-1].roadMode;
+////                            MapTrace[i+2].roadMode=MapTrace[i-1].roadMode;
+//                        }
+
+//                    }
+//                }
+
+                //20230106,end
+    if(nSmoothMode == 0)
+    {
+
                 for(int i=0;i<MapTrace.size();i++)
                 {
                     if(MapTrace[i]->roadMode==0){
                         doubledata[i][4]=straightSpeed;
+                        if(MapTrace[i]->speed>0)
+                        {
+     //                       double fSpeed = MapTrace[i]->speed;
+                            doubledata[i][4] = MapTrace[i]->speed*3.6;
+                        }
                         mode0to5count++;
                         //mode0to18count++;
                         mode18count=0;
                         mode0to5flash=mode0to5count;
                     }else if(MapTrace[i]->roadMode==5){
                         doubledata[i][4]=straightCurveSpeed;
+                        if(MapTrace[i]->speed>0)
+                        {
+     //                       double fSpeed = MapTrace[i]->speed;
+                            doubledata[i][4] = MapTrace[i]->speed*3.6;
+                        }
                         mode0to5count++;
                         //mode0to18count++;
                         mode18count=0;
@@ -396,7 +737,7 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                         mode0to5countSum=mode0to5count;
                         doubledata[i][4]=Curve_SmallSpeed;
 
-                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
+                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*5/0.6; //1228
                         int brake_distance=(int)brake_distance_double;
                         int step_count=0;
                         if((i>brake_distance)&&(mode0to5countSum>brake_distance))
@@ -405,11 +746,13 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
 
                             for(int j=i;j>i-brake_distance;j--){
                                 doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+
                                 step_count++;
                             }
                         }else if(mode0to5countSum>0){
+                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;//1228
                             for(int j=i;j>=i-mode0to5countSum;j--){
-                                doubledata[j][4]=Curve_SmallSpeed;
+                                doubledata[j][4]=Curve_SmallSpeed;//min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_SmallSpeed;
                                 step_count++;
                             }
                         }else{
@@ -419,11 +762,11 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                         //mode0to18count++;
                         mode18count++;
                         mode18flash=mode18count;
-                    }else if(MapTrace[i]->roadMode==14){
+                    }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
                         mode0to18countSum=mode0to5flash+mode18flash;
                         mode18countSum=mode18count;
-                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
-                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/0.6; //1228
+                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*5/0.6;
                         int brake_distanceLarge=(int)brake_distanceLarge_double;
                         int brake_distance0to18=(int)brake_distance0to18_double;
                         int step_count=0;
@@ -443,12 +786,15 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                                 double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
 
                                 for(int j=i;j>i-brake_distance0to18;j--){
+                                    if(MapTrace[j]->roadMode==18)//20230106,fangzhi 18moshi de sudu dayu yuzhi
+                                        continue;
                                     doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
                                     step_count++;
                                 }
                         }else if(mode0to18countSum>0){
+                            double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;//1228
                             for(int j=i;j>=i-mode0to18countSum;j--){
-                                doubledata[j][4]=Curve_LargeSpeed;
+                                doubledata[j][4]=Curve_LargeSpeed;//min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);//1228Curve_LargeSpeed;
                                 step_count++;
                             }
                         }else{
@@ -462,19 +808,311 @@ int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
                     }
                 }
 
-                /*for(int i=0;i<MapTrace.size();i++){
-                    //将数据写入文件开始
-                    ofstream outfile;
-                    outfile.open("ctr0108003.txt", ostream::app);
-                    outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
-                           <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<"num,curve"<< ","  <<i<< ","
-                           <<MapTrace[i]->mfCurvature<<endl;
-                    outfile.close();
-                    //将数据写入文件结束
-                }*/
+
+    }
+    else
+    {
+        for(int i=0;i<MapTrace.size();i++)
+        {
+            if(MapTrace[i]->roadMode==0){
+                doubledata[i][4]=straightSpeed;
+                if(MapTrace[i]->speed>0)
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+
+                }
+
+
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
+                    }
+                }
+
+            }else if(MapTrace[i]->roadMode==5){
+                doubledata[i][4]=straightCurveSpeed;
+                if(MapTrace[i]->speed>0)
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+
+                }
+
+
+                if(i>0)
+                {
+                    if((doubledata[i-1][4] - doubledata[i][4]) > 5.0)
+                    {
+                        SmoothHighData(MapTrace,doubledata,i);
+                    }
+                }
+
+            }else if(MapTrace[i]->roadMode==18){
+
+                if(fabs(MapTrace[i]->mfCurvature)>0.06)
+                {
+                    doubledata[i][4]=(Curve_SmallSpeed + Curve_LargeSpeed)/2.0;
+                }
+                else
+                    doubledata[i][4]=Curve_SmallSpeed;
+
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
+                    }
+                }
+
+
+            }else if((MapTrace[i]->roadMode==14)||(MapTrace[i]->roadMode==15)){
+
+                doubledata[i][4]=Curve_LargeSpeed;
+                if((MapTrace[i]->speed>0) &&(doubledata[i][4]>(MapTrace[i]->speed*3.6)))
+                {
+//                       double fSpeed = MapTrace[i]->speed;
+                    doubledata[i][4] = MapTrace[i]->speed*3.6;
+                }
+                if(i>0)
+                {
+                    if(MapTrace[i-1]->roadMode != MapTrace[i]->roadMode)
+                    {
+                        SmoothData(MapTrace,doubledata,i);
+                    }
+                }
+
+            }
+        }
+
+        SmoothACC(MapTrace,doubledata);
+
+
+
+
+    }
+
+
+
+                //}
+
+//   for(int i=0;i<MapTrace.size();i++){
+//       //将数据写入文件开始
+//       ofstream outfile;
+//       outfile.open("ctr0108003.txt", ostream::app);
+//       outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+//              <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<"num,curve"<< ","  <<i<< ","
+//              <<MapTrace[i]->mfCurvature<<","<<"frenet_s"<< "," <<MapTrace[i]->frenet_s<<endl;
+//       outfile.close();
+//       //将数据写入文件结束
+//   }
 
 }
 
+//int iv::decition::Compute00::getPlanSpeed(std::vector<GPSData> MapTrace)
+//{
+//    int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
+//    double straightSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode0,60.0);
+//    double straightCurveSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode5,40.0);
+//    double Curve_SmallSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode18,15.0);
+//    double Curve_LargeSpeed=min(ServiceCarStatus.mroadmode_vel.mfmode14,12.0);
+//    getMapDelta(MapTrace);
+//    for(int i=0;i<doubledata.size();i++)
+//    {
+//        if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
+//            MapTrace[i]->roadMode=5;
+//        }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
+//            MapTrace[i]->roadMode=18;
+//        }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
+//            MapTrace[i]->roadMode=14;
+//        }
+//    }
+////    //1227，begin
+//   double dis_mode0to5countSum_frenet_s = -1.0;
+//   double mode0_start_frenet_s=-1.0;
+//   double mode0_start_frenet_s_flash=-1.0;
+//   double mode18_start_frenet_s=-1.0;
+//   double mode18countSum_frenet_s=-1.0;
+//   double mode0to18countSum_frenet_s=-1.0;
+
+//   for(int i=0;i<MapTrace.size();i++)
+//   {
+//       if(MapTrace[i]->roadMode==0)
+//       {
+//           doubledata[i][4]=straightSpeed;
+//           if(mode0to5count==0)
+//           {
+//               mode0_start_frenet_s =MapTrace[i]->frenet_s;
+//               mode0_start_frenet_s_flash=mode0_start_frenet_s;
+//           }
+//           mode0to5count++;
+//           //mode0to18count++;
+//           mode18count=0;
+//           mode0to5flash=mode0to5count;
+//       }
+//       else if(MapTrace[i]->roadMode==5)
+//       {
+//           doubledata[i][4]=straightCurveSpeed;
+//           if(mode0to5count==0)
+//           {
+//               mode0_start_frenet_s =MapTrace[i]->frenet_s;
+//               mode0_start_frenet_s_flash=mode0_start_frenet_s;
+//           }
+//           mode0to5count++;
+//           //mode0to18count++;
+//           mode18count=0;
+//           mode0to5flash=mode0to5count;
+//       }
+//       else if(MapTrace[i]->roadMode==18)
+//       {
+//           mode0to5countSum=mode0to5count;
+//           doubledata[i][4]=Curve_SmallSpeed;
+//           if(mode18count==0)
+//           {
+//               mode18_start_frenet_s=MapTrace[i]->frenet_s;
+//           }
+
+//           double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*1/0.6;  //frenet 将10改成1
+//           int brake_distance=(int)brake_distance_double;
+//           int step_count=0;
+//           //1227,begin
+
+//            if((mode0to5countSum>0)&&(mode0_start_frenet_s>=0.0))
+//            {
+//               dis_mode0to5countSum_frenet_s=MapTrace[i]->frenet_s-mode0_start_frenet_s;
+//            }
+
+//            if((MapTrace[i]->frenet_s>brake_distance)&&(mode0to5countSum>0)&&(dis_mode0to5countSum_frenet_s>brake_distance))
+//            {
+//               double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+//               for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distance;j--){
+//                   if(MapTrace[j]->roadMode==5)
+//                     doubledata[j][4]=min((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+//                   else if(MapTrace[j]->roadMode==14)
+//                     doubledata[j][4]= min((Curve_SmallSpeed+step_count*step_speed),Curve_LargeSpeed);
+//                   step_count++;
+//               }
+//            }
+//            else if(mode0to5countSum>0)
+//            {
+//                for(int j=i;j>=i-mode0to5countSum;j--){
+//                       double v0 = sqrt(0.6*1*(MapTrace[i]->frenet_s-MapTrace[j]->frenet_s)+pow(Curve_SmallSpeed,2));
+//                       doubledata[j][4]=min(v0,straightCurveSpeed);//Curve_SmallSpeed;
+//                    step_count++;
+//                }
+//            }
+//           else
+//           {
+//               doubledata[i][4]=Curve_SmallSpeed;
+//           }
+//           //1227，end
+
+
+//           mode0to5count=0;
+//           mode0_start_frenet_s=-1.0;
+//           dis_mode0to5countSum_frenet_s=-1.0;
+//           //mode0to18count++;
+//           mode18count++;
+//           mode18flash=mode18count;
+//       }else if(MapTrace[i]->roadMode==14){
+//           mode0to18countSum=mode0to5flash+mode18flash;
+//           mode18countSum=mode18count;
+//           double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*1/0.6;
+//           double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*1/0.6;
+//           int brake_distanceLarge=(int)brake_distanceLarge_double;
+//           int brake_distance0to18=(int)brake_distance0to18_double;
+//           int step_count=0;
+//           doubledata[i][4]=Curve_LargeSpeed;
+//           if((mode18countSum>0)&&(mode18_start_frenet_s>=0.0))
+//           {
+//               mode18countSum_frenet_s = MapTrace[i]->frenet_s - mode18_start_frenet_s;
+//           }
+//           if((mode0to18countSum>0)&&(mode0_start_frenet_s_flash>=0.0))
+//           {
+//              mode0to18countSum_frenet_s = MapTrace[i]->frenet_s-mode0_start_frenet_s_flash;
+//           }
+
+//           if((mode18countSum_frenet_s>brake_distanceLarge)&&(mode18countSum>0)&&(i>=1)&&(MapTrace[i-1]->roadMode==18))
+//           {
+//               double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+//               for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distanceLarge;j--){
+//                   doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+//                   step_count++;
+//               }
+
+//           }
+//           else if(mode0to18countSum_frenet_s>=brake_distance0to18)
+//           {
+
+//                   double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+//                   for(int j=i;MapTrace[j]->frenet_s>MapTrace[i]->frenet_s-brake_distance0to18;j--){
+//                       if(MapTrace[j]->roadMode==5)
+//                         doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+//                       else if(MapTrace[j]->roadMode==18)
+//                         doubledata[j][4]=min((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+//                       step_count++;
+//                   }
+//           }
+//           else if(mode0to18countSum>0)
+//           {
+//               for(int j=i;j>=i-mode0to18countSum;j--){
+//                   double v0 = sqrt(0.6*1*(MapTrace[i]->frenet_s-MapTrace[j]->frenet_s)+pow(Curve_LargeSpeed,2));
+//                   if(MapTrace[j]->roadMode==5)
+//                        doubledata[j][4]=min(v0,straightCurveSpeed);//Curve_SmallSpeed;
+//                   else if(MapTrace[j]->roadMode==18)
+//                        doubledata[j][4]=min(v0,Curve_SmallSpeed);
+//                   else
+//                        doubledata[j][4]=Curve_LargeSpeed;
+//                   step_count++;
+//               }
+//           }
+//           else
+//           {
+//                   doubledata[i][4]=Curve_LargeSpeed;
+//           }
+
+//           mode0to5count=0;
+//           mode0_start_frenet_s=-1.0;
+//           mode0_start_frenet_s_flash=-1.0;
+//           mode18_start_frenet_s=-1.0;
+//           mode18countSum_frenet_s = -1.0;
+//           mode0to18countSum_frenet_s = -1.0;
+//           mode18count=0;
+//           mode0to5flash=0;
+//           mode18flash=0;
+//       }
+//   }
+
+//   //1227,end
+
+//   for(int i=0;i<MapTrace.size();i++){
+//       //将数据写入文件开始
+//       ofstream outfile;
+//       outfile.open("ctr0108003.txt", ostream::app);
+//       outfile<<"Delta"<< ","  <<doubledata[i][3]<< ","<<"roadMode"<< ","  <<MapTrace[i]->roadMode<< ","
+//              <<"plan_speed"<< ","  << doubledata[i][4]<< ","<<"num,curve"<< ","  <<i<< ","
+//              <<MapTrace[i]->mfCurvature<<","<<"frenet_s"<< "," <<MapTrace[i]->frenet_s<<endl;
+//       outfile.close();
+//       //将数据写入文件结束
+//   }
+
+//}
+
+
+
+
+
+
 //首次找点
 
 int iv::decition::Compute00::getFirstNearestPointIndex(GPS_INS rp, std::vector<GPSData> gpsMap, int lastIndex, double mindis, double maxAngle)
@@ -1925,8 +2563,8 @@ double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
     double angle_1 = getQieXianAngle(nowGps,aimGps);
     double x_2 = 0.0, y_2 = 0.0;
     double steering_angle;
-    double l = 2.950;
-    double r =6;
+    double l = 2.845;
+    double r = 4.5;
     double x_o, y_o, x_o_1, y_o_1, x_o_2, y_o_2, x_3, y_3;
     double x_t_n, y_t_n, x_t_f, y_t_f;//近切点和远切点
     double x_t_1, y_t_1, x_t_2, y_t_2;//圆形1的切点
@@ -2078,7 +2716,7 @@ double iv::decition::Compute00::bocheCompute(GPS_INS nowGps, GPS_INS aimGps) {
     //    }
     Point2D ptN = Coordinate_Transfer(nearTpoint.gps_x, nearTpoint.gps_y, nowGps);
     double disA = GetDistance(aimGps,nowGps);
-    if(y_t_n>0 && ptN.y<0 && y_t_f>0.1 && disA<40){
+    if(y_t_n>0 && ptN.y<(-0.5) && y_t_f>0.1 && disA<40){
         return 1;
     }
 
@@ -2277,7 +2915,7 @@ double iv::decition::Compute00::min_rad_zhuanxiang(double *R_M, double *min_rad)
     double L = 2.95;//轴距
     double L_f =1.2 ;//前悬
     double L_r =0.7 ;//后悬
-    double R_min =6.5 ;//最小转弯半径
+    double R_min =7.0 ;//最小转弯半径
     *R_M = fabs(sqrt(R_min*R_min - (L + L_f)*(L + L_f))) - L_k*0.5;//double	R_M  ;//后轴中点的转弯半径
     //rad_1 = atan2(sqrt(R_min*R_min - (R_M - L_k*0.5)*(R_M - L_k*0.5)), R_M - L_k*0.5);
     //rad_2 = atan2(L + L_f, R_M + L_k*0.5);

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_tools/compute_00.h

@@ -28,6 +28,8 @@ namespace iv {
             static   int lastEsrIDAvoid;          //上次毫米波障碍物ID Avoid
             static   int  lastEsrCountAvoid;      //毫米波障碍物累计次数 Avoid
 
+            static  double  planbrakeacc;
+
             static iv::GPS_INS nearTpoint, farTpoint,dTpoint0,dTpoint1,dTpoint2,dTpoint3;
             static double bocheAngle,dBocheAngle;
 
@@ -38,6 +40,11 @@ namespace iv {
             static int getMapDelta(std::vector<GPSData> MapTrace);
             static int getPlanSpeed(std::vector<GPSData> MapTrace);
 
+            static void SmoothACC(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata);
+            static void SmoothData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i);
+
+            static void SmoothHighData(std::vector<iv::GPSData> & MapTrace,std::vector<std::vector<double>> & vdoubledata,unsigned int i);
+
 
             static double getDecideAngle(std::vector<Point2D>  gpsTrace, double realSpeed);
             static double getAveDef(std::vector<Point2D> farTrace);

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_tools/gnss_coordinate_convert.cpp

@@ -1,4 +1,11 @@
 #include <decition/adc_tools/gnss_coordinate_convert.h>
+#include <math/gnss_coordinate_convert.h>
+
+
+#ifdef USE_UTM
+#include <GeographicLib/UTMUPS.hpp>
+#include <iostream>
+#endif
 
 void gps2xy(double J4, double K4, double *x, double *y)
 {
@@ -22,11 +29,22 @@ void gps2xy(double J4, double K4, double *x, double *y)
     *x = 500000 + T4 * V4 * (Y4 + Z4);
 }
 
-
-
 //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
 void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 {
+
+#ifdef USE_UTM
+
+    int zone{};
+    bool northp{};
+    try {
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, *X,*Y);
+      zone = zone;
+    } catch (GeographicLib::GeographicErr& e) {
+  //    throw ForwardProjectionError(e.what());
+        std::cout<<"GeographicLib::UTMUPS::Forward Fail. what: "<<e.what()<<std::endl;
+    }
+#else
     int ProjNo = 0; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double a, f, e2, ee, NN, T, C, A, M, iPI;
@@ -57,13 +75,14 @@ void GaussProjCal(double longitude, double latitude, double *X, double *Y)
     xval = xval + X0; yval = yval + Y0;
     *X = xval;
     *Y = yval;
+#endif
 }
 
-/*
+
 
 //=======================zhaobo0904
 #define PI  3.14159265358979
-void GaussProjCal(double lon, double lat, double *X, double *Y)
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
 {
     // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
     double a = 6378140.0;
@@ -110,12 +129,26 @@ void GaussProjCal(double lon, double lat, double *X, double *Y)
 }
 //==========================================================
 
-*/
+
+
 
 
 //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
 void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
 {
+
+#ifdef USE_UTM
+    int zone = 50;
+    bool isInNorthernHemisphere_ = true;
+    try {
+      GeographicLib::UTMUPS::Reverse(zone, isInNorthernHemisphere_, X,
+                                     Y, *latitude, *longitude);
+    } catch (GeographicLib::GeographicErr& e) {
+      std::cout<<"GeographicLib::UTMUPS::Reverse what:  "<<e.what()<<std::endl;
+    }
+
+#else
+
     int ProjNo; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
@@ -151,4 +184,6 @@ void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
     //转换为度 DD
     *longitude = longitude1 / iPI;
     *latitude = latitude1 / iPI;
+
+#endif
 }

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_tools/obs_predict.cpp

@@ -29,6 +29,9 @@ namespace iv {
 namespace decition {
 iv::decition::BrainDecition * gbrain;
 
+double gplanbrakeacc = 0.3;
+double gplanacc = 1.0;
+
 
         void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
@@ -39,11 +42,12 @@ iv::decition::BrainDecition * gbrain;
         void ListenVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
             gbrain->UpdateVbox(strdata,nSize);
-            gbrain->UpdateSate();
+   //         gbrain->UpdateSate();
         }
 
         void ListenHMI(const char * strdata,const unsigned int nSize ,const unsigned int index,const QDateTime * dt,const char * strmemname)
         {
+            std::cout<<"receive hmi."<<std::endl;
             gbrain->UpdateHMI(strdata,nSize);
         }
 
@@ -99,12 +103,14 @@ iv::decition::BrainDecition::BrainDecition()
     //    controller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());
 
     //    carcall = new iv::carcalling::carcalling();
-    eyes = new iv::perception::Eyes();
+
     //	decitionMaker = new iv::decition::DecitionMaker();
 
     decitionGps00 = new iv::decition::DecideGps00();
     decitionLine00=new iv::decition::DecideLine00();
 
+    eyes = new iv::perception::Eyes();
+
     //	decitionGpsUnit = new iv::decition::DecideGpsUnit();
 
     //	decitionExecuter = new iv::decition::DecitionExecuter(controller);
@@ -114,7 +120,9 @@ iv::decition::BrainDecition::BrainDecition()
 
     mpa = iv::modulecomm::RegisterRecv("tracemap",iv::decition::ListenTraceMap);
 
-    mpvbox = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);
+//    mpvbox = iv::modulecomm::RegisterRecv("vbox",iv::decition::ListenVbox);
+
+    mpvbox = iv::modulecomm::RegisterRecv("vboxLight",iv::decition::ListenVbox);
 
     mpaVechicleState = iv::modulecomm::RegisterSend(gstrmembrainstate.data(),10000,10);
     mpaCarstate= iv::modulecomm::RegisterSend(gstrmembraincarstate.data(),10000,10);
@@ -419,6 +427,19 @@ void iv::decition::BrainDecition::run() {
     std::string groupID = xp.GetParam("groupid","1");
     ServiceCarStatus.curID = atoi(groupID.data());
 
+
+    gplanbrakeacc = fabs(xp.GetParam("planbrakeacc",0.3));
+    if(gplanbrakeacc <= 0.01)gplanbrakeacc = 0.01;
+
+    gplanacc = fabs(xp.GetParam("planacc",1.0));
+    if(gplanacc <0.1)gplanacc = 0.1;
+
+
+    decitionGps00->mstopbrake = fabs(xp.GetParam("stopbrake",0.6));
+    if(decitionGps00->mstopbrake < 0.1)decitionGps00->mstopbrake = 0.1;
+
+
+
      adjuseGpsLidarPosition();
 
     if(strexternmpc == "true")
@@ -503,7 +524,27 @@ void iv::decition::BrainDecition::run() {
             ServiceCarStatus.mbRunPause = true;
             ServiceCarStatus.mnBocheComplete--;
         }
-        if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
+        if((ServiceCarStatus.mbRunPause))
+        {
+            iv::brain::brainstate xbs;
+            xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
+            xbs.set_mbbrainrunning(false);
+            xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
+            xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
+            xbs.set_mfobs(ServiceCarStatus.mObs);
+            xbs.set_decision_period(decision_period);
+            ShareBrainstate(xbs);
+#ifdef Q_OS_LINUX
+            usleep(10000);
+#endif
+#ifdef Q_OS_WIN32
+            boost::this_thread::sleep(boost::posix_time::milliseconds(10));
+#endif
+             std::cout<<"enter mbRunPause"<<std::endl;
+            continue;
+        }
+ //       if((ServiceCarStatus.mbRunPause)||(navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
+        if((navigation_data.size()<1)||(ServiceCarStatus.mnBocheComplete>0))
         {
             ServiceCarStatus.mbBrainCtring = false;
             decition_gps->brake = 6;
@@ -512,14 +553,14 @@ void iv::decition::BrainDecition::run() {
             decition_gps->torque = 0;
             decition_gps->mode = 0;
             //decitionExecuter->executeDecition(decition_gps);
-                   ShareDecition(decition_gps);  //apollo_fu 20200413 qudiaozhushi
+            ShareDecition(decition_gps);  //apollo_fu 20200413 qudiaozhushi
             ServiceCarStatus.mfAcc = decition_gps->accelerator;
             ServiceCarStatus.mfWheel = decition_gps->wheel_angle;
             ServiceCarStatus.mfBrake = decition_gps->brake;
 
             iv::brain::brainstate xbs;
             xbs.set_mbbocheenable(ServiceCarStatus.bocheEnable);
-            xbs.set_mbbrainrunning(false);
+            xbs.set_mbbrainrunning(true);
             xbs.set_mflidarobs(ServiceCarStatus.mLidarObs);
             xbs.set_mfradarobs(ServiceCarStatus.mRadarObs);
             xbs.set_mfobs(ServiceCarStatus.mObs);
@@ -541,7 +582,7 @@ void iv::decition::BrainDecition::run() {
 #ifdef Q_OS_WIN32
             boost::this_thread::sleep(boost::posix_time::milliseconds(10));
 #endif
-             std::cout<<"enter mbRunPause"<<std::endl;
+             std::cout<<"enter no map or boche  completeeeeeeee"<<std::endl;
             continue;
 
         }
@@ -806,6 +847,17 @@ void iv::decition::BrainDecition::run() {
 
 
 
+            if(mnv2xvalidnum <0)
+            {
+                trafficLight.straightColor = 0;
+                trafficLight.leftColor = 0;
+                trafficLight.rightColor = 0;
+                trafficLight.uturnColor = 0;
+            }
+            else
+            {
+                mnv2xvalidnum--;
+            }
 
 
             mMutexMap.lock();
@@ -1243,6 +1295,46 @@ void iv::decition::BrainDecition::UpdateMap(const char *mapdata, const int mapda
 
         mmpcapi.SetMAP(xvectorMAP);
 
+        if(nMapSize>0)
+        {
+            bool bNeedBoche = false;
+            double x_last,y_last;
+            double x_boche,y_boche;
+            GaussProjCal(navigation_data.at(nMapSize -1)->gps_lng,navigation_data.at(nMapSize -1)->gps_lat,
+                         &x_last,&y_last);
+            GaussProjCal(ServiceCarStatus.mfParkLng,ServiceCarStatus.mfParkLat,
+                         &x_boche,&y_boche);
+
+            if((sqrt(pow(x_boche - x_last,2)+pow(y_boche - y_last,2))<30)&&(ServiceCarStatus.mnParkType == 0))
+            {
+                bNeedBoche = true;
+            }
+
+            if(bNeedBoche)
+            {
+                for(i=(nMapSize -1);i>=0;i--)
+                {
+                    if(i<(nMapSize-500))break;
+                    double fMove = 0;
+                    fMove =  navigation_data.at(i)->mfDisToLaneLeft - navigation_data.at(i)->mfLaneWidth/2.0;
+                    if(fMove < 0.01)break;
+                    double fHdg = navigation_data.at(i)->ins_heading_angle;
+                    fHdg = (90 - fHdg)*M_PI/180.0;
+                    double x_old,y_old;
+                    double x_new,y_new;
+                    GaussProjCal(navigation_data.at(i)->gps_lng,navigation_data.at(i)->gps_lat,
+                                 &x_old,&y_old);
+                    x_new = x_old + fMove*cos(fHdg + M_PI/2.0);
+                    y_new = y_old + fMove*sin(fHdg + M_PI/2.0);
+                    GaussProjInvCal(x_new,y_new,&(navigation_data.at(i)->gps_lng),&(navigation_data.at(i)->gps_lat));
+                    navigation_data.at(i)->gps_x = x_new;
+                    navigation_data.at(i)->gps_y = y_new;
+                    navigation_data.at(i)->mfDisToLaneLeft = navigation_data.at(i)->mfDisToLaneLeft - fMove;
+                    navigation_data.at(i)->mfDisToRoadLeft = navigation_data.at(i)->mfDisToRoadLeft - fMove;
+                }
+            }
+        }
+
 //        if(ServiceCarStatus.speed_control==true){
 //        Compute00().getDesiredSpeed(navigation_data);
 //            Compute00().getPlanSpeed(navigation_data);
@@ -1605,7 +1697,7 @@ void iv::decition::BrainDecition::UpdateGroupInfo(const char *pdata, const int n
 
 void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatasize){
 
-    iv::vbox::vbox group_message;
+    iv::vbox::vbox_light group_message;
     if(false == group_message.ParseFromArray(pdata,ndatasize))
     {
         std::cout<<"Listencansend Parse fail."<<std::endl;
@@ -1618,15 +1710,16 @@ void iv::decition::BrainDecition::UpdateVbox(const char *pdata, const int ndatas
 //    ServiceCarStatus.group_state= group_message.cmd_mode();
 
 
-    trafficLight.leftColor=group_message.st_left();
-    trafficLight.rightColor=group_message.st_right();
+    if(group_message.has_st_left()) trafficLight.leftColor=group_message.st_left();
+    if(group_message.has_st_right())trafficLight.rightColor=group_message.st_right();
     trafficLight.straightColor=group_message.st_straight();
-    trafficLight.uturnColor=group_message.st_turn();
-    trafficLight.leftTime=group_message.time_left();
-    trafficLight.rightTime=group_message.time_right();
+    if(group_message.has_st_turn())trafficLight.uturnColor=group_message.st_turn();
+    if(group_message.has_time_left())trafficLight.leftTime=group_message.time_left();
+    if(group_message.has_time_right())trafficLight.rightTime=group_message.time_right();
     trafficLight.straightTime=group_message.time_straight();
-    trafficLight.uturnTime=group_message.time_turn();
+    if(group_message.has_time_turn())trafficLight.uturnTime=group_message.time_turn();
 
+    mnv2xvalidnum = 500;
 
 
 }

src/decition/decition_brain_sf_Jeely_xingyueL/decition/brain.h

@@ -122,6 +122,9 @@ namespace iv {
             int miss_lidar_per_limit=20;
 
 
+            int mnv2xvalidnum = -1;  //if > 0 valid
+
+
 
             int lastMode;
             bool lastPause;

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.cpp

@@ -28,6 +28,8 @@ extern iv::Ivlog * givlog;
 #define AVOID_NEW 1
 iv::decition::DecideGps00::DecideGps00() {
 
+    std::cout<<" init gps00"<<std::endl;
+
 }
 iv::decition::DecideGps00::~DecideGps00() {
 
@@ -286,6 +288,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
+
     //如果useFrenet、useOldAvoid两者不互为相异，则采用原来的方法。“^”为异或运算符。
     if(!(useFrenet^useOldAvoid)){
         useFrenet = false;
@@ -312,6 +315,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     static int file_num;
     if (isFirstRun)
     {
+//        std::cout<<"InitM  Init M   ........................"<<std::endl;
         file_num=0;
         initMethods();
         vehState = normalRun;
@@ -362,6 +366,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     ServiceCarStatus.mavoidX=avoidXNew;
 
 
+
+    if(front_car_decide_avoid == false)
+    {
+        obstacle_avoid_flag =  0;
+    }
+
+
     if(ServiceCarStatus.msysparam.gpsOffset_y!=0 || ServiceCarStatus.msysparam.gpsOffset_x!=0){
         GPS_INS gpsOffset = Coordinate_UnTransfer(ServiceCarStatus.msysparam.gpsOffset_x, ServiceCarStatus.msysparam.gpsOffset_y, now_gps_ins);
         now_gps_ins.gps_x=gpsOffset.gps_x;
@@ -546,7 +557,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == reversing)
     {
         double dis = GetDistance(now_gps_ins, Compute00().nearTpoint);
-        if (dis<2.0)//0.5
+
+        iv::Point2D pt = iv::decition::Coordinate_Transfer(now_gps_ins.gps_x,now_gps_ins.gps_y,aim_gps_ins);
+        iv::Point2D ptnear = iv::decition::Coordinate_Transfer(Compute00().nearTpoint.gps_x,Compute00().nearTpoint.gps_y,aim_gps_ins);
+
+        double fdistonear = fabs(pt.x - ptnear.x);
+        if(fdistonear<0.5)
+ //       if (dis<2.0)//0.5
         {
             vehState = reverseCircle;
             qiedianCount = true;
@@ -603,7 +620,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             }
 
             controlAng = Compute00().bocheAngle*16.5;
-            gps_decition->wheel_angle = 0 - controlAng*1.05;
+            gps_decition->wheel_angle = 0 - controlAng*0.95;
 
             cout<<"farTpointDistance================"<<dis<<endl;
 
@@ -1042,23 +1059,38 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     double acc_end = 0.1;
     static double distoend=1000;
-    if((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))
+    int useaccend = 1;
+    if(((ServiceCarStatus.table_look_up_on==true)&&(hapo_Adapter->IsINterpolationOK()))||(useaccend == 1))
     {
                 if(PathPoint+1000>gpsMapLine.size()){
                     distoend=computeDistToEnd(gpsMapLine,PathPoint);
                 }else{
                     distoend=1000;
                 }
-                if(!circleMode && distoend<50){
+                if(!circleMode && distoend<100){
                         double nowspeed = realspeed/3.6;
-                        if((distoend<10)||(distoend<(nowspeed*nowspeed)+2))
+                        double brakedis = 100;
+                        double stopbrake = mstopbrake;
+                        if(nowspeed>10)
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*2.0);
+                        }
+                        else
+                        {
+                            brakedis = (nowspeed*nowspeed)/(2*stopbrake)+3;
+                        }
+                        if((distoend<3)||(distoend<brakedis))
                         {
                             if(distoend == 0.0)distoend = 0.09;
                             acc_end = (-1.0)*nowspeed*nowspeed/(2.0*distoend);
-                            if(acc_end<(-3.0))acc_end = -3.0;
+                            if((acc_end<(-3.0))&&(nowspeed<10))acc_end = -3.0;
                         }
 
-                        if((distoend < 2.1)||(PathPoint>=gpsMapLine.size()-2))acc_end = -0.5;
+                        if((distoend < 0.5)||(PathPoint>=gpsMapLine.size()-2))acc_end = (stopbrake + 0.1)*(-1.0);
+
+
+                        if(acc_end<0)minDecelerate = acc_end;
+
                 }
     }else{
 //                if(!circleMode && PathPoint>gpsMapLine.size()-brake_distance){
@@ -1668,20 +1700,22 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //group map end park pro--------start
     if(front_car_id>0){
         static  bool final_lock=false;
-        if((distoend<90)&&(front_car.front_car_dis<obsDistance+10)){
-            if((obsDistance>3)&&(obsDistance<20)){
+        if((distoend<50)&&(front_car.front_car_dis<obsDistance+10)){
+            if((obsDistance>10)&&(obsDistance<20)){
                 if((realspeed>3)&&(final_lock==false)){
-                    minDecelerate=-0.7;
+     //               minDecelerate=-0.7;
+                     minDecelerate= (-1.0) * pow(realspeed/3.6,2)/(2.0* (obsDistance - 8.0));
                 }else{
-                    dSpeed = min(3.0,dSpeed);
+   //                 dSpeed = min(3.0,dSpeed);
+                    minDecelerate=-0.7;
                     final_lock=true;
                 }
                 obsDistance=200;
-            }else if((obsDistance>1)&&(obsDistance<3)){
-                minDecelerate=-0.5;
-                obsDistance=200;
-            }else if(obsDistance<1){
+            }else if((obsDistance>5)&&(obsDistance<=10)){
                 minDecelerate=-1.0;
+   //             obsDistance=200;
+            }else if(obsDistance<=5){
+                minDecelerate=-3.0;
             }
             if(realspeed<1){
                 final_lock=false;
@@ -2450,10 +2484,24 @@ if (vehState == preAvoid)
         obsDistance=500;  //chongxin guihua zhangaiwu juli 0  ,jisuan  acc=-0.5,20220823
     }
 
+    if(fabs(obsDistance)<0.001)
+    {
+        std::cout<<" obs dis = 0."<<std::endl;
+    }
+
     phaseSpeedDecition(gps_decition, secSpeed, obsDistance, obsSpeed,now_gps_ins);
 
     Point2D now_s_d=iv::decition::cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
 
+
+    if(PathPoint+100>gpsMapLine.size())
+    {
+        if(distoend<1.0)
+        {
+            gps_decition->wheel_angle = 0;
+        }
+    }
+
     givlog->debug("decition_brain","vehState: %d,PathPoint: %d,dSpeed: %f,obsDistance: %f,obsSpeed: %f,realspeed: %f,minDecelerate: %f,torque: %f,brake: %f,wheel_angle: %f,obs_speed_for_avoid: %f,mode2: %d,now_s: %f,now_d: %f",
             vehState,PathPoint,dSpeed,obsDistance,obsSpeed,realspeed,minDecelerate,gps_decition->torque,gps_decition->brake,gps_decition->wheel_angle,obs_speed_for_avoid,gpsMapLine[PathPoint]->mode2,now_s_d.s,now_s_d.d);
 
@@ -4451,14 +4499,25 @@ float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight tr
     }
 
 
+    double fTrafficBrake = 1.0;
     passThrough=computeTrafficPass(nearTrafficDis,traffic_color,traffic_time,secSpeed,dSecSpeed);
     if(passThrough){
         return trafficSpeed;
     }else{
         trafficSpeed=computeTrafficSpeedLimt(nearTrafficDis);
-        if(nearTrafficDis<6){
-            float decelerate =0-( secSpeed*secSpeed*0.5/nearTrafficDis);
-            minDecelerate=min(minDecelerate,decelerate);
+        if(nearTrafficDis<(dSecSpeed*dSecSpeed/(2*fTrafficBrake))){
+            if(fabs(nearTrafficDis)<1.0)
+            {
+                minDecelerate = -1.0 * fTrafficBrake;
+            }
+            else
+            {
+                if((0.5*secSpeed*secSpeed/fTrafficBrake) > (nearTrafficDis*0.9))
+                {
+                    float decelerate =0-( secSpeed*secSpeed*0.5/(nearTrafficDis-0.5));
+                    minDecelerate=min(minDecelerate,decelerate);
+                }
+            }
         }
         return trafficSpeed;
     }
@@ -4468,13 +4527,26 @@ float iv::decition::DecideGps00::ComputeV2XTrafficLightSpeed(iv::TrafficLight tr
 
 bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficColor,float trafficTime,float realSecSpeed,float dSecSpeed){
     float passTime=0;
+    double fAcc = 0.6;
+    if(dSecSpeed == 0)return true;
     if (trafficColor==2 || trafficColor==3){
         return false;
     }else if(trafficColor==0){
         return true;
     }else{
+
         passTime=trafficDis/dSecSpeed;
         if(passTime+1< trafficTime){
+            double passTime2= trafficTime - 0.1;
+            double disthru = realSecSpeed * passTime2 + 0.5 * fAcc * pow(passTime2,2);
+            if((disthru -1.0)< trafficDis )
+            {
+                if((realSecSpeed<3.0)&&(trafficDis>10))
+                {
+                    return true;
+                }
+                return false;
+            }
             return true;
         }else{
             return false;
@@ -4486,6 +4558,16 @@ bool iv::decition::DecideGps00::computeTrafficPass(float trafficDis,int trafficC
 
 float iv::decition::DecideGps00::computeTrafficSpeedLimt(float trafficDis){
     float limit=200;
+    double fBrake = 1.0;
+    if(trafficDis < 1.0)
+    {
+        limit =0;
+    }
+    else
+    {
+        limit = 3.6*sqrt(2.0* fabs(fBrake) * (trafficDis-1.0) );
+    }
+    return limit;
     if(trafficDis<10){
         limit = 0;
     }else if(trafficDis<15){

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.h

@@ -69,6 +69,8 @@ public:
     static double obsDistanceAvoid;
     static double lastDistanceAvoid;
 
+    bool tem1;
+
     GPS_INS group_ori_gps;
     GPS_INS startAvoid_gps_ins;
     static int finishPointNum;
@@ -91,6 +93,8 @@ public:
 //    static bool startingFlag;//起步标志，在起步时需要进行frenet规划。
     static float minDecelerate;
 
+    double mstopbrake = 0.6;
+
 
     double startTime = 0;
     double firstTime = 0;

src/decition/decition_brain_sf_Jeely_xingyueL/decition_brain_sf_Jeely_xingyueL.pro

@@ -2,7 +2,7 @@ QT -= gui
 
 QT += network dbus xml widgets
 
-CONFIG += c++11 console
+CONFIG += c++11 # console
 CONFIG -= app_bundle
 
 # The following define makes your compiler emit warnings if you use
@@ -13,6 +13,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 QMAKE_LFLAGS += -no-pie
 
+QMAKE_CXXFLAGS +=  -O0
+
 # You can also make your code fail to compile if you use deprecated APIs.
 # In order to do so, uncomment the following line.
 # You can also select to disable deprecated APIs only up to a certain version of Qt.
@@ -100,3 +102,8 @@ HEADERS += \
     ../../include/msgtype/groupmsg.pb.h
 
 
+#DEFINES += USE_UTM
+
+if(contains(DEFINES,USE_UTM)){
+LIBS += -lGeographic
+}

src/decition/decition_brain_sf_midcar_xunluoche/decition/adc_tools/obs_predict.cpp

@@ -237,6 +237,14 @@ void grpcclient::run()
 
                       stub_ = iv::UploadThread::NewStub(channel);
                   }
+                  if(status.error_code() == 14)
+                  {
+                      std::cout<<" Not Available, Create New stub_"<<std::endl;
+                      channel = grpc::CreateCustomChannel(
+                               target_str, grpc::InsecureChannelCredentials(),cargs);
+
+                      stub_ = iv::UploadThread::NewStub(channel);
+                  }
                   std::this_thread::sleep_for(std::chrono::milliseconds(300));
 
                 }

src/driver/driver_gps_hcp2/driver_gps_hcp2.pro

@@ -19,9 +19,7 @@ DEFINES += QT_DEPRECATED_WARNINGS
 
 SOURCES += main.cpp \
     hcp2.cpp \
-    ../../include/msgtype/gps.pb.cc \
-    ../../include/msgtype/gpsimu.pb.cc \
-    ../../include/msgtype/imu.pb.cc
+    ../../common/common/math/gnss_coordinate_convert.cpp
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )
@@ -34,6 +32,20 @@ SOURCES += main.cpp \
 
 HEADERS += \
     hcp2.h \
-    ../../include/msgtype/gps.pb.h \
-    ../../include/msgtype/gpsimu.pb.h \
-    ../../include/msgtype/imu.pb.h
+    ../../common/common/math/gnss_coordinate_convert.h
+
+
+INCLUDEPATH += $$PWD/../../common/common/math
+
+LIBS += -livprotoif
+
+
+
+DEFINES += USE_UTM
+
+if(contains(DEFINES,USE_UTM)){
+LIBS += -lGeographic
+}
+
+
+

src/driver/driver_gps_hcp2/hcp2.cpp

@@ -1,8 +1,130 @@
 #include "hcp2.h"
 
+#include "gnss_coordinate_convert.h"
+
 extern iv::Ivlog * givlog;
 extern iv::Ivfault *gfault;
 
+
+
+#include <math.h>
+
+//定义一些常量
+double x_PI = 3.14159265358979324 * 3000.0 / 180.0;
+double PI = 3.1415926535897932384626;
+double a = 6378245.0;
+double ee = 0.00669342162296594323;
+
+bool out_of_china(double lng, double lat);
+double transformlat(double lng, double lat);
+double transformlng(double lng, double lat);
+ /**
+ * 百度坐标系 (BD-09) 与 火星坐标系 (GCJ-02)的转换
+ * 即 百度 转 谷歌、高德
+ * @param bd_lon
+ * @param bd_lat
+ * @returns {*[]}
+ */
+void bd09togcj02(double bd_lon, double bd_lat,double & gg_lng,double & gg_lat) {
+    double x_pi = 3.14159265358979324 * 3000.0 / 180.0;
+    double x = bd_lon - 0.0065;
+    double y = bd_lat - 0.006;
+    double z = sqrt(x * x + y * y) - 0.00002 * sin(y * x_pi);
+    double theta = atan2(y, x) - 0.000003 * cos(x * x_pi);
+    gg_lng = z * cos(theta);
+    gg_lat = z * sin(theta);
+}
+/**
+* 火星坐标系 (GCJ-02) 与百度坐标系 (BD-09) 的转换
+* 即谷歌、高德 转 百度
+* @param lng
+* @param lat
+* @returns {*[]}
+*/
+void  gcj02tobd09(double lng, double lat,double & bd_lng, double & bd_lat) {
+    double z = sqrt(lng * lng + lat * lat) + 0.00002 * sin(lat * x_PI);
+    double theta = atan2(lat, lng) + 0.000003 * cos(lng * x_PI);
+    bd_lng = z * cos(theta) + 0.0065;
+    bd_lat = z * sin(theta) + 0.006;
+}
+/**
+* WGS84转GCj02
+* @param lng
+* @param lat
+* @returns {*[]}
+*/
+void  wgs84togcj02(double lng, double lat,double & lng_gcj,double & lat_gcj) {
+if (out_of_china(lng, lat)) {
+lng_gcj = lng;
+lat_gcj = lat;
+}
+else {
+double dlat = transformlat(lng - 105.0, lat - 35.0);
+double dlng = transformlng(lng - 105.0, lat - 35.0);
+double radlat = lat / 180.0 * PI;
+double magic = sin(radlat);
+magic = 1 - ee * magic * magic;
+double sqrtmagic = sqrt(magic);
+dlat = (dlat * 180.0) / ((a * (1 - ee)) / (magic * sqrtmagic) * PI);
+dlng = (dlng * 180.0) / (a / sqrtmagic * cos(radlat) * PI);
+double mglat = lat + dlat;
+double mglng = lng + dlng;
+lng_gcj = mglng;
+lat_gcj = mglat;
+
+}
+}
+/**
+* GCJ02 转换为 WGS84
+* @param lng
+* @param lat
+* @returns {*[]}
+*/
+void gcj02towgs84(double lng, double lat,double & lng_wgs,double & lat_wgs) {
+if (out_of_china(lng, lat)) {
+    lng_wgs = lng;
+    lat_wgs = lat;
+    return ;
+}
+else {
+double dlat = transformlat(lng - 105.0, lat - 35.0);
+double dlng = transformlng(lng - 105.0, lat - 35.0);
+double radlat = lat / 180.0 * PI;
+double magic = sin(radlat);
+magic = 1 - ee * magic * magic;
+double sqrtmagic = sqrt(magic);
+dlat = (dlat * 180.0) / ((a * (1 - ee)) / (magic * sqrtmagic) * PI);
+dlng = (dlng * 180.0) / (a / sqrtmagic * cos(radlat) * PI);
+double mglat = lat + dlat;
+double mglng = lng + dlng;
+lng_wgs = lng * 2 - mglng;
+lat_wgs = lat * 2 - mglat;
+}
+}
+double transformlat(double lng, double lat) {
+double ret = -100.0 + 2.0 * lng + 3.0 * lat + 0.2 * lat * lat + 0.1 * lng * lat + 0.2 * sqrt(abs(lng));
+ret += (20.0 * sin(6.0 * lng * PI) + 20.0 * sin(2.0 * lng * PI)) * 2.0 / 3.0;
+ret += (20.0 * sin(lat * PI) + 40.0 * sin(lat / 3.0 * PI)) * 2.0 / 3.0;
+ret += (160.0 * sin(lat / 12.0 * PI) + 320 * sin(lat * PI / 30.0)) * 2.0 / 3.0;
+return ret;
+}
+double transformlng(double lng, double lat) {
+double ret = 300.0 + lng + 2.0 * lat + 0.1 * lng * lng + 0.1 * lng * lat + 0.1 * sqrt(abs(lng));
+ret += (20.0 * sin(6.0 * lng * PI) + 20.0 * sin(2.0 * lng * PI)) * 2.0 / 3.0;
+ret += (20.0 * sin(lng * PI) + 40.0 * sin(lng / 3.0 * PI)) * 2.0 / 3.0;
+ret += (150.0 * sin(lng / 12.0 * PI) + 300.0 * sin(lng / 30.0 * PI)) * 2.0 / 3.0;
+return ret;
+}
+/**
+* 判断是否在国内，不在国内则不做偏移
+* @param lng
+* @param lat
+* @returns {boolean}
+*/
+bool out_of_china(double lng, double lat) {
+return (lng < 72.004 || lng > 137.8347) || ((lat < 0.8293 || lat > 55.8271) || false);
+}
+
 static bool checknmeasen(const char * strsen,const unsigned int nlen)
 {
     if(nlen< 4)return false;
@@ -42,8 +164,15 @@ static bool checknmeasen(const char * strsen,const unsigned int nlen)
 }
 
 hcp2::hcp2(const char * struartdev,const char * strmsggpsimu,const char * strmsggps,
-                         const char * strmsgimu)
+                         const char * strmsgimu,double fmovex,double fmovey,bool bOutGCJ02)
 {
+    mbOutGCJ02 = bOutGCJ02;
+    if(mbOutGCJ02 == true)
+    {
+        std::cout<<" Out GCJ02 Lon Lat. "<<std::endl;
+    }
+    mfmovex = fmovex;
+    mfmovey = fmovey;
     mTime.start();
     mfCalc_acc = 0;
     mfOldVel = 0;
@@ -206,7 +335,7 @@ void hcp2::ins550dDecode()
     data2 = CalcValue(pdata,ins550DecodeType::i16,48,1.0);
     data3 = CalcValue(pdata,ins550DecodeType::i16,50,1.0);
  //   std::cout<<" data1="<<data1<<"  data2="<<data2<<"  data3="<<data3<<std::endl;
-    std::cout<<"type is "<<(int)type<<std::endl;
+//    std::cout<<"type is "<<(int)type<<std::endl;
     switch (type) {
     case 0:
         latstd = pow(2.718281,data1/100.0);
@@ -591,12 +720,28 @@ void hcp2::SerialGPSDecodeSen(QString strsen)
    }
 
    iv::gps::gpsimu gpsimu;
+
+   if((fabs(mfmovex)>0.0000001)||(fabs(mfmovey)>0.0000001))
+   {
+       MoveLonLat(fLon,fLat,fheading,mfmovex,mfmovey);
+   }
+
    gpsimu.set_vd(vu);
    gpsimu.set_ve(ve);
    gpsimu.set_vn(vn);
    gpsimu.set_speed(fVel);
-   gpsimu.set_lat(fLat);
-   gpsimu.set_lon(fLon);
+   if(mbOutGCJ02 == false)
+   {
+    gpsimu.set_lat(fLat);
+    gpsimu.set_lon(fLon);
+   }
+   else
+   {
+       double lon_gcj,lat_gcj;
+       wgs84togcj02(fLon,fLat,lon_gcj,lat_gcj);
+       gpsimu.set_lat(lat_gcj);
+       gpsimu.set_lon(lon_gcj);
+   }
    gpsimu.set_heading(fheading);
    gpsimu.set_state(4);
    gpsimu.set_msgtime(QDateTime::currentMSecsSinceEpoch());
@@ -708,3 +853,15 @@ void hcp2::BroadCastData()
 
     gfault->SetFaultState(1, 0, "ok");
 }
+
+void hcp2::MoveLonLat(double & flon,double & flat,const double fHeading,const double fMoveX,const double fMoveY)
+{
+    double fHdg = (90-fHeading)*M_PI/180.0;
+    double x,y;
+    GaussProjCal(flon,flat,&x,&y);
+    double xnew,ynew;
+    xnew = x+ fMoveY *cos(fHdg+ M_PI/2.0) + fMoveX*cos(fHdg);
+    ynew = y + fMoveY *sin(fHdg + M_PI/2.0) + fMoveX*sin(fHdg);
+    GaussProjInvCal(xnew,ynew,&flon,&flat);
+}
+

src/driver/driver_gps_hcp2/hcp2.h

@@ -35,7 +35,7 @@ class hcp2 : public QThread
     Q_OBJECT
 public:
     hcp2(const char * struartdev,const char * strmsggpsimu,const char * strmsggps,
-                const char * strmsgimu);
+                const char * strmsgimu,double fmovex,double fmovey,bool bOutGCJ02 = false);
 
 private slots:
     void onTimer();
@@ -43,12 +43,15 @@ private slots:
 private:
     void run();
 
+    void MoveLonLat(double & flon,double & flat,const double fHeading,const double fMoveX,const double fMoveY);
+
 private:
     QByteArray mbaGPSBuf;
     bool mbSerialOpen;
     QSerialPort *m_serialPort_GPS;
     int mnNotRecvCount;
     qint64 mnLastOpenTime;
+    bool mbOutGCJ02;
     void ins550dDecode();
 
     QString mstrHCP2;
@@ -68,6 +71,9 @@ private:
 
     int mngpscount;
 
+    double mfmovex = 0;
+    double mfmovey = 0;
+
 private:
 
     double mhdtheading;

src/driver/driver_gps_hcp2/main.cpp

@@ -1,15 +1,21 @@
 #include <QCoreApplication>
 
 #include "ivversion.h"
-
 #include "hcp2.h"
 
 
 iv::Ivlog * givlog = nullptr;
 iv::Ivfault *gfault = nullptr;
 
+#include "gnss_coordinate_convert.h"
+
 int main(int argc, char *argv[])
 {
+
+    double lat = 28.32;
+    double lon = 117.81;
+    double xv,yv;
+    GaussProjCal(lon,lat,&xv,&yv);
     showversion("driver_gps_hcp2");
     QCoreApplication a(argc, argv);
 
@@ -24,11 +30,18 @@ int main(int argc, char *argv[])
     std::cout<<strpath.toStdString()<<std::endl;
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
-    std::string strdevname = xp.GetParam("devname","/dev/ttysWK0");
+    std::string strdevname = xp.GetParam("devname","/dev/ttyUSB0");
     std::string strgpsimumemname = xp.GetParam("msg_gpsimu","hcp2_gpsimu");
     std::string strgpsmemname = xp.GetParam("msg_gps","hcp2_gps");
     std::string strimumemname = xp.GetParam("msg_imu","hcp2_imu");
-    hcp2 x(strdevname.data(),strgpsimumemname.data(),strgpsmemname.data(),strimumemname.data());
+    std::string strmovex = xp.GetParam("movefront","0.0");
+    std::string strmovey = xp.GetParam("moveleft","0");
+    std::string stroutgcj02 = xp.GetParam("outgcj02","false");
+    bool bOutGCJ02 = false;
+    if(stroutgcj02 == "true")bOutGCJ02 = true;
+    double fmovex = atof(strmovex.data());
+    double fmovey = atof(strmovey.data());
+    hcp2 x(strdevname.data(),strgpsimumemname.data(),strgpsmemname.data(),strimumemname.data(),fmovex,fmovey,bOutGCJ02);
     x.start();
     return a.exec();
 }

src/driver/driver_lidar_merge/driver_lidar_merge.pro

@@ -1,51 +1,51 @@
-#pragma once
-
-#ifndef _IV_BOOST_H_
-#define _IV_BOOST_H_
-
-#if defined __GNUC__
-#  pragma GCC system_header 
-#endif
-#ifndef __CUDACC__
-//https://bugreports.qt-project.org/browse/QTBUG-22829
-#ifndef Q_MOC_RUN
-#include <boost/version.hpp>
-#include <boost/numeric/conversion/cast.hpp>
-#include <boost/thread/mutex.hpp>
-#include <boost/thread/condition.hpp>
-#include <boost/thread.hpp>
-#include <boost/thread/thread.hpp>
-#include <boost/filesystem.hpp>
-#include <boost/bind.hpp>
-#include <boost/cstdint.hpp>
-#include <boost/function.hpp>
-#include <boost/tuple/tuple.hpp>
-#include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <boost/mpl/fold.hpp>
-#include <boost/mpl/inherit.hpp>
-#include <boost/mpl/inherit_linearly.hpp>
-#include <boost/mpl/joint_view.hpp>
-#include <boost/mpl/transform.hpp>
-#include <boost/mpl/vector.hpp>
-#include <boost/algorithm/string.hpp>
-#ifndef Q_MOC_RUN
-#include <boost/date_time/posix_time/posix_time.hpp>
-#endif
-#if BOOST_VERSION >= 104700
-#include <boost/chrono.hpp>
-#endif
-#include <boost/tokenizer.hpp>
-#include <boost/foreach.hpp>
-#include <boost/shared_array.hpp>
-#include <boost/interprocess/sync/file_lock.hpp>
-#if BOOST_VERSION >= 104900
-#include <boost/interprocess/permissions.hpp>
-#endif
-#include <boost/iostreams/device/mapped_file.hpp>
-#define BOOST_PARAMETER_MAX_ARITY 7
-#include <boost/signals2.hpp>
-#include <boost/signals2/slot.hpp>
-#endif
-#endif
-#endif    // _IV_BOOST_H_
+#pragma once
+
+#ifndef _IV_BOOST_H_
+#define _IV_BOOST_H_
+
+#if defined __GNUC__
+#  pragma GCC system_header 
+#endif
+#ifndef __CUDACC__
+//https://bugreports.qt-project.org/browse/QTBUG-22829
+#ifndef Q_MOC_RUN
+#include <boost/version.hpp>
+#include <boost/numeric/conversion/cast.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
+#include <boost/thread.hpp>
+#include <boost/thread/thread.hpp>
+#include <boost/filesystem.hpp>
+#include <boost/bind.hpp>
+#include <boost/cstdint.hpp>
+#include <boost/function.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
+#include <boost/mpl/fold.hpp>
+#include <boost/mpl/inherit.hpp>
+#include <boost/mpl/inherit_linearly.hpp>
+#include <boost/mpl/joint_view.hpp>
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/vector.hpp>
+#include <boost/algorithm/string.hpp>
+#ifndef Q_MOC_RUN
+#include <boost/date_time/posix_time/posix_time.hpp>
+#endif
+#if BOOST_VERSION >= 104700
+#include <boost/chrono.hpp>
+#endif
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/shared_array.hpp>
+#include <boost/interprocess/sync/file_lock.hpp>
+#if BOOST_VERSION >= 104900
+#include <boost/interprocess/permissions.hpp>
+#endif
+#include <boost/iostreams/device/mapped_file.hpp>
+#define BOOST_PARAMETER_MAX_ARITY 7
+#include <boost/signals2.hpp>
+#include <boost/signals2/slot.hpp>
+#endif
+#endif
+#endif    // _IV_BOOST_H_

src/driver/driver_map_xodrload/const.cpp

@@ -1,255 +1,255 @@
-/*							const.c
- *
- *	Globally declared constants
- *
- *
- *
- * SYNOPSIS:
- *
- * extern double nameofconstant;
- *
- *
- *
- *
- * DESCRIPTION:
- *
- * This file contains a number of mathematical constants and
- * also some needed size parameters of the computer arithmetic.
- * The values are supplied as arrays of hexadecimal integers
- * for IEEE arithmetic; arrays of octal constants for DEC
- * arithmetic; and in a normal decimal scientific notation for
- * other machines.  The particular notation used is determined
- * by a symbol (DEC, IBMPC, or UNK) defined in the include file
- * mconf.h.
- *
- * The default size parameters are as follows.
- *
- * For DEC and UNK modes:
- * MACHEP =  1.38777878078144567553E-17       2**-56
- * MAXLOG =  8.8029691931113054295988E1       log(2**127)
- * MINLOG = -8.872283911167299960540E1        log(2**-128)
- * MAXNUM =  1.701411834604692317316873e38    2**127
- *
- * For IEEE arithmetic (IBMPC):
- * MACHEP =  1.11022302462515654042E-16       2**-53
- * MAXLOG =  7.09782712893383996843E2         log(2**1024)
- * MINLOG = -7.08396418532264106224E2         log(2**-1022)
- * MAXNUM =  1.7976931348623158E308           2**1024
- *
- * The global symbols for mathematical constants are
- * PI     =  3.14159265358979323846           pi
- * PIO2   =  1.57079632679489661923           pi/2
- * PIO4   =  7.85398163397448309616E-1        pi/4
- * SQRT2  =  1.41421356237309504880           sqrt(2)
- * SQRTH  =  7.07106781186547524401E-1        sqrt(2)/2
- * LOG2E  =  1.4426950408889634073599         1/log(2)
- * SQ2OPI =  7.9788456080286535587989E-1      sqrt( 2/pi )
- * LOGE2  =  6.93147180559945309417E-1        log(2)
- * LOGSQ2 =  3.46573590279972654709E-1        log(2)/2
- * THPIO4 =  2.35619449019234492885           3*pi/4
- * TWOOPI =  6.36619772367581343075535E-1     2/pi
- *
- * These lists are subject to change.
- */
-
-/*							const.c */
-
-/*
-Cephes Math Library Release 2.3:  March, 1995
-Copyright 1984, 1995 by Stephen L. Moshier
-*/
-
-#include "mconf.h"
-#include <stdlib.h>
-#include <stdio.h>
-
-#ifdef UNK
-#if 1
-double MACHEP =  1.11022302462515654042E-16;   /* 2**-53 */
-#else
-double MACHEP =  1.38777878078144567553E-17;   /* 2**-56 */
-#endif
-double UFLOWTHRESH =  2.22507385850720138309E-308; /* 2**-1022 */
-#ifdef DENORMAL
-double MAXLOG =  7.09782712893383996732E2;     /* log(MAXNUM) */
-/* double MINLOG = -7.44440071921381262314E2; */     /* log(2**-1074) */
-double MINLOG = -7.451332191019412076235E2;     /* log(2**-1075) */
-#else
-double MAXLOG =  7.08396418532264106224E2;     /* log 2**1022 */
-double MINLOG = -7.08396418532264106224E2;     /* log 2**-1022 */
-#endif
-double MAXNUM =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
-double PI     =  3.14159265358979323846;       /* pi */
-double PIO2   =  1.57079632679489661923;       /* pi/2 */
-double PIO4   =  7.85398163397448309616E-1;    /* pi/4 */
-double SQRT2  =  1.41421356237309504880;       /* sqrt(2) */
-double SQRTH  =  7.07106781186547524401E-1;    /* sqrt(2)/2 */
-double LOG2E  =  1.4426950408889634073599;     /* 1/log(2) */
-double SQ2OPI =  7.9788456080286535587989E-1;  /* sqrt( 2/pi ) */
-double LOGE2  =  6.93147180559945309417E-1;    /* log(2) */
-double LOGSQ2 =  3.46573590279972654709E-1;    /* log(2)/2 */
-double THPIO4 =  2.35619449019234492885;       /* 3*pi/4 */
-double TWOOPI =  6.36619772367581343075535E-1; /* 2/pi */
-#ifdef INFINITIES
-//double INFINITY = 1.0/0.0;  /* 99e999; */
-double INFINITY = atof("infinity");  /* 99e999; */
-#else
-double INFINITY =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
-#endif
-#ifdef NANS
-double NAN =atof("infinity") - atof("infinity");
-#else
-double NAN = 0.0;
-#endif
-#ifdef MINUSZERO
-double NEGZERO = -0.0;
-#else
-double NEGZERO = 0.0;
-#endif
-#endif
-
-#ifdef IBMPC
-			/* 2**-53 =  1.11022302462515654042E-16 */
-unsigned short MACHEP[4] = {0x0000,0x0000,0x0000,0x3ca0};
-unsigned short UFLOWTHRESH[4] = {0x0000,0x0000,0x0000,0x0010};
-#ifdef DENORMAL
-			/* log(MAXNUM) =  7.09782712893383996732224E2 */
-unsigned short MAXLOG[4] = {0x39ef,0xfefa,0x2e42,0x4086};
-			/* log(2**-1074) = - -7.44440071921381262314E2 */
-/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087};*/
-unsigned short MINLOG[4] = {0x3052,0xd52d,0x4910,0xc087};
-#else
-			/* log(2**1022) =   7.08396418532264106224E2 */
-unsigned short MAXLOG[4] = {0xbcd2,0xdd7a,0x232b,0x4086};
-			/* log(2**-1022) = - 7.08396418532264106224E2 */
-unsigned short MINLOG[4] = {0xbcd2,0xdd7a,0x232b,0xc086};
-#endif
-			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
-unsigned short MAXNUM[4] = {0xffff,0xffff,0xffff,0x7fef};
-unsigned short PI[4]     = {0x2d18,0x5444,0x21fb,0x4009};
-unsigned short PIO2[4]   = {0x2d18,0x5444,0x21fb,0x3ff9};
-unsigned short PIO4[4]   = {0x2d18,0x5444,0x21fb,0x3fe9};
-unsigned short SQRT2[4]  = {0x3bcd,0x667f,0xa09e,0x3ff6};
-unsigned short SQRTH[4]  = {0x3bcd,0x667f,0xa09e,0x3fe6};
-unsigned short LOG2E[4]  = {0x82fe,0x652b,0x1547,0x3ff7};
-unsigned short SQ2OPI[4] = {0x3651,0x33d4,0x8845,0x3fe9};
-unsigned short LOGE2[4]  = {0x39ef,0xfefa,0x2e42,0x3fe6};
-unsigned short LOGSQ2[4] = {0x39ef,0xfefa,0x2e42,0x3fd6};
-unsigned short THPIO4[4] = {0x21d2,0x7f33,0xd97c,0x4002};
-unsigned short TWOOPI[4] = {0xc883,0x6dc9,0x5f30,0x3fe4};
-#ifdef INFINITIES
-unsigned short INFINITY[4] = {0x0000,0x0000,0x0000,0x7ff0};
-#else
-unsigned short INFINITY[4] = {0xffff,0xffff,0xffff,0x7fef};
-#endif
-#ifdef NANS
-unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x7ffc};
-#else
-unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#ifdef MINUSZERO
-unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x8000};
-#else
-unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#endif
-
-#ifdef MIEEE
-			/* 2**-53 =  1.11022302462515654042E-16 */
-unsigned short MACHEP[4] = {0x3ca0,0x0000,0x0000,0x0000};
-unsigned short UFLOWTHRESH[4] = {0x0010,0x0000,0x0000,0x0000};
-#ifdef DENORMAL
-			/* log(2**1024) =   7.09782712893383996843E2 */
-unsigned short MAXLOG[4] = {0x4086,0x2e42,0xfefa,0x39ef};
-			/* log(2**-1074) = - -7.44440071921381262314E2 */
-/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */
-unsigned short MINLOG[4] = {0xc087,0x4910,0xd52d,0x3052};
-#else
-			/* log(2**1022) =  7.08396418532264106224E2 */
-unsigned short MAXLOG[4] = {0x4086,0x232b,0xdd7a,0xbcd2};
-			/* log(2**-1022) = - 7.08396418532264106224E2 */
-unsigned short MINLOG[4] = {0xc086,0x232b,0xdd7a,0xbcd2};
-#endif
-			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
-unsigned short MAXNUM[4] = {0x7fef,0xffff,0xffff,0xffff};
-unsigned short PI[4]     = {0x4009,0x21fb,0x5444,0x2d18};
-unsigned short PIO2[4]   = {0x3ff9,0x21fb,0x5444,0x2d18};
-unsigned short PIO4[4]   = {0x3fe9,0x21fb,0x5444,0x2d18};
-unsigned short SQRT2[4]  = {0x3ff6,0xa09e,0x667f,0x3bcd};
-unsigned short SQRTH[4]  = {0x3fe6,0xa09e,0x667f,0x3bcd};
-unsigned short LOG2E[4]  = {0x3ff7,0x1547,0x652b,0x82fe};
-unsigned short SQ2OPI[4] = {0x3fe9,0x8845,0x33d4,0x3651};
-unsigned short LOGE2[4]  = {0x3fe6,0x2e42,0xfefa,0x39ef};
-unsigned short LOGSQ2[4] = {0x3fd6,0x2e42,0xfefa,0x39ef};
-unsigned short THPIO4[4] = {0x4002,0xd97c,0x7f33,0x21d2};
-unsigned short TWOOPI[4] = {0x3fe4,0x5f30,0x6dc9,0xc883};
-#ifdef INFINITIES
-unsigned short INFINITY[4] = {0x7ff0,0x0000,0x0000,0x0000};
-#else
-unsigned short INFINITY[4] = {0x7fef,0xffff,0xffff,0xffff};
-#endif
-#ifdef NANS
-unsigned short NAN[4] = {0x7ff8,0x0000,0x0000,0x0000};
-#else
-unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#ifdef MINUSZERO
-unsigned short NEGZERO[4] = {0x8000,0x0000,0x0000,0x0000};
-#else
-unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
-#endif
-#endif
-
-#ifdef DEC
-			/* 2**-56 =  1.38777878078144567553E-17 */
-unsigned short MACHEP[4] = {0022200,0000000,0000000,0000000};
-unsigned short UFLOWTHRESH[4] = {0x0080,0x0000,0x0000,0x0000};
-			/* log 2**127 = 88.029691931113054295988 */
-unsigned short MAXLOG[4] = {041660,007463,0143742,025733,};
-			/* log 2**-128 = -88.72283911167299960540 */
-unsigned short MINLOG[4] = {0141661,071027,0173721,0147572,};
-			/* 2**127 = 1.701411834604692317316873e38 */
-unsigned short MAXNUM[4] = {077777,0177777,0177777,0177777,};
-unsigned short PI[4]     = {040511,007732,0121041,064302,};
-unsigned short PIO2[4]   = {040311,007732,0121041,064302,};
-unsigned short PIO4[4]   = {040111,007732,0121041,064302,};
-unsigned short SQRT2[4]  = {040265,002363,031771,0157145,};
-unsigned short SQRTH[4]  = {040065,002363,031771,0157144,};
-unsigned short LOG2E[4]  = {040270,0125073,024534,013761,};
-unsigned short SQ2OPI[4] = {040114,041051,0117241,0131204,};
-unsigned short LOGE2[4]  = {040061,071027,0173721,0147572,};
-unsigned short LOGSQ2[4] = {037661,071027,0173721,0147572,};
-unsigned short THPIO4[4] = {040426,0145743,0174631,007222,};
-unsigned short TWOOPI[4] = {040042,0174603,067116,042025,};
-/* Approximate infinity by MAXNUM.  */
-unsigned short INFINITY[4] = {077777,0177777,0177777,0177777,};
-unsigned short NAN[4] = {0000000,0000000,0000000,0000000};
-#ifdef MINUSZERO
-unsigned short NEGZERO[4] = {0000000,0000000,0000000,0100000};
-#else
-unsigned short NEGZERO[4] = {0000000,0000000,0000000,0000000};
-#endif
-#endif
-
-#ifndef UNK
-extern unsigned short MACHEP[];
-extern unsigned short UFLOWTHRESH[];
-extern unsigned short MAXLOG[];
-extern unsigned short UNDLOG[];
-extern unsigned short MINLOG[];
-extern unsigned short MAXNUM[];
-extern unsigned short PI[];
-extern unsigned short PIO2[];
-extern unsigned short PIO4[];
-extern unsigned short SQRT2[];
-extern unsigned short SQRTH[];
-extern unsigned short LOG2E[];
-extern unsigned short SQ2OPI[];
-extern unsigned short LOGE2[];
-extern unsigned short LOGSQ2[];
-extern unsigned short THPIO4[];
-extern unsigned short TWOOPI[];
-extern unsigned short INFINITY[];
-extern unsigned short NAN[];
-extern unsigned short NEGZERO[];
-#endif
+/*							const.c
+ *
+ *	Globally declared constants
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * extern double nameofconstant;
+ *
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains a number of mathematical constants and
+ * also some needed size parameters of the computer arithmetic.
+ * The values are supplied as arrays of hexadecimal integers
+ * for IEEE arithmetic; arrays of octal constants for DEC
+ * arithmetic; and in a normal decimal scientific notation for
+ * other machines.  The particular notation used is determined
+ * by a symbol (DEC, IBMPC, or UNK) defined in the include file
+ * mconf.h.
+ *
+ * The default size parameters are as follows.
+ *
+ * For DEC and UNK modes:
+ * MACHEP =  1.38777878078144567553E-17       2**-56
+ * MAXLOG =  8.8029691931113054295988E1       log(2**127)
+ * MINLOG = -8.872283911167299960540E1        log(2**-128)
+ * MAXNUM =  1.701411834604692317316873e38    2**127
+ *
+ * For IEEE arithmetic (IBMPC):
+ * MACHEP =  1.11022302462515654042E-16       2**-53
+ * MAXLOG =  7.09782712893383996843E2         log(2**1024)
+ * MINLOG = -7.08396418532264106224E2         log(2**-1022)
+ * MAXNUM =  1.7976931348623158E308           2**1024
+ *
+ * The global symbols for mathematical constants are
+ * PI     =  3.14159265358979323846           pi
+ * PIO2   =  1.57079632679489661923           pi/2
+ * PIO4   =  7.85398163397448309616E-1        pi/4
+ * SQRT2  =  1.41421356237309504880           sqrt(2)
+ * SQRTH  =  7.07106781186547524401E-1        sqrt(2)/2
+ * LOG2E  =  1.4426950408889634073599         1/log(2)
+ * SQ2OPI =  7.9788456080286535587989E-1      sqrt( 2/pi )
+ * LOGE2  =  6.93147180559945309417E-1        log(2)
+ * LOGSQ2 =  3.46573590279972654709E-1        log(2)/2
+ * THPIO4 =  2.35619449019234492885           3*pi/4
+ * TWOOPI =  6.36619772367581343075535E-1     2/pi
+ *
+ * These lists are subject to change.
+ */
+
+/*							const.c */
+
+/*
+Cephes Math Library Release 2.3:  March, 1995
+Copyright 1984, 1995 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef UNK
+#if 1
+double MACHEP =  1.11022302462515654042E-16;   /* 2**-53 */
+#else
+double MACHEP =  1.38777878078144567553E-17;   /* 2**-56 */
+#endif
+double UFLOWTHRESH =  2.22507385850720138309E-308; /* 2**-1022 */
+#ifdef DENORMAL
+double MAXLOG =  7.09782712893383996732E2;     /* log(MAXNUM) */
+/* double MINLOG = -7.44440071921381262314E2; */     /* log(2**-1074) */
+double MINLOG = -7.451332191019412076235E2;     /* log(2**-1075) */
+#else
+double MAXLOG =  7.08396418532264106224E2;     /* log 2**1022 */
+double MINLOG = -7.08396418532264106224E2;     /* log 2**-1022 */
+#endif
+double MAXNUM =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+double PI     =  3.14159265358979323846;       /* pi */
+double PIO2   =  1.57079632679489661923;       /* pi/2 */
+double PIO4   =  7.85398163397448309616E-1;    /* pi/4 */
+double SQRT2  =  1.41421356237309504880;       /* sqrt(2) */
+double SQRTH  =  7.07106781186547524401E-1;    /* sqrt(2)/2 */
+double LOG2E  =  1.4426950408889634073599;     /* 1/log(2) */
+double SQ2OPI =  7.9788456080286535587989E-1;  /* sqrt( 2/pi ) */
+double LOGE2  =  6.93147180559945309417E-1;    /* log(2) */
+double LOGSQ2 =  3.46573590279972654709E-1;    /* log(2)/2 */
+double THPIO4 =  2.35619449019234492885;       /* 3*pi/4 */
+double TWOOPI =  6.36619772367581343075535E-1; /* 2/pi */
+#ifdef INFINITIES
+//double INFINITY = 1.0/0.0;  /* 99e999; */
+double INFINITY = atof("infinity");  /* 99e999; */
+#else
+double INFINITY =  1.79769313486231570815E308;    /* 2**1024*(1-MACHEP) */
+#endif
+#ifdef NANS
+double NAN =atof("infinity") - atof("infinity");
+#else
+double NAN = 0.0;
+#endif
+#ifdef MINUSZERO
+double NEGZERO = -0.0;
+#else
+double NEGZERO = 0.0;
+#endif
+#endif
+
+#ifdef IBMPC
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x0000,0x0000,0x0000,0x3ca0};
+unsigned short UFLOWTHRESH[4] = {0x0000,0x0000,0x0000,0x0010};
+#ifdef DENORMAL
+			/* log(MAXNUM) =  7.09782712893383996732224E2 */
+unsigned short MAXLOG[4] = {0x39ef,0xfefa,0x2e42,0x4086};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087};*/
+unsigned short MINLOG[4] = {0x3052,0xd52d,0x4910,0xc087};
+#else
+			/* log(2**1022) =   7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0xbcd2,0xdd7a,0x232b,0x4086};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xbcd2,0xdd7a,0x232b,0xc086};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0xffff,0xffff,0xffff,0x7fef};
+unsigned short PI[4]     = {0x2d18,0x5444,0x21fb,0x4009};
+unsigned short PIO2[4]   = {0x2d18,0x5444,0x21fb,0x3ff9};
+unsigned short PIO4[4]   = {0x2d18,0x5444,0x21fb,0x3fe9};
+unsigned short SQRT2[4]  = {0x3bcd,0x667f,0xa09e,0x3ff6};
+unsigned short SQRTH[4]  = {0x3bcd,0x667f,0xa09e,0x3fe6};
+unsigned short LOG2E[4]  = {0x82fe,0x652b,0x1547,0x3ff7};
+unsigned short SQ2OPI[4] = {0x3651,0x33d4,0x8845,0x3fe9};
+unsigned short LOGE2[4]  = {0x39ef,0xfefa,0x2e42,0x3fe6};
+unsigned short LOGSQ2[4] = {0x39ef,0xfefa,0x2e42,0x3fd6};
+unsigned short THPIO4[4] = {0x21d2,0x7f33,0xd97c,0x4002};
+unsigned short TWOOPI[4] = {0xc883,0x6dc9,0x5f30,0x3fe4};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x0000,0x0000,0x0000,0x7ff0};
+#else
+unsigned short INFINITY[4] = {0xffff,0xffff,0xffff,0x7fef};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x7ffc};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x8000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef MIEEE
+			/* 2**-53 =  1.11022302462515654042E-16 */
+unsigned short MACHEP[4] = {0x3ca0,0x0000,0x0000,0x0000};
+unsigned short UFLOWTHRESH[4] = {0x0010,0x0000,0x0000,0x0000};
+#ifdef DENORMAL
+			/* log(2**1024) =   7.09782712893383996843E2 */
+unsigned short MAXLOG[4] = {0x4086,0x2e42,0xfefa,0x39ef};
+			/* log(2**-1074) = - -7.44440071921381262314E2 */
+/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */
+unsigned short MINLOG[4] = {0xc087,0x4910,0xd52d,0x3052};
+#else
+			/* log(2**1022) =  7.08396418532264106224E2 */
+unsigned short MAXLOG[4] = {0x4086,0x232b,0xdd7a,0xbcd2};
+			/* log(2**-1022) = - 7.08396418532264106224E2 */
+unsigned short MINLOG[4] = {0xc086,0x232b,0xdd7a,0xbcd2};
+#endif
+			/* 2**1024*(1-MACHEP) =  1.7976931348623158E308 */
+unsigned short MAXNUM[4] = {0x7fef,0xffff,0xffff,0xffff};
+unsigned short PI[4]     = {0x4009,0x21fb,0x5444,0x2d18};
+unsigned short PIO2[4]   = {0x3ff9,0x21fb,0x5444,0x2d18};
+unsigned short PIO4[4]   = {0x3fe9,0x21fb,0x5444,0x2d18};
+unsigned short SQRT2[4]  = {0x3ff6,0xa09e,0x667f,0x3bcd};
+unsigned short SQRTH[4]  = {0x3fe6,0xa09e,0x667f,0x3bcd};
+unsigned short LOG2E[4]  = {0x3ff7,0x1547,0x652b,0x82fe};
+unsigned short SQ2OPI[4] = {0x3fe9,0x8845,0x33d4,0x3651};
+unsigned short LOGE2[4]  = {0x3fe6,0x2e42,0xfefa,0x39ef};
+unsigned short LOGSQ2[4] = {0x3fd6,0x2e42,0xfefa,0x39ef};
+unsigned short THPIO4[4] = {0x4002,0xd97c,0x7f33,0x21d2};
+unsigned short TWOOPI[4] = {0x3fe4,0x5f30,0x6dc9,0xc883};
+#ifdef INFINITIES
+unsigned short INFINITY[4] = {0x7ff0,0x0000,0x0000,0x0000};
+#else
+unsigned short INFINITY[4] = {0x7fef,0xffff,0xffff,0xffff};
+#endif
+#ifdef NANS
+unsigned short NAN[4] = {0x7ff8,0x0000,0x0000,0x0000};
+#else
+unsigned short NAN[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0x8000,0x0000,0x0000,0x0000};
+#else
+unsigned short NEGZERO[4] = {0x0000,0x0000,0x0000,0x0000};
+#endif
+#endif
+
+#ifdef DEC
+			/* 2**-56 =  1.38777878078144567553E-17 */
+unsigned short MACHEP[4] = {0022200,0000000,0000000,0000000};
+unsigned short UFLOWTHRESH[4] = {0x0080,0x0000,0x0000,0x0000};
+			/* log 2**127 = 88.029691931113054295988 */
+unsigned short MAXLOG[4] = {041660,007463,0143742,025733,};
+			/* log 2**-128 = -88.72283911167299960540 */
+unsigned short MINLOG[4] = {0141661,071027,0173721,0147572,};
+			/* 2**127 = 1.701411834604692317316873e38 */
+unsigned short MAXNUM[4] = {077777,0177777,0177777,0177777,};
+unsigned short PI[4]     = {040511,007732,0121041,064302,};
+unsigned short PIO2[4]   = {040311,007732,0121041,064302,};
+unsigned short PIO4[4]   = {040111,007732,0121041,064302,};
+unsigned short SQRT2[4]  = {040265,002363,031771,0157145,};
+unsigned short SQRTH[4]  = {040065,002363,031771,0157144,};
+unsigned short LOG2E[4]  = {040270,0125073,024534,013761,};
+unsigned short SQ2OPI[4] = {040114,041051,0117241,0131204,};
+unsigned short LOGE2[4]  = {040061,071027,0173721,0147572,};
+unsigned short LOGSQ2[4] = {037661,071027,0173721,0147572,};
+unsigned short THPIO4[4] = {040426,0145743,0174631,007222,};
+unsigned short TWOOPI[4] = {040042,0174603,067116,042025,};
+/* Approximate infinity by MAXNUM.  */
+unsigned short INFINITY[4] = {077777,0177777,0177777,0177777,};
+unsigned short NAN[4] = {0000000,0000000,0000000,0000000};
+#ifdef MINUSZERO
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0100000};
+#else
+unsigned short NEGZERO[4] = {0000000,0000000,0000000,0000000};
+#endif
+#endif
+
+#ifndef UNK
+extern unsigned short MACHEP[];
+extern unsigned short UFLOWTHRESH[];
+extern unsigned short MAXLOG[];
+extern unsigned short UNDLOG[];
+extern unsigned short MINLOG[];
+extern unsigned short MAXNUM[];
+extern unsigned short PI[];
+extern unsigned short PIO2[];
+extern unsigned short PIO4[];
+extern unsigned short SQRT2[];
+extern unsigned short SQRTH[];
+extern unsigned short LOG2E[];
+extern unsigned short SQ2OPI[];
+extern unsigned short LOGE2[];
+extern unsigned short LOGSQ2[];
+extern unsigned short THPIO4[];
+extern unsigned short TWOOPI[];
+extern unsigned short INFINITY[];
+extern unsigned short NAN[];
+extern unsigned short NEGZERO[];
+#endif

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -1,80 +1,102 @@
-QT -= gui
-
-QT += xml dbus
-
-CONFIG += c++11 console
-CONFIG -= app_bundle
-
-
-# The following define makes your compiler emit warnings if you use
-# any feature of Qt which as been marked deprecated (the exact warnings
-# depend on your compiler). Please consult the documentation of the
-# deprecated API in order to know how to port your code away from it.
-DEFINES += QT_DEPRECATED_WARNINGS
-
-QMAKE_CXXFLAGS +=  -g
-
-# You can also make your code fail to compile if you use deprecated APIs.
-# In order to do so, uncomment the following line.
-# You can also select to disable deprecated APIs only up to a certain version of Qt.
-#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
-
-SOURCES += main.cpp     \
-    fresnl.cpp \
-    const.cpp \
-    polevl.c \
-    globalplan.cpp \
-    dubins.c \
-    gnss_coordinate_convert.cpp \
-    service_roi_xodr.cpp \
-    xodrplan.cpp
-
-HEADERS += \
-    ../../../include/ivexit.h \
-    mconf.h \
-    globalplan.h \
-    gps_type.h \
-    dubins.h \
-    gnss_coordinate_convert.h \
-    planpoint.h \
-    service_roi_xodr.h \
-    xodrplan.h
-
-
-!include(../../../include/common.pri ) {
-    error( "Couldn't find the common.pri file!" )
-}
-
-!include(../../../include/ivprotobuf.pri ) {
-    error( "Couldn't find the ivprotobuf.pri file!" )
-}
-
-!include(../../../include/ivboost.pri ) {
-    error( "Couldn't find the ivboost.pri file!" )
-}
-
-!include(../../../include/iveigen.pri ) {
-    error( "Couldn't find the iveigen.pri file!" )
-}
-
-
-!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
-    error( "Couldn't find the OpenDrive.pri file!" )
-}
-
-!include(../../common/common/xodr/TinyXML/TinyXML.pri ) {
-    error( "Couldn't find the TinyXML.pri file!" )
-}
-
-!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
-    error( "Couldn't find the xodrfunc.pri file!" )
-}
-
-INCLUDEPATH += $$PWD/../../common/common/xodr
-INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
-
-DEFINES += INPILOT
-
-LIBS += -livprotoif
-
-
+QT -= gui
+
+QT += xml dbus
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+QMAKE_CXXFLAGS +=  -g
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp     \
+    fresnl.cpp \
+    const.cpp \
+    polevl.c \
+    globalplan.cpp \
+    dubins.c \
+    gnss_coordinate_convert.cpp \
+    service_roi_xodr.cpp \
+    xodrplan.cpp \
+    ../../include/msgtype/mapglobal.pb.cc \
+    planglobal.cpp
+
+HEADERS += \
+    ../../../include/ivexit.h \
+    mconf.h \
+    globalplan.h \
+    gps_type.h \
+    dubins.h \
+    gnss_coordinate_convert.h \
+    planpoint.h \
+    service_roi_xodr.h \
+    xodrplan.h \
+    ../../include/msgtype/mapglobal.pb.h \
+    planglobal.h
+
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+!include(../../../include/iveigen.pri ) {
+    error( "Couldn't find the iveigen.pri file!" )
+}
+
+
+!include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
+    error( "Couldn't find the OpenDrive.pri file!" )
+}
+
+!include(../../common/common/xodr/TinyXML/TinyXML.pri ) {
+    error( "Couldn't find the TinyXML.pri file!" )
+}
+
+!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
+    error( "Couldn't find the xodrfunc.pri file!" )
+}
+
+INCLUDEPATH += $$PWD/../../common/common/xodr
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
+
+DEFINES += INPILOT
+
+LIBS += -livprotoif
+
+#DEFINES += USE_UTM
+
+if(contains(DEFINES,USE_UTM)){
+LIBS += -lGeographic
+}
+
+
+#DEFINES += USE_TMERS
+if(contains(DEFINES,USE_TMERS)){
+INCLUDEPATH += /home/nvidia/Downloads/proj-8.2.1/src
+LIBS += /home/nvidia/Downloads/proj-8.2.1/build/lib/libproj.so.22
+}
+
+
+#DEFINES += TESTSPEEDPLAN
+
+#LIBS += -lproj
+
+

src/driver/driver_map_xodrload/driver_map_xodrload.xml

@@ -1,6 +1,6 @@
-<xml>	
-	<node name="driver_map_xodrload">
-		<param name="extendmap" value="true" />
-	</node>
-</xml>
-
+<xml>	
+	<node name="driver_map_xodrload">
+		<param name="extendmap" value="true" />
+	</node>
+</xml>
+

src/driver/driver_map_xodrload/dubins.c

@@ -1,439 +1,439 @@
-/*
- * Copyright (c) 2008-2018, Andrew Walker
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-#ifdef WIN32
-#define _USE_MATH_DEFINES
-#endif
-#include <math.h>
-#include "dubins.h"
-
-#define EPSILON (10e-10)
-
-typedef enum 
-{
-    L_SEG = 0,
-    S_SEG = 1,
-    R_SEG = 2
-} SegmentType;
-
-/* The segment types for each of the Path types */
-const SegmentType DIRDATA[][3] = {
-    { L_SEG, S_SEG, L_SEG },
-    { L_SEG, S_SEG, R_SEG },
-    { R_SEG, S_SEG, L_SEG },
-    { R_SEG, S_SEG, R_SEG },
-    { R_SEG, L_SEG, R_SEG },
-    { L_SEG, R_SEG, L_SEG }
-};
-
-typedef struct 
-{
-    double alpha;
-    double beta;
-    double d;
-    double sa;
-    double sb;
-    double ca;
-    double cb;
-    double c_ab;
-    double d_sq;
-} DubinsIntermediateResults;
-
-
-int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
-int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
-
-/**
- * Floating point modulus suitable for rings
- *
- * fmod doesn't behave correctly for angular quantities, this function does
- */
-double fmodr( double x, double y)
-{
-    return x - y*floor(x/y);
-}
-
-double mod2pi( double theta )
-{
-    return fmodr( theta, 2 * M_PI );
-}
-
-int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
-{
-    int i, errcode;
-    DubinsIntermediateResults in;
-    double params[3];
-    double cost;
-    double best_cost = INFINITY;
-    int best_word = -1;
-    errcode = dubins_intermediate_results(&in, q0, q1, rho);
-    if(errcode != EDUBOK) {
-        return errcode;
-    }
-
-
-    path->qi[0] = q0[0];
-    path->qi[1] = q0[1];
-    path->qi[2] = q0[2];
-    path->rho = rho;
- 
-    for( i = 0; i < 6; i++ ) {
-        DubinsPathType pathType = (DubinsPathType)i;
-        errcode = dubins_word(&in, pathType, params);
-        if(errcode == EDUBOK) {
-            cost = params[0] + params[1] + params[2];
-            if(cost < best_cost) {
-                best_word = i;
-                best_cost = cost;
-                path->param[0] = params[0];
-                path->param[1] = params[1];
-                path->param[2] = params[2];
-                path->type = pathType;
-            }
-        }
-    }
-    if(best_word == -1) {
-        return EDUBNOPATH;
-    }
-    return EDUBOK;
-}
-
-int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
-{
-    int errcode;
-    DubinsIntermediateResults in;
-    errcode = dubins_intermediate_results(&in, q0, q1, rho);
-    if(errcode == EDUBOK) {
-        double params[3];
-        errcode = dubins_word(&in, pathType, params);
-        if(errcode == EDUBOK) {
-            path->param[0] = params[0];
-            path->param[1] = params[1];
-            path->param[2] = params[2];
-            path->qi[0] = q0[0];
-            path->qi[1] = q0[1];
-            path->qi[2] = q0[2];
-            path->rho = rho;
-            path->type = pathType;
-        }
-    }
-    return errcode;
-}
-
-double dubins_path_length( DubinsPath* path )
-{
-    double length = 0.;
-    length += path->param[0];
-    length += path->param[1];
-    length += path->param[2];
-    length = length * path->rho;
-    return length;
-}
-
-double dubins_segment_length( DubinsPath* path, int i )
-{
-    if( (i < 0) || (i > 2) )
-    {
-        return INFINITY;
-    }
-    return path->param[i] * path->rho;
-}
-
-double dubins_segment_length_normalized( DubinsPath* path, int i )
-{
-    if( (i < 0) || (i > 2) )
-    {
-        return INFINITY;
-    }
-    return path->param[i];
-} 
-
-DubinsPathType dubins_path_type( DubinsPath* path ) 
-{
-    return path->type;
-}
-
-void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
-{
-    double st = sin(qi[2]);
-    double ct = cos(qi[2]);
-    if( type == L_SEG ) {
-        qt[0] = +sin(qi[2]+t) - st;
-        qt[1] = -cos(qi[2]+t) + ct;
-        qt[2] = t;
-    }
-    else if( type == R_SEG ) {
-        qt[0] = -sin(qi[2]-t) + st;
-        qt[1] = +cos(qi[2]-t) - ct;
-        qt[2] = -t;
-    }
-    else if( type == S_SEG ) {
-        qt[0] = ct * t;
-        qt[1] = st * t;
-        qt[2] = 0.0;
-    }
-    qt[0] += qi[0];
-    qt[1] += qi[1];
-    qt[2] += qi[2];
-}
-
-int dubins_path_sample( DubinsPath* path, double t, double q[3] )
-{
-    /* tprime is the normalised variant of the parameter t */
-    double tprime = t / path->rho;
-    double qi[3]; /* The translated initial configuration */
-    double q1[3]; /* end-of segment 1 */
-    double q2[3]; /* end-of segment 2 */
-    const SegmentType* types = DIRDATA[path->type];
-    double p1, p2;
-
-    if( t < 0 || t > dubins_path_length(path) ) {
-        return EDUBPARAM;
-    }
-
-    /* initial configuration */
-    qi[0] = 0.0;
-    qi[1] = 0.0;
-    qi[2] = path->qi[2];
-
-    /* generate the target configuration */
-    p1 = path->param[0];
-    p2 = path->param[1];
-    dubins_segment( p1,      qi,    q1, types[0] );
-    dubins_segment( p2,      q1,    q2, types[1] );
-    if( tprime < p1 ) {
-        dubins_segment( tprime, qi, q, types[0] );
-    }
-    else if( tprime < (p1+p2) ) {
-        dubins_segment( tprime-p1, q1, q,  types[1] );
-    }
-    else {
-        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
-    }
-
-    /* scale the target configuration, translate back to the original starting point */
-    q[0] = q[0] * path->rho + path->qi[0];
-    q[1] = q[1] * path->rho + path->qi[1];
-    q[2] = mod2pi(q[2]);
-
-    return EDUBOK;
-}
-
-int dubins_path_sample_many(DubinsPath* path, double stepSize, 
-                            DubinsPathSamplingCallback cb, void* user_data)
-{
-    int retcode;
-    double q[3];
-    double x = 0.0;
-    double length = dubins_path_length(path);
-    while( x <  length ) {
-        dubins_path_sample( path, x, q );
-        retcode = cb(q, x, user_data);
-        if( retcode != 0 ) {
-            return retcode;
-        }
-        x += stepSize;
-    }
-    return 0;
-}
-
-int dubins_path_endpoint( DubinsPath* path, double q[3] )
-{
-    return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
-}
-
-int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
-{
-    /* calculate the true parameter */
-    double tprime = t / path->rho;
-
-    if((t < 0) || (t > dubins_path_length(path)))
-    {
-        return EDUBPARAM; 
-    }
-
-    /* copy most of the data */
-    newpath->qi[0] = path->qi[0];
-    newpath->qi[1] = path->qi[1];
-    newpath->qi[2] = path->qi[2];
-    newpath->rho   = path->rho;
-    newpath->type  = path->type;
-
-    /* fix the parameters */
-    newpath->param[0] = fmin( path->param[0], tprime );
-    newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
-    newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
-    return 0;
-}
-
-int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
-{
-    double dx, dy, D, d, theta, alpha, beta;
-    if( rho <= 0.0 ) {
-        return EDUBBADRHO;
-    }
-
-    dx = q1[0] - q0[0];
-    dy = q1[1] - q0[1];
-    D = sqrt( dx * dx + dy * dy );
-    d = D / rho;
-    theta = 0;
-
-    /* test required to prevent domain errors if dx=0 and dy=0 */
-    if(d > 0) {
-        theta = mod2pi(atan2( dy, dx ));
-    }
-    alpha = mod2pi(q0[2] - theta);
-    beta  = mod2pi(q1[2] - theta);
-
-    in->alpha = alpha;
-    in->beta  = beta;
-    in->d     = d;
-    in->sa    = sin(alpha);
-    in->sb    = sin(beta);
-    in->ca    = cos(alpha);
-    in->cb    = cos(beta);
-    in->c_ab  = cos(alpha - beta);
-    in->d_sq  = d * d;
-
-    return EDUBOK;
-}
-
-int dubins_LSL(DubinsIntermediateResults* in, double out[3]) 
-{
-    double tmp0, tmp1, p_sq;
-    
-    tmp0 = in->d + in->sa - in->sb;
-    p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
-
-    if(p_sq >= 0) {
-        tmp1 = atan2( (in->cb - in->ca), tmp0 );
-        out[0] = mod2pi(tmp1 - in->alpha);
-        out[1] = sqrt(p_sq);
-        out[2] = mod2pi(in->beta - tmp1);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-
-int dubins_RSR(DubinsIntermediateResults* in, double out[3]) 
-{
-    double tmp0 = in->d - in->sa + in->sb;
-    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
-    if( p_sq >= 0 ) {
-        double tmp1 = atan2( (in->ca - in->cb), tmp0 );
-        out[0] = mod2pi(in->alpha - tmp1);
-        out[1] = sqrt(p_sq);
-        out[2] = mod2pi(tmp1 -in->beta);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_LSR(DubinsIntermediateResults* in, double out[3]) 
-{
-    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
-    if( p_sq >= 0 ) {
-        double p    = sqrt(p_sq);
-        double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
-        out[0] = mod2pi(tmp0 - in->alpha);
-        out[1] = p;
-        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_RSL(DubinsIntermediateResults* in, double out[3]) 
-{
-    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
-    if( p_sq >= 0 ) {
-        double p    = sqrt(p_sq);
-        double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
-        out[0] = mod2pi(in->alpha - tmp0);
-        out[1] = p;
-        out[2] = mod2pi(in->beta - tmp0);
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_RLR(DubinsIntermediateResults* in, double out[3]) 
-{
-    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
-    double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
-    if( fabs(tmp0) <= 1) {
-        double p = mod2pi((2*M_PI) - acos(tmp0) );
-        double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
-        out[0] = t;
-        out[1] = p;
-        out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_LRL(DubinsIntermediateResults* in, double out[3])
-{
-    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
-    double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
-    if( fabs(tmp0) <= 1) {
-        double p = mod2pi( 2*M_PI - acos( tmp0) );
-        double t = mod2pi(-in->alpha - phi + p/2.);
-        out[0] = t;
-        out[1] = p;
-        out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
-        return EDUBOK;
-    }
-    return EDUBNOPATH;
-}
-
-int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]) 
-{
-    int result;
-    switch(pathType)
-    {
-    case LSL:
-        result = dubins_LSL(in, out);
-        break;
-    case RSL:
-        result = dubins_RSL(in, out);
-        break;
-    case LSR:
-        result = dubins_LSR(in, out);
-        break;
-    case RSR:
-        result = dubins_RSR(in, out);
-        break;
-    case LRL:
-        result = dubins_LRL(in, out);
-        break;
-    case RLR:
-        result = dubins_RLR(in, out);
-        break;
-    default:
-        result = EDUBNOPATH;
-    }
-    return result;
-}
-
-
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifdef WIN32
+#define _USE_MATH_DEFINES
+#endif
+#include <math.h>
+#include "dubins.h"
+
+#define EPSILON (10e-10)
+
+typedef enum 
+{
+    L_SEG = 0,
+    S_SEG = 1,
+    R_SEG = 2
+} SegmentType;
+
+/* The segment types for each of the Path types */
+const SegmentType DIRDATA[][3] = {
+    { L_SEG, S_SEG, L_SEG },
+    { L_SEG, S_SEG, R_SEG },
+    { R_SEG, S_SEG, L_SEG },
+    { R_SEG, S_SEG, R_SEG },
+    { R_SEG, L_SEG, R_SEG },
+    { L_SEG, R_SEG, L_SEG }
+};
+
+typedef struct 
+{
+    double alpha;
+    double beta;
+    double d;
+    double sa;
+    double sb;
+    double ca;
+    double cb;
+    double c_ab;
+    double d_sq;
+} DubinsIntermediateResults;
+
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
+
+/**
+ * Floating point modulus suitable for rings
+ *
+ * fmod doesn't behave correctly for angular quantities, this function does
+ */
+double fmodr( double x, double y)
+{
+    return x - y*floor(x/y);
+}
+
+double mod2pi( double theta )
+{
+    return fmodr( theta, 2 * M_PI );
+}
+
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
+{
+    int i, errcode;
+    DubinsIntermediateResults in;
+    double params[3];
+    double cost;
+    double best_cost = INFINITY;
+    int best_word = -1;
+    errcode = dubins_intermediate_results(&in, q0, q1, rho);
+    if(errcode != EDUBOK) {
+        return errcode;
+    }
+
+
+    path->qi[0] = q0[0];
+    path->qi[1] = q0[1];
+    path->qi[2] = q0[2];
+    path->rho = rho;
+ 
+    for( i = 0; i < 6; i++ ) {
+        DubinsPathType pathType = (DubinsPathType)i;
+        errcode = dubins_word(&in, pathType, params);
+        if(errcode == EDUBOK) {
+            cost = params[0] + params[1] + params[2];
+            if(cost < best_cost) {
+                best_word = i;
+                best_cost = cost;
+                path->param[0] = params[0];
+                path->param[1] = params[1];
+                path->param[2] = params[2];
+                path->type = pathType;
+            }
+        }
+    }
+    if(best_word == -1) {
+        return EDUBNOPATH;
+    }
+    return EDUBOK;
+}
+
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
+{
+    int errcode;
+    DubinsIntermediateResults in;
+    errcode = dubins_intermediate_results(&in, q0, q1, rho);
+    if(errcode == EDUBOK) {
+        double params[3];
+        errcode = dubins_word(&in, pathType, params);
+        if(errcode == EDUBOK) {
+            path->param[0] = params[0];
+            path->param[1] = params[1];
+            path->param[2] = params[2];
+            path->qi[0] = q0[0];
+            path->qi[1] = q0[1];
+            path->qi[2] = q0[2];
+            path->rho = rho;
+            path->type = pathType;
+        }
+    }
+    return errcode;
+}
+
+double dubins_path_length( DubinsPath* path )
+{
+    double length = 0.;
+    length += path->param[0];
+    length += path->param[1];
+    length += path->param[2];
+    length = length * path->rho;
+    return length;
+}
+
+double dubins_segment_length( DubinsPath* path, int i )
+{
+    if( (i < 0) || (i > 2) )
+    {
+        return INFINITY;
+    }
+    return path->param[i] * path->rho;
+}
+
+double dubins_segment_length_normalized( DubinsPath* path, int i )
+{
+    if( (i < 0) || (i > 2) )
+    {
+        return INFINITY;
+    }
+    return path->param[i];
+} 
+
+DubinsPathType dubins_path_type( DubinsPath* path ) 
+{
+    return path->type;
+}
+
+void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
+{
+    double st = sin(qi[2]);
+    double ct = cos(qi[2]);
+    if( type == L_SEG ) {
+        qt[0] = +sin(qi[2]+t) - st;
+        qt[1] = -cos(qi[2]+t) + ct;
+        qt[2] = t;
+    }
+    else if( type == R_SEG ) {
+        qt[0] = -sin(qi[2]-t) + st;
+        qt[1] = +cos(qi[2]-t) - ct;
+        qt[2] = -t;
+    }
+    else if( type == S_SEG ) {
+        qt[0] = ct * t;
+        qt[1] = st * t;
+        qt[2] = 0.0;
+    }
+    qt[0] += qi[0];
+    qt[1] += qi[1];
+    qt[2] += qi[2];
+}
+
+int dubins_path_sample( DubinsPath* path, double t, double q[3] )
+{
+    /* tprime is the normalised variant of the parameter t */
+    double tprime = t / path->rho;
+    double qi[3]; /* The translated initial configuration */
+    double q1[3]; /* end-of segment 1 */
+    double q2[3]; /* end-of segment 2 */
+    const SegmentType* types = DIRDATA[path->type];
+    double p1, p2;
+
+    if( t < 0 || t > dubins_path_length(path) ) {
+        return EDUBPARAM;
+    }
+
+    /* initial configuration */
+    qi[0] = 0.0;
+    qi[1] = 0.0;
+    qi[2] = path->qi[2];
+
+    /* generate the target configuration */
+    p1 = path->param[0];
+    p2 = path->param[1];
+    dubins_segment( p1,      qi,    q1, types[0] );
+    dubins_segment( p2,      q1,    q2, types[1] );
+    if( tprime < p1 ) {
+        dubins_segment( tprime, qi, q, types[0] );
+    }
+    else if( tprime < (p1+p2) ) {
+        dubins_segment( tprime-p1, q1, q,  types[1] );
+    }
+    else {
+        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
+    }
+
+    /* scale the target configuration, translate back to the original starting point */
+    q[0] = q[0] * path->rho + path->qi[0];
+    q[1] = q[1] * path->rho + path->qi[1];
+    q[2] = mod2pi(q[2]);
+
+    return EDUBOK;
+}
+
+int dubins_path_sample_many(DubinsPath* path, double stepSize, 
+                            DubinsPathSamplingCallback cb, void* user_data)
+{
+    int retcode;
+    double q[3];
+    double x = 0.0;
+    double length = dubins_path_length(path);
+    while( x <  length ) {
+        dubins_path_sample( path, x, q );
+        retcode = cb(q, x, user_data);
+        if( retcode != 0 ) {
+            return retcode;
+        }
+        x += stepSize;
+    }
+    return 0;
+}
+
+int dubins_path_endpoint( DubinsPath* path, double q[3] )
+{
+    return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
+}
+
+int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
+{
+    /* calculate the true parameter */
+    double tprime = t / path->rho;
+
+    if((t < 0) || (t > dubins_path_length(path)))
+    {
+        return EDUBPARAM; 
+    }
+
+    /* copy most of the data */
+    newpath->qi[0] = path->qi[0];
+    newpath->qi[1] = path->qi[1];
+    newpath->qi[2] = path->qi[2];
+    newpath->rho   = path->rho;
+    newpath->type  = path->type;
+
+    /* fix the parameters */
+    newpath->param[0] = fmin( path->param[0], tprime );
+    newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
+    newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
+    return 0;
+}
+
+int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
+{
+    double dx, dy, D, d, theta, alpha, beta;
+    if( rho <= 0.0 ) {
+        return EDUBBADRHO;
+    }
+
+    dx = q1[0] - q0[0];
+    dy = q1[1] - q0[1];
+    D = sqrt( dx * dx + dy * dy );
+    d = D / rho;
+    theta = 0;
+
+    /* test required to prevent domain errors if dx=0 and dy=0 */
+    if(d > 0) {
+        theta = mod2pi(atan2( dy, dx ));
+    }
+    alpha = mod2pi(q0[2] - theta);
+    beta  = mod2pi(q1[2] - theta);
+
+    in->alpha = alpha;
+    in->beta  = beta;
+    in->d     = d;
+    in->sa    = sin(alpha);
+    in->sb    = sin(beta);
+    in->ca    = cos(alpha);
+    in->cb    = cos(beta);
+    in->c_ab  = cos(alpha - beta);
+    in->d_sq  = d * d;
+
+    return EDUBOK;
+}
+
+int dubins_LSL(DubinsIntermediateResults* in, double out[3]) 
+{
+    double tmp0, tmp1, p_sq;
+    
+    tmp0 = in->d + in->sa - in->sb;
+    p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
+
+    if(p_sq >= 0) {
+        tmp1 = atan2( (in->cb - in->ca), tmp0 );
+        out[0] = mod2pi(tmp1 - in->alpha);
+        out[1] = sqrt(p_sq);
+        out[2] = mod2pi(in->beta - tmp1);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+
+int dubins_RSR(DubinsIntermediateResults* in, double out[3]) 
+{
+    double tmp0 = in->d - in->sa + in->sb;
+    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
+    if( p_sq >= 0 ) {
+        double tmp1 = atan2( (in->ca - in->cb), tmp0 );
+        out[0] = mod2pi(in->alpha - tmp1);
+        out[1] = sqrt(p_sq);
+        out[2] = mod2pi(tmp1 -in->beta);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_LSR(DubinsIntermediateResults* in, double out[3]) 
+{
+    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
+    if( p_sq >= 0 ) {
+        double p    = sqrt(p_sq);
+        double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
+        out[0] = mod2pi(tmp0 - in->alpha);
+        out[1] = p;
+        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_RSL(DubinsIntermediateResults* in, double out[3]) 
+{
+    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
+    if( p_sq >= 0 ) {
+        double p    = sqrt(p_sq);
+        double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
+        out[0] = mod2pi(in->alpha - tmp0);
+        out[1] = p;
+        out[2] = mod2pi(in->beta - tmp0);
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_RLR(DubinsIntermediateResults* in, double out[3]) 
+{
+    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
+    double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
+    if( fabs(tmp0) <= 1) {
+        double p = mod2pi((2*M_PI) - acos(tmp0) );
+        double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
+        out[0] = t;
+        out[1] = p;
+        out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_LRL(DubinsIntermediateResults* in, double out[3])
+{
+    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
+    double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
+    if( fabs(tmp0) <= 1) {
+        double p = mod2pi( 2*M_PI - acos( tmp0) );
+        double t = mod2pi(-in->alpha - phi + p/2.);
+        out[0] = t;
+        out[1] = p;
+        out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
+        return EDUBOK;
+    }
+    return EDUBNOPATH;
+}
+
+int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]) 
+{
+    int result;
+    switch(pathType)
+    {
+    case LSL:
+        result = dubins_LSL(in, out);
+        break;
+    case RSL:
+        result = dubins_RSL(in, out);
+        break;
+    case LSR:
+        result = dubins_LSR(in, out);
+        break;
+    case RSR:
+        result = dubins_RSR(in, out);
+        break;
+    case LRL:
+        result = dubins_LRL(in, out);
+        break;
+    case RLR:
+        result = dubins_RLR(in, out);
+        break;
+    default:
+        result = EDUBNOPATH;
+    }
+    return result;
+}
+
+

src/driver/driver_map_xodrload/dubins.h

@@ -1,170 +1,170 @@
-/*
- * Copyright (c) 2008-2018, Andrew Walker
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-#ifndef DUBINS_H
-#define DUBINS_H
-
-typedef enum 
-{
-    LSL = 0,
-    LSR = 1,
-    RSL = 2,
-    RSR = 3,
-    RLR = 4,
-    LRL = 5
-} DubinsPathType;
-
-typedef struct 
-{
-    /* the initial configuration */
-    double qi[3];        
-    /* the lengths of the three segments */
-    double param[3];     
-    /* model forward velocity / model angular velocity */
-    double rho;          
-    /* the path type described */
-    DubinsPathType type; 
-} DubinsPath;
-
-#define EDUBOK        (0)   /* No error */
-#define EDUBCOCONFIGS (1)   /* Colocated configurations */
-#define EDUBPARAM     (2)   /* Path parameterisitation error */
-#define EDUBBADRHO    (3)   /* the rho value is invalid */
-#define EDUBNOPATH    (4)   /* no connection between configurations with this word */
-
-/**
- * Callback function for path sampling
- *
- * @note the q parameter is a configuration
- * @note the t parameter is the distance along the path
- * @note the user_data parameter is forwarded from the caller
- * @note return non-zero to denote sampling should be stopped
- */
-typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
-
-/**
- * Generate a path from an initial configuration to
- * a target configuration, with a specified maximum turning
- * radii
- *
- * A configuration is (x, y, theta), where theta is in radians, with zero
- * along the line x = 0, and counter-clockwise is positive
- *
- * @param path  - the resultant path
- * @param q0    - a configuration specified as an array of x, y, theta
- * @param q1    - a configuration specified as an array of x, y, theta
- * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
- * @return      - non-zero on error
- */
-int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
-
-/**
- * Generate a path with a specified word from an initial configuration to
- * a target configuration, with a specified turning radius 
- *
- * @param path     - the resultant path
- * @param q0       - a configuration specified as an array of x, y, theta
- * @param q1       - a configuration specified as an array of x, y, theta
- * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
- * @param pathType - the specific path type to use
- * @return         - non-zero on error
- */
-int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
-
-/**
- * Calculate the length of an initialised path
- *
- * @param path - the path to find the length of
- */
-double dubins_path_length(DubinsPath* path);
-
-/**
- * Return the length of a specific segment in an initialized path
- *
- * @param path - the path to find the length of
- * @param i    - the segment you to get the length of (0-2)
- */
-double dubins_segment_length(DubinsPath* path, int i);
-
-/**
- * Return the normalized length of a specific segment in an initialized path
- *
- * @param path - the path to find the length of
- * @param i    - the segment you to get the length of (0-2)
- */
-double dubins_segment_length_normalized( DubinsPath* path, int i );
-
-/**
- * Extract an integer that represents which path type was used
- *
- * @param path    - an initialised path
- * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL 
- */
-DubinsPathType dubins_path_type(DubinsPath* path);
-
-/**
- * Calculate the configuration along the path, using the parameter t
- *
- * @param path - an initialised path
- * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
- * @param q    - the configuration result
- * @returns    - non-zero if 't' is not in the correct range
- */
-int dubins_path_sample(DubinsPath* path, double t, double q[3]);
-
-/**
- * Walk along the path at a fixed sampling interval, calling the
- * callback function at each interval
- *
- * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
- *
- * @param path      - the path to sample
- * @param stepSize  - the distance along the path for subsequent samples
- * @param cb        - the callback function to call for each sample
- * @param user_data - optional information to pass on to the callback
- *
- * @returns - zero on successful completion, or the result of the callback
- */
-int dubins_path_sample_many(DubinsPath* path, 
-                            double stepSize, 
-                            DubinsPathSamplingCallback cb, 
-                            void* user_data);
-
-/**
- * Convenience function to identify the endpoint of a path
- *
- * @param path - an initialised path
- * @param q    - the configuration result
- */
-int dubins_path_endpoint(DubinsPath* path, double q[3]);
-
-/**
- * Convenience function to extract a subset of a path
- *
- * @param path    - an initialised path
- * @param t       - a length measure, where 0 < t < dubins_path_length(path)
- * @param newpath - the resultant path
- */
-int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
-
-
-#endif /* DUBINS_H */
-
+/*
+ * Copyright (c) 2008-2018, Andrew Walker
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+#ifndef DUBINS_H
+#define DUBINS_H
+
+typedef enum 
+{
+    LSL = 0,
+    LSR = 1,
+    RSL = 2,
+    RSR = 3,
+    RLR = 4,
+    LRL = 5
+} DubinsPathType;
+
+typedef struct 
+{
+    /* the initial configuration */
+    double qi[3];        
+    /* the lengths of the three segments */
+    double param[3];     
+    /* model forward velocity / model angular velocity */
+    double rho;          
+    /* the path type described */
+    DubinsPathType type; 
+} DubinsPath;
+
+#define EDUBOK        (0)   /* No error */
+#define EDUBCOCONFIGS (1)   /* Colocated configurations */
+#define EDUBPARAM     (2)   /* Path parameterisitation error */
+#define EDUBBADRHO    (3)   /* the rho value is invalid */
+#define EDUBNOPATH    (4)   /* no connection between configurations with this word */
+
+/**
+ * Callback function for path sampling
+ *
+ * @note the q parameter is a configuration
+ * @note the t parameter is the distance along the path
+ * @note the user_data parameter is forwarded from the caller
+ * @note return non-zero to denote sampling should be stopped
+ */
+typedef int (*DubinsPathSamplingCallback)(double q[3], double t, void* user_data);
+
+/**
+ * Generate a path from an initial configuration to
+ * a target configuration, with a specified maximum turning
+ * radii
+ *
+ * A configuration is (x, y, theta), where theta is in radians, with zero
+ * along the line x = 0, and counter-clockwise is positive
+ *
+ * @param path  - the resultant path
+ * @param q0    - a configuration specified as an array of x, y, theta
+ * @param q1    - a configuration specified as an array of x, y, theta
+ * @param rho   - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @return      - non-zero on error
+ */
+int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho);
+
+/**
+ * Generate a path with a specified word from an initial configuration to
+ * a target configuration, with a specified turning radius 
+ *
+ * @param path     - the resultant path
+ * @param q0       - a configuration specified as an array of x, y, theta
+ * @param q1       - a configuration specified as an array of x, y, theta
+ * @param rho      - turning radius of the vehicle (forward velocity divided by maximum angular velocity)
+ * @param pathType - the specific path type to use
+ * @return         - non-zero on error
+ */
+int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType);
+
+/**
+ * Calculate the length of an initialised path
+ *
+ * @param path - the path to find the length of
+ */
+double dubins_path_length(DubinsPath* path);
+
+/**
+ * Return the length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length(DubinsPath* path, int i);
+
+/**
+ * Return the normalized length of a specific segment in an initialized path
+ *
+ * @param path - the path to find the length of
+ * @param i    - the segment you to get the length of (0-2)
+ */
+double dubins_segment_length_normalized( DubinsPath* path, int i );
+
+/**
+ * Extract an integer that represents which path type was used
+ *
+ * @param path    - an initialised path
+ * @return        - one of LSL, LSR, RSL, RSR, RLR or LRL 
+ */
+DubinsPathType dubins_path_type(DubinsPath* path);
+
+/**
+ * Calculate the configuration along the path, using the parameter t
+ *
+ * @param path - an initialised path
+ * @param t    - a length measure, where 0 <= t < dubins_path_length(path)
+ * @param q    - the configuration result
+ * @returns    - non-zero if 't' is not in the correct range
+ */
+int dubins_path_sample(DubinsPath* path, double t, double q[3]);
+
+/**
+ * Walk along the path at a fixed sampling interval, calling the
+ * callback function at each interval
+ *
+ * The sampling process continues until the whole path is sampled, or the callback returns a non-zero value
+ *
+ * @param path      - the path to sample
+ * @param stepSize  - the distance along the path for subsequent samples
+ * @param cb        - the callback function to call for each sample
+ * @param user_data - optional information to pass on to the callback
+ *
+ * @returns - zero on successful completion, or the result of the callback
+ */
+int dubins_path_sample_many(DubinsPath* path, 
+                            double stepSize, 
+                            DubinsPathSamplingCallback cb, 
+                            void* user_data);
+
+/**
+ * Convenience function to identify the endpoint of a path
+ *
+ * @param path - an initialised path
+ * @param q    - the configuration result
+ */
+int dubins_path_endpoint(DubinsPath* path, double q[3]);
+
+/**
+ * Convenience function to extract a subset of a path
+ *
+ * @param path    - an initialised path
+ * @param t       - a length measure, where 0 < t < dubins_path_length(path)
+ * @param newpath - the resultant path
+ */
+int dubins_extract_subpath(DubinsPath* path, double t, DubinsPath* newpath);
+
+
+#endif /* DUBINS_H */
+

src/driver/driver_map_xodrload/fresnl.cpp

@@ -1,518 +1,518 @@
-/*							fresnl.c
- *
- *	Fresnel integral
- *
- *
- *
- * SYNOPSIS:
- *
- * double x, S, C;
- * void fresnl();
- *
- * fresnl( x, _&S, _&C );
- *
- *
- * DESCRIPTION:
- *
- * Evaluates the Fresnel integrals
- *
- *           x
- *           -
- *          | |
- * C(x) =   |   cos(pi/2 t**2) dt,
- *        | |
- *         -
- *          0
- *
- *           x
- *           -
- *          | |
- * S(x) =   |   sin(pi/2 t**2) dt.
- *        | |
- *         -
- *          0
- *
- *
- * The integrals are evaluated by a power series for x < 1.
- * For x >= 1 auxiliary functions f(x) and g(x) are employed
- * such that
- *
- * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 )
- * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 )
- *
- *
- *
- * ACCURACY:
- *
- *  Relative error.
- *
- * Arithmetic  function   domain     # trials      peak         rms
- *   IEEE       S(x)      0, 10       10000       2.0e-15     3.2e-16
- *   IEEE       C(x)      0, 10       10000       1.8e-15     3.3e-16
- *   DEC        S(x)      0, 10        6000       2.2e-16     3.9e-17
- *   DEC        C(x)      0, 10        5000       2.3e-16     3.9e-17
- */
-
-/*
-Cephes Math Library Release 2.8:  June, 2000
-Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier
-*/
-
-#include "mconf.h"
-
-
-/* S(x) for small x */
-#ifdef UNK
-static double sn[6] = {
--2.99181919401019853726E3,
- 7.08840045257738576863E5,
--6.29741486205862506537E7,
- 2.54890880573376359104E9,
--4.42979518059697779103E10,
- 3.18016297876567817986E11,
-};
-static double sd[6] = {
-/* 1.00000000000000000000E0,*/
- 2.81376268889994315696E2,
- 4.55847810806532581675E4,
- 5.17343888770096400730E6,
- 4.19320245898111231129E8,
- 2.24411795645340920940E10,
- 6.07366389490084639049E11,
-};
-#endif
-#ifdef DEC
-static unsigned short sn[24] = {
-0143072,0176433,0065455,0127034,
-0045055,0007200,0134540,0026661,
-0146560,0035061,0023667,0127545,
-0050027,0166503,0002673,0153756,
-0151045,0002721,0121737,0102066,
-0051624,0013177,0033451,0021271,
-};
-static unsigned short sd[24] = {
-/*0040200,0000000,0000000,0000000,*/
-0042214,0130051,0112070,0101617,
-0044062,0010307,0172346,0152510,
-0045635,0160575,0143200,0136642,
-0047307,0171215,0127457,0052361,
-0050647,0031447,0032621,0013510,
-0052015,0064733,0117362,0012653,
-};
-#endif
-#ifdef IBMPC
-static unsigned short sn[24] = {
-0xb5c3,0x6d65,0x5fa3,0xc0a7,
-0x05b6,0x172c,0xa1d0,0x4125,
-0xf5ed,0x24f6,0x0746,0xc18e,
-0x7afe,0x60b7,0xfda8,0x41e2,
-0xf087,0x347b,0xa0ba,0xc224,
-0x2457,0xe6e5,0x82cf,0x4252,
-};
-static unsigned short sd[24] = {
-/*0x0000,0x0000,0x0000,0x3ff0,*/
-0x1072,0x3287,0x9605,0x4071,
-0xdaa9,0xfe9c,0x4218,0x40e6,
-0x17b4,0xb8d0,0xbc2f,0x4153,
-0xea9e,0xb5e5,0xfe51,0x41b8,
-0x22e9,0xe6b2,0xe664,0x4214,
-0x42b5,0x73de,0xad3b,0x4261,
-};
-#endif
-#ifdef MIEEE
-static unsigned short sn[24] = {
-0xc0a7,0x5fa3,0x6d65,0xb5c3,
-0x4125,0xa1d0,0x172c,0x05b6,
-0xc18e,0x0746,0x24f6,0xf5ed,
-0x41e2,0xfda8,0x60b7,0x7afe,
-0xc224,0xa0ba,0x347b,0xf087,
-0x4252,0x82cf,0xe6e5,0x2457,
-};
-static unsigned short sd[24] = {
-/*0x3ff0,0x0000,0x0000,0x0000,*/
-0x4071,0x9605,0x3287,0x1072,
-0x40e6,0x4218,0xfe9c,0xdaa9,
-0x4153,0xbc2f,0xb8d0,0x17b4,
-0x41b8,0xfe51,0xb5e5,0xea9e,
-0x4214,0xe664,0xe6b2,0x22e9,
-0x4261,0xad3b,0x73de,0x42b5,
-};
-#endif
-
-/* C(x) for small x */
-#ifdef UNK
-static double cn[6] = {
--4.98843114573573548651E-8,
- 9.50428062829859605134E-6,
--6.45191435683965050962E-4,
- 1.88843319396703850064E-2,
--2.05525900955013891793E-1,
- 9.99999999999999998822E-1,
-};
-static double cd[7] = {
- 3.99982968972495980367E-12,
- 9.15439215774657478799E-10,
- 1.25001862479598821474E-7,
- 1.22262789024179030997E-5,
- 8.68029542941784300606E-4,
- 4.12142090722199792936E-2,
- 1.00000000000000000118E0,
-};
-#endif
-#ifdef DEC
-static unsigned short cn[24] = {
-0132126,0040141,0063733,0013231,
-0034037,0072223,0010200,0075637,
-0135451,0021020,0073264,0036057,
-0036632,0131520,0101316,0060233,
-0137522,0072541,0136124,0132202,
-0040200,0000000,0000000,0000000,
-};
-static unsigned short cd[28] = {
-0026614,0135503,0051776,0032631,
-0030573,0121116,0154033,0126712,
-0032406,0034100,0012442,0106212,
-0034115,0017567,0150520,0164623,
-0035543,0106171,0177336,0146351,
-0037050,0150073,0000607,0171635,
-0040200,0000000,0000000,0000000,
-};
-#endif
-#ifdef IBMPC
-static unsigned short cn[24] = {
-0x62d3,0x2cfb,0xc80c,0xbe6a,
-0x0f74,0x6210,0xee92,0x3ee3,
-0x8786,0x0ed6,0x2442,0xbf45,
-0xcc13,0x1059,0x566a,0x3f93,
-0x9690,0x378a,0x4eac,0xbfca,
-0x0000,0x0000,0x0000,0x3ff0,
-};
-static unsigned short cd[28] = {
-0xc6b3,0x6a7f,0x9768,0x3d91,
-0x75b9,0xdb03,0x7449,0x3e0f,
-0x5191,0x02a4,0xc708,0x3e80,
-0x1d32,0xfa2a,0xa3ee,0x3ee9,
-0xd99d,0x3fdb,0x718f,0x3f4c,
-0xfe74,0x6030,0x1a07,0x3fa5,
-0x0000,0x0000,0x0000,0x3ff0,
-};
-#endif
-#ifdef MIEEE
-static unsigned short cn[24] = {
-0xbe6a,0xc80c,0x2cfb,0x62d3,
-0x3ee3,0xee92,0x6210,0x0f74,
-0xbf45,0x2442,0x0ed6,0x8786,
-0x3f93,0x566a,0x1059,0xcc13,
-0xbfca,0x4eac,0x378a,0x9690,
-0x3ff0,0x0000,0x0000,0x0000,
-};
-static unsigned short cd[28] = {
-0x3d91,0x9768,0x6a7f,0xc6b3,
-0x3e0f,0x7449,0xdb03,0x75b9,
-0x3e80,0xc708,0x02a4,0x5191,
-0x3ee9,0xa3ee,0xfa2a,0x1d32,
-0x3f4c,0x718f,0x3fdb,0xd99d,
-0x3fa5,0x1a07,0x6030,0xfe74,
-0x3ff0,0x0000,0x0000,0x0000,
-};
-#endif
-
-/* Auxiliary function f(x) */
-#ifdef UNK
-static double fn[10] = {
-  4.21543555043677546506E-1,
-  1.43407919780758885261E-1,
-  1.15220955073585758835E-2,
-  3.45017939782574027900E-4,
-  4.63613749287867322088E-6,
-  3.05568983790257605827E-8,
-  1.02304514164907233465E-10,
-  1.72010743268161828879E-13,
-  1.34283276233062758925E-16,
-  3.76329711269987889006E-20,
-};
-static double fd[10] = {
-/*  1.00000000000000000000E0,*/
-  7.51586398353378947175E-1,
-  1.16888925859191382142E-1,
-  6.44051526508858611005E-3,
-  1.55934409164153020873E-4,
-  1.84627567348930545870E-6,
-  1.12699224763999035261E-8,
-  3.60140029589371370404E-11,
-  5.88754533621578410010E-14,
-  4.52001434074129701496E-17,
-  1.25443237090011264384E-20,
-};
-#endif
-#ifdef DEC
-static unsigned short fn[40] = {
-0037727,0152216,0106601,0016214,
-0037422,0154606,0112710,0071355,
-0036474,0143453,0154253,0166545,
-0035264,0161606,0022250,0073743,
-0033633,0110036,0024653,0136246,
-0032003,0036652,0041164,0036413,
-0027740,0174122,0046305,0036726,
-0025501,0125270,0121317,0167667,
-0023032,0150555,0076175,0047443,
-0020061,0133570,0070130,0027657,
-};
-static unsigned short fd[40] = {
-/*0040200,0000000,0000000,0000000,*/
-0040100,0063767,0054413,0151452,
-0037357,0061566,0007243,0065754,
-0036323,0005365,0033552,0133625,
-0035043,0101123,0000275,0165402,
-0033367,0146614,0110623,0023647,
-0031501,0116644,0125222,0144263,
-0027436,0062051,0117235,0001411,
-0025204,0111543,0056370,0036201,
-0022520,0071351,0015227,0122144,
-0017554,0172240,0112713,0005006,
-};
-#endif
-#ifdef IBMPC
-static unsigned short fn[40] = {
-0x2391,0xd1b0,0xfa91,0x3fda,
-0x0e5e,0xd2b9,0x5b30,0x3fc2,
-0x7dad,0x7b15,0x98e5,0x3f87,
-0x0efc,0xc495,0x9c70,0x3f36,
-0x7795,0xc535,0x7203,0x3ed3,
-0x87a1,0x484e,0x67b5,0x3e60,
-0xa7bb,0x4998,0x1f0a,0x3ddc,
-0xfdf7,0x1459,0x3557,0x3d48,
-0xa9e4,0xaf8f,0x5a2d,0x3ca3,
-0x05f6,0x0e0b,0x36ef,0x3be6,
-};
-static unsigned short fd[40] = {
-/*0x0000,0x0000,0x0000,0x3ff0,*/
-0x7a65,0xeb21,0x0cfe,0x3fe8,
-0x6d7d,0xc1d4,0xec6e,0x3fbd,
-0x56f3,0xa6ed,0x615e,0x3f7a,
-0xbd60,0x6017,0x704a,0x3f24,
-0x64f5,0x9232,0xf9b1,0x3ebe,
-0x5916,0x9552,0x33b4,0x3e48,
-0xa061,0x33d3,0xcc85,0x3dc3,
-0x0790,0x6b9f,0x926c,0x3d30,
-0xf48d,0x2352,0x0e5d,0x3c8a,
-0x6141,0x12b9,0x9e94,0x3bcd,
-};
-#endif
-#ifdef MIEEE
-static unsigned short fn[40] = {
-0x3fda,0xfa91,0xd1b0,0x2391,
-0x3fc2,0x5b30,0xd2b9,0x0e5e,
-0x3f87,0x98e5,0x7b15,0x7dad,
-0x3f36,0x9c70,0xc495,0x0efc,
-0x3ed3,0x7203,0xc535,0x7795,
-0x3e60,0x67b5,0x484e,0x87a1,
-0x3ddc,0x1f0a,0x4998,0xa7bb,
-0x3d48,0x3557,0x1459,0xfdf7,
-0x3ca3,0x5a2d,0xaf8f,0xa9e4,
-0x3be6,0x36ef,0x0e0b,0x05f6,
-};
-static unsigned short fd[40] = {
-/*0x3ff0,0x0000,0x0000,0x0000,*/
-0x3fe8,0x0cfe,0xeb21,0x7a65,
-0x3fbd,0xec6e,0xc1d4,0x6d7d,
-0x3f7a,0x615e,0xa6ed,0x56f3,
-0x3f24,0x704a,0x6017,0xbd60,
-0x3ebe,0xf9b1,0x9232,0x64f5,
-0x3e48,0x33b4,0x9552,0x5916,
-0x3dc3,0xcc85,0x33d3,0xa061,
-0x3d30,0x926c,0x6b9f,0x0790,
-0x3c8a,0x0e5d,0x2352,0xf48d,
-0x3bcd,0x9e94,0x12b9,0x6141,
-};
-#endif
-
-
-/* Auxiliary function g(x) */
-#ifdef UNK
-static double gn[11] = {
-  5.04442073643383265887E-1,
-  1.97102833525523411709E-1,
-  1.87648584092575249293E-2,
-  6.84079380915393090172E-4,
-  1.15138826111884280931E-5,
-  9.82852443688422223854E-8,
-  4.45344415861750144738E-10,
-  1.08268041139020870318E-12,
-  1.37555460633261799868E-15,
-  8.36354435630677421531E-19,
-  1.86958710162783235106E-22,
-};
-static double gd[11] = {
-/*  1.00000000000000000000E0,*/
-  1.47495759925128324529E0,
-  3.37748989120019970451E-1,
-  2.53603741420338795122E-2,
-  8.14679107184306179049E-4,
-  1.27545075667729118702E-5,
-  1.04314589657571990585E-7,
-  4.60680728146520428211E-10,
-  1.10273215066240270757E-12,
-  1.38796531259578871258E-15,
-  8.39158816283118707363E-19,
-  1.86958710162783236342E-22,
-};
-#endif
-#ifdef DEC
-static unsigned short gn[44] = {
-0040001,0021435,0120406,0053123,
-0037511,0152523,0037703,0122011,
-0036631,0134302,0122721,0110235,
-0035463,0051712,0043215,0114732,
-0034101,0025677,0147725,0057630,
-0032323,0010342,0067523,0002206,
-0030364,0152247,0110007,0054107,
-0026230,0057654,0035464,0047124,
-0023706,0036401,0167705,0045440,
-0021166,0154447,0105632,0142461,
-0016142,0002353,0011175,0170530,
-};
-static unsigned short gd[44] = {
-/*0040200,0000000,0000000,0000000,*/
-0040274,0145551,0016742,0127005,
-0037654,0166557,0076416,0015165,
-0036717,0140217,0030675,0050111,
-0035525,0110060,0076405,0070502,
-0034125,0176061,0060120,0031730,
-0032340,0001615,0054343,0120501,
-0030375,0041414,0070747,0107060,
-0026233,0031034,0160757,0074526,
-0023710,0003341,0137100,0144664,
-0021167,0126414,0023774,0015435,
-0016142,0002353,0011175,0170530,
-};
-#endif
-#ifdef IBMPC
-static unsigned short gn[44] = {
-0xcaca,0xb420,0x2463,0x3fe0,
-0x7481,0x67f8,0x3aaa,0x3fc9,
-0x3214,0x54ba,0x3718,0x3f93,
-0xb33b,0x48d1,0x6a79,0x3f46,
-0xabf3,0xf9fa,0x2577,0x3ee8,
-0x6091,0x4dea,0x621c,0x3e7a,
-0xeb09,0xf200,0x9a94,0x3dfe,
-0x89cb,0x8766,0x0bf5,0x3d73,
-0xa964,0x3df8,0xc7a0,0x3cd8,
-0x58a6,0xf173,0xdb24,0x3c2e,
-0xbe2b,0x624f,0x409d,0x3b6c,
-};
-static unsigned short gd[44] = {
-/*0x0000,0x0000,0x0000,0x3ff0,*/
-0x55c1,0x23bc,0x996d,0x3ff7,
-0xc34f,0xefa1,0x9dad,0x3fd5,
-0xaa09,0xe637,0xf811,0x3f99,
-0xae28,0x0fa0,0xb206,0x3f4a,
-0x067b,0x2c0a,0xbf86,0x3eea,
-0x7428,0xab1c,0x0071,0x3e7c,
-0xf1c6,0x8e3c,0xa861,0x3dff,
-0xef2b,0x9c3d,0x6643,0x3d73,
-0x1936,0x37c8,0x00dc,0x3cd9,
-0x8364,0x84ff,0xf5a1,0x3c2e,
-0xbe2b,0x624f,0x409d,0x3b6c,
-};
-#endif
-#ifdef MIEEE
-static unsigned short gn[44] = {
-0x3fe0,0x2463,0xb420,0xcaca,
-0x3fc9,0x3aaa,0x67f8,0x7481,
-0x3f93,0x3718,0x54ba,0x3214,
-0x3f46,0x6a79,0x48d1,0xb33b,
-0x3ee8,0x2577,0xf9fa,0xabf3,
-0x3e7a,0x621c,0x4dea,0x6091,
-0x3dfe,0x9a94,0xf200,0xeb09,
-0x3d73,0x0bf5,0x8766,0x89cb,
-0x3cd8,0xc7a0,0x3df8,0xa964,
-0x3c2e,0xdb24,0xf173,0x58a6,
-0x3b6c,0x409d,0x624f,0xbe2b,
-};
-static unsigned short gd[44] = {
-/*0x3ff0,0x0000,0x0000,0x0000,*/
-0x3ff7,0x996d,0x23bc,0x55c1,
-0x3fd5,0x9dad,0xefa1,0xc34f,
-0x3f99,0xf811,0xe637,0xaa09,
-0x3f4a,0xb206,0x0fa0,0xae28,
-0x3eea,0xbf86,0x2c0a,0x067b,
-0x3e7c,0x0071,0xab1c,0x7428,
-0x3dff,0xa861,0x8e3c,0xf1c6,
-0x3d73,0x6643,0x9c3d,0xef2b,
-0x3cd9,0x00dc,0x37c8,0x1936,
-0x3c2e,0xf5a1,0x84ff,0x8364,
-0x3b6c,0x409d,0x624f,0xbe2b,
-};
-#endif
-
-#ifdef ANSIPROT
-extern "C" {double fabs ( double );}
-extern "C" {double cos ( double );}
-extern "C" {double sin ( double );}
-extern "C" {double polevl ( double, double *, int );}
-extern "C" {double p1evl ( double, double *, int );}
-#else
-double fabs(), cos(), sin(), polevl(), p1evl();
-#endif
-extern "C" {extern double PI, PIO2, MACHEP;}
-
-//int fresnl( xxa, ssa, cca )
-//double xxa, *ssa, *cca;
-
-int fresnl( double xxa, double *ssa, double *cca )
-{
-double f, g, cc, ss, c, s, t, u;
-double x, x2;
-
-x = fabs(xxa);
-x2 = x * x;
-if( x2 < 2.5625 )
-	{
-	t = x2 * x2;
-	ss = x * x2 * polevl( t, sn, 5)/p1evl( t, sd, 6 );
-	cc = x * polevl( t, cn, 5)/polevl(t, cd, 6 );
-	goto done;
-	}
-
-
-
-
-
-
-if( x > 36974.0 )
-	{
-	cc = 0.5;
-	ss = 0.5;
-	goto done;
-	}
-
-
-/*		Asymptotic power series auxiliary functions
- *		for large argument
- */
-	x2 = x * x;
-	t = PI * x2;
-	u = 1.0/(t * t);
-	t = 1.0/t;
-	f = 1.0 - u * polevl( u, fn, 9)/p1evl(u, fd, 10);
-	g = t * polevl( u, gn, 10)/p1evl(u, gd, 11);
-
-	t = PIO2 * x2;
-	c = cos(t);
-	s = sin(t);
-	t = PI * x;
-	cc = 0.5  +  (f * s  -  g * c)/t;
-	ss = 0.5  -  (f * c  +  g * s)/t;
-
-done:
-if( xxa < 0.0 )
-	{
-	cc = -cc;
-	ss = -ss;
-	}
-
-*cca = cc;
-*ssa = ss;
-return(0);
-}
+/*							fresnl.c
+ *
+ *	Fresnel integral
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * double x, S, C;
+ * void fresnl();
+ *
+ * fresnl( x, _&S, _&C );
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates the Fresnel integrals
+ *
+ *           x
+ *           -
+ *          | |
+ * C(x) =   |   cos(pi/2 t**2) dt,
+ *        | |
+ *         -
+ *          0
+ *
+ *           x
+ *           -
+ *          | |
+ * S(x) =   |   sin(pi/2 t**2) dt.
+ *        | |
+ *         -
+ *          0
+ *
+ *
+ * The integrals are evaluated by a power series for x < 1.
+ * For x >= 1 auxiliary functions f(x) and g(x) are employed
+ * such that
+ *
+ * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 )
+ * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 )
+ *
+ *
+ *
+ * ACCURACY:
+ *
+ *  Relative error.
+ *
+ * Arithmetic  function   domain     # trials      peak         rms
+ *   IEEE       S(x)      0, 10       10000       2.0e-15     3.2e-16
+ *   IEEE       C(x)      0, 10       10000       1.8e-15     3.3e-16
+ *   DEC        S(x)      0, 10        6000       2.2e-16     3.9e-17
+ *   DEC        C(x)      0, 10        5000       2.3e-16     3.9e-17
+ */
+
+/*
+Cephes Math Library Release 2.8:  June, 2000
+Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier
+*/
+
+#include "mconf.h"
+
+
+/* S(x) for small x */
+#ifdef UNK
+static double sn[6] = {
+-2.99181919401019853726E3,
+ 7.08840045257738576863E5,
+-6.29741486205862506537E7,
+ 2.54890880573376359104E9,
+-4.42979518059697779103E10,
+ 3.18016297876567817986E11,
+};
+static double sd[6] = {
+/* 1.00000000000000000000E0,*/
+ 2.81376268889994315696E2,
+ 4.55847810806532581675E4,
+ 5.17343888770096400730E6,
+ 4.19320245898111231129E8,
+ 2.24411795645340920940E10,
+ 6.07366389490084639049E11,
+};
+#endif
+#ifdef DEC
+static unsigned short sn[24] = {
+0143072,0176433,0065455,0127034,
+0045055,0007200,0134540,0026661,
+0146560,0035061,0023667,0127545,
+0050027,0166503,0002673,0153756,
+0151045,0002721,0121737,0102066,
+0051624,0013177,0033451,0021271,
+};
+static unsigned short sd[24] = {
+/*0040200,0000000,0000000,0000000,*/
+0042214,0130051,0112070,0101617,
+0044062,0010307,0172346,0152510,
+0045635,0160575,0143200,0136642,
+0047307,0171215,0127457,0052361,
+0050647,0031447,0032621,0013510,
+0052015,0064733,0117362,0012653,
+};
+#endif
+#ifdef IBMPC
+static unsigned short sn[24] = {
+0xb5c3,0x6d65,0x5fa3,0xc0a7,
+0x05b6,0x172c,0xa1d0,0x4125,
+0xf5ed,0x24f6,0x0746,0xc18e,
+0x7afe,0x60b7,0xfda8,0x41e2,
+0xf087,0x347b,0xa0ba,0xc224,
+0x2457,0xe6e5,0x82cf,0x4252,
+};
+static unsigned short sd[24] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x1072,0x3287,0x9605,0x4071,
+0xdaa9,0xfe9c,0x4218,0x40e6,
+0x17b4,0xb8d0,0xbc2f,0x4153,
+0xea9e,0xb5e5,0xfe51,0x41b8,
+0x22e9,0xe6b2,0xe664,0x4214,
+0x42b5,0x73de,0xad3b,0x4261,
+};
+#endif
+#ifdef MIEEE
+static unsigned short sn[24] = {
+0xc0a7,0x5fa3,0x6d65,0xb5c3,
+0x4125,0xa1d0,0x172c,0x05b6,
+0xc18e,0x0746,0x24f6,0xf5ed,
+0x41e2,0xfda8,0x60b7,0x7afe,
+0xc224,0xa0ba,0x347b,0xf087,
+0x4252,0x82cf,0xe6e5,0x2457,
+};
+static unsigned short sd[24] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x4071,0x9605,0x3287,0x1072,
+0x40e6,0x4218,0xfe9c,0xdaa9,
+0x4153,0xbc2f,0xb8d0,0x17b4,
+0x41b8,0xfe51,0xb5e5,0xea9e,
+0x4214,0xe664,0xe6b2,0x22e9,
+0x4261,0xad3b,0x73de,0x42b5,
+};
+#endif
+
+/* C(x) for small x */
+#ifdef UNK
+static double cn[6] = {
+-4.98843114573573548651E-8,
+ 9.50428062829859605134E-6,
+-6.45191435683965050962E-4,
+ 1.88843319396703850064E-2,
+-2.05525900955013891793E-1,
+ 9.99999999999999998822E-1,
+};
+static double cd[7] = {
+ 3.99982968972495980367E-12,
+ 9.15439215774657478799E-10,
+ 1.25001862479598821474E-7,
+ 1.22262789024179030997E-5,
+ 8.68029542941784300606E-4,
+ 4.12142090722199792936E-2,
+ 1.00000000000000000118E0,
+};
+#endif
+#ifdef DEC
+static unsigned short cn[24] = {
+0132126,0040141,0063733,0013231,
+0034037,0072223,0010200,0075637,
+0135451,0021020,0073264,0036057,
+0036632,0131520,0101316,0060233,
+0137522,0072541,0136124,0132202,
+0040200,0000000,0000000,0000000,
+};
+static unsigned short cd[28] = {
+0026614,0135503,0051776,0032631,
+0030573,0121116,0154033,0126712,
+0032406,0034100,0012442,0106212,
+0034115,0017567,0150520,0164623,
+0035543,0106171,0177336,0146351,
+0037050,0150073,0000607,0171635,
+0040200,0000000,0000000,0000000,
+};
+#endif
+#ifdef IBMPC
+static unsigned short cn[24] = {
+0x62d3,0x2cfb,0xc80c,0xbe6a,
+0x0f74,0x6210,0xee92,0x3ee3,
+0x8786,0x0ed6,0x2442,0xbf45,
+0xcc13,0x1059,0x566a,0x3f93,
+0x9690,0x378a,0x4eac,0xbfca,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+static unsigned short cd[28] = {
+0xc6b3,0x6a7f,0x9768,0x3d91,
+0x75b9,0xdb03,0x7449,0x3e0f,
+0x5191,0x02a4,0xc708,0x3e80,
+0x1d32,0xfa2a,0xa3ee,0x3ee9,
+0xd99d,0x3fdb,0x718f,0x3f4c,
+0xfe74,0x6030,0x1a07,0x3fa5,
+0x0000,0x0000,0x0000,0x3ff0,
+};
+#endif
+#ifdef MIEEE
+static unsigned short cn[24] = {
+0xbe6a,0xc80c,0x2cfb,0x62d3,
+0x3ee3,0xee92,0x6210,0x0f74,
+0xbf45,0x2442,0x0ed6,0x8786,
+0x3f93,0x566a,0x1059,0xcc13,
+0xbfca,0x4eac,0x378a,0x9690,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+static unsigned short cd[28] = {
+0x3d91,0x9768,0x6a7f,0xc6b3,
+0x3e0f,0x7449,0xdb03,0x75b9,
+0x3e80,0xc708,0x02a4,0x5191,
+0x3ee9,0xa3ee,0xfa2a,0x1d32,
+0x3f4c,0x718f,0x3fdb,0xd99d,
+0x3fa5,0x1a07,0x6030,0xfe74,
+0x3ff0,0x0000,0x0000,0x0000,
+};
+#endif
+
+/* Auxiliary function f(x) */
+#ifdef UNK
+static double fn[10] = {
+  4.21543555043677546506E-1,
+  1.43407919780758885261E-1,
+  1.15220955073585758835E-2,
+  3.45017939782574027900E-4,
+  4.63613749287867322088E-6,
+  3.05568983790257605827E-8,
+  1.02304514164907233465E-10,
+  1.72010743268161828879E-13,
+  1.34283276233062758925E-16,
+  3.76329711269987889006E-20,
+};
+static double fd[10] = {
+/*  1.00000000000000000000E0,*/
+  7.51586398353378947175E-1,
+  1.16888925859191382142E-1,
+  6.44051526508858611005E-3,
+  1.55934409164153020873E-4,
+  1.84627567348930545870E-6,
+  1.12699224763999035261E-8,
+  3.60140029589371370404E-11,
+  5.88754533621578410010E-14,
+  4.52001434074129701496E-17,
+  1.25443237090011264384E-20,
+};
+#endif
+#ifdef DEC
+static unsigned short fn[40] = {
+0037727,0152216,0106601,0016214,
+0037422,0154606,0112710,0071355,
+0036474,0143453,0154253,0166545,
+0035264,0161606,0022250,0073743,
+0033633,0110036,0024653,0136246,
+0032003,0036652,0041164,0036413,
+0027740,0174122,0046305,0036726,
+0025501,0125270,0121317,0167667,
+0023032,0150555,0076175,0047443,
+0020061,0133570,0070130,0027657,
+};
+static unsigned short fd[40] = {
+/*0040200,0000000,0000000,0000000,*/
+0040100,0063767,0054413,0151452,
+0037357,0061566,0007243,0065754,
+0036323,0005365,0033552,0133625,
+0035043,0101123,0000275,0165402,
+0033367,0146614,0110623,0023647,
+0031501,0116644,0125222,0144263,
+0027436,0062051,0117235,0001411,
+0025204,0111543,0056370,0036201,
+0022520,0071351,0015227,0122144,
+0017554,0172240,0112713,0005006,
+};
+#endif
+#ifdef IBMPC
+static unsigned short fn[40] = {
+0x2391,0xd1b0,0xfa91,0x3fda,
+0x0e5e,0xd2b9,0x5b30,0x3fc2,
+0x7dad,0x7b15,0x98e5,0x3f87,
+0x0efc,0xc495,0x9c70,0x3f36,
+0x7795,0xc535,0x7203,0x3ed3,
+0x87a1,0x484e,0x67b5,0x3e60,
+0xa7bb,0x4998,0x1f0a,0x3ddc,
+0xfdf7,0x1459,0x3557,0x3d48,
+0xa9e4,0xaf8f,0x5a2d,0x3ca3,
+0x05f6,0x0e0b,0x36ef,0x3be6,
+};
+static unsigned short fd[40] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x7a65,0xeb21,0x0cfe,0x3fe8,
+0x6d7d,0xc1d4,0xec6e,0x3fbd,
+0x56f3,0xa6ed,0x615e,0x3f7a,
+0xbd60,0x6017,0x704a,0x3f24,
+0x64f5,0x9232,0xf9b1,0x3ebe,
+0x5916,0x9552,0x33b4,0x3e48,
+0xa061,0x33d3,0xcc85,0x3dc3,
+0x0790,0x6b9f,0x926c,0x3d30,
+0xf48d,0x2352,0x0e5d,0x3c8a,
+0x6141,0x12b9,0x9e94,0x3bcd,
+};
+#endif
+#ifdef MIEEE
+static unsigned short fn[40] = {
+0x3fda,0xfa91,0xd1b0,0x2391,
+0x3fc2,0x5b30,0xd2b9,0x0e5e,
+0x3f87,0x98e5,0x7b15,0x7dad,
+0x3f36,0x9c70,0xc495,0x0efc,
+0x3ed3,0x7203,0xc535,0x7795,
+0x3e60,0x67b5,0x484e,0x87a1,
+0x3ddc,0x1f0a,0x4998,0xa7bb,
+0x3d48,0x3557,0x1459,0xfdf7,
+0x3ca3,0x5a2d,0xaf8f,0xa9e4,
+0x3be6,0x36ef,0x0e0b,0x05f6,
+};
+static unsigned short fd[40] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3fe8,0x0cfe,0xeb21,0x7a65,
+0x3fbd,0xec6e,0xc1d4,0x6d7d,
+0x3f7a,0x615e,0xa6ed,0x56f3,
+0x3f24,0x704a,0x6017,0xbd60,
+0x3ebe,0xf9b1,0x9232,0x64f5,
+0x3e48,0x33b4,0x9552,0x5916,
+0x3dc3,0xcc85,0x33d3,0xa061,
+0x3d30,0x926c,0x6b9f,0x0790,
+0x3c8a,0x0e5d,0x2352,0xf48d,
+0x3bcd,0x9e94,0x12b9,0x6141,
+};
+#endif
+
+
+/* Auxiliary function g(x) */
+#ifdef UNK
+static double gn[11] = {
+  5.04442073643383265887E-1,
+  1.97102833525523411709E-1,
+  1.87648584092575249293E-2,
+  6.84079380915393090172E-4,
+  1.15138826111884280931E-5,
+  9.82852443688422223854E-8,
+  4.45344415861750144738E-10,
+  1.08268041139020870318E-12,
+  1.37555460633261799868E-15,
+  8.36354435630677421531E-19,
+  1.86958710162783235106E-22,
+};
+static double gd[11] = {
+/*  1.00000000000000000000E0,*/
+  1.47495759925128324529E0,
+  3.37748989120019970451E-1,
+  2.53603741420338795122E-2,
+  8.14679107184306179049E-4,
+  1.27545075667729118702E-5,
+  1.04314589657571990585E-7,
+  4.60680728146520428211E-10,
+  1.10273215066240270757E-12,
+  1.38796531259578871258E-15,
+  8.39158816283118707363E-19,
+  1.86958710162783236342E-22,
+};
+#endif
+#ifdef DEC
+static unsigned short gn[44] = {
+0040001,0021435,0120406,0053123,
+0037511,0152523,0037703,0122011,
+0036631,0134302,0122721,0110235,
+0035463,0051712,0043215,0114732,
+0034101,0025677,0147725,0057630,
+0032323,0010342,0067523,0002206,
+0030364,0152247,0110007,0054107,
+0026230,0057654,0035464,0047124,
+0023706,0036401,0167705,0045440,
+0021166,0154447,0105632,0142461,
+0016142,0002353,0011175,0170530,
+};
+static unsigned short gd[44] = {
+/*0040200,0000000,0000000,0000000,*/
+0040274,0145551,0016742,0127005,
+0037654,0166557,0076416,0015165,
+0036717,0140217,0030675,0050111,
+0035525,0110060,0076405,0070502,
+0034125,0176061,0060120,0031730,
+0032340,0001615,0054343,0120501,
+0030375,0041414,0070747,0107060,
+0026233,0031034,0160757,0074526,
+0023710,0003341,0137100,0144664,
+0021167,0126414,0023774,0015435,
+0016142,0002353,0011175,0170530,
+};
+#endif
+#ifdef IBMPC
+static unsigned short gn[44] = {
+0xcaca,0xb420,0x2463,0x3fe0,
+0x7481,0x67f8,0x3aaa,0x3fc9,
+0x3214,0x54ba,0x3718,0x3f93,
+0xb33b,0x48d1,0x6a79,0x3f46,
+0xabf3,0xf9fa,0x2577,0x3ee8,
+0x6091,0x4dea,0x621c,0x3e7a,
+0xeb09,0xf200,0x9a94,0x3dfe,
+0x89cb,0x8766,0x0bf5,0x3d73,
+0xa964,0x3df8,0xc7a0,0x3cd8,
+0x58a6,0xf173,0xdb24,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+static unsigned short gd[44] = {
+/*0x0000,0x0000,0x0000,0x3ff0,*/
+0x55c1,0x23bc,0x996d,0x3ff7,
+0xc34f,0xefa1,0x9dad,0x3fd5,
+0xaa09,0xe637,0xf811,0x3f99,
+0xae28,0x0fa0,0xb206,0x3f4a,
+0x067b,0x2c0a,0xbf86,0x3eea,
+0x7428,0xab1c,0x0071,0x3e7c,
+0xf1c6,0x8e3c,0xa861,0x3dff,
+0xef2b,0x9c3d,0x6643,0x3d73,
+0x1936,0x37c8,0x00dc,0x3cd9,
+0x8364,0x84ff,0xf5a1,0x3c2e,
+0xbe2b,0x624f,0x409d,0x3b6c,
+};
+#endif
+#ifdef MIEEE
+static unsigned short gn[44] = {
+0x3fe0,0x2463,0xb420,0xcaca,
+0x3fc9,0x3aaa,0x67f8,0x7481,
+0x3f93,0x3718,0x54ba,0x3214,
+0x3f46,0x6a79,0x48d1,0xb33b,
+0x3ee8,0x2577,0xf9fa,0xabf3,
+0x3e7a,0x621c,0x4dea,0x6091,
+0x3dfe,0x9a94,0xf200,0xeb09,
+0x3d73,0x0bf5,0x8766,0x89cb,
+0x3cd8,0xc7a0,0x3df8,0xa964,
+0x3c2e,0xdb24,0xf173,0x58a6,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+static unsigned short gd[44] = {
+/*0x3ff0,0x0000,0x0000,0x0000,*/
+0x3ff7,0x996d,0x23bc,0x55c1,
+0x3fd5,0x9dad,0xefa1,0xc34f,
+0x3f99,0xf811,0xe637,0xaa09,
+0x3f4a,0xb206,0x0fa0,0xae28,
+0x3eea,0xbf86,0x2c0a,0x067b,
+0x3e7c,0x0071,0xab1c,0x7428,
+0x3dff,0xa861,0x8e3c,0xf1c6,
+0x3d73,0x6643,0x9c3d,0xef2b,
+0x3cd9,0x00dc,0x37c8,0x1936,
+0x3c2e,0xf5a1,0x84ff,0x8364,
+0x3b6c,0x409d,0x624f,0xbe2b,
+};
+#endif
+
+#ifdef ANSIPROT
+extern "C" {double fabs ( double );}
+extern "C" {double cos ( double );}
+extern "C" {double sin ( double );}
+extern "C" {double polevl ( double, double *, int );}
+extern "C" {double p1evl ( double, double *, int );}
+#else
+double fabs(), cos(), sin(), polevl(), p1evl();
+#endif
+extern "C" {extern double PI, PIO2, MACHEP;}
+
+//int fresnl( xxa, ssa, cca )
+//double xxa, *ssa, *cca;
+
+int fresnl( double xxa, double *ssa, double *cca )
+{
+double f, g, cc, ss, c, s, t, u;
+double x, x2;
+
+x = fabs(xxa);
+x2 = x * x;
+if( x2 < 2.5625 )
+	{
+	t = x2 * x2;
+	ss = x * x2 * polevl( t, sn, 5)/p1evl( t, sd, 6 );
+	cc = x * polevl( t, cn, 5)/polevl(t, cd, 6 );
+	goto done;
+	}
+
+
+
+
+
+
+if( x > 36974.0 )
+	{
+	cc = 0.5;
+	ss = 0.5;
+	goto done;
+	}
+
+
+/*		Asymptotic power series auxiliary functions
+ *		for large argument
+ */
+	x2 = x * x;
+	t = PI * x2;
+	u = 1.0/(t * t);
+	t = 1.0/t;
+	f = 1.0 - u * polevl( u, fn, 9)/p1evl(u, fd, 10);
+	g = t * polevl( u, gn, 10)/p1evl(u, gd, 11);
+
+	t = PIO2 * x2;
+	c = cos(t);
+	s = sin(t);
+	t = PI * x;
+	cc = 0.5  +  (f * s  -  g * c)/t;
+	ss = 0.5  -  (f * c  +  g * s)/t;
+
+done:
+if( xxa < 0.0 )
+	{
+	cc = -cc;
+	ss = -ss;
+	}
+
+*cca = cc;
+*ssa = ss;
+return(0);
+}

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1,3028 +1,3244 @@
-#include "globalplan.h"
-#include "limits"
-#include <iostream>
-
-#include <Eigen/Dense>
-#include <Eigen/Cholesky>
-#include <Eigen/LU>
-#include <Eigen/QR>
-#include <Eigen/SVD>
-
-#include <QDebug>
-#include <QPointF>
-
-
-using namespace  Eigen;
-
-extern "C"
-{
-#include "dubins.h"
-}
-
-
-static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
-/**
- * @brief GetLineDis 获得点到直线Geometry的距离。
- * @param pline
- * @param x
- * @param y
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
-static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
-                         double & neary,double & nearhead)
-{
-    double fRtn = 1000.0;
-
-    double a1,a2,a3,a4,b1,b2;
-    double ratio = pline->GetHdg();
-    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
-    while(ratio<0)ratio = ratio+2.0*M_PI;
-
-    double dis1,dis2,dis3;
-    double x1,x2,x3,y1,y2,y3;
-    x1 = pline->GetX();y1=pline->GetY();
-    if((ratio == 0)||(ratio == M_PI))
-    {
-        a1 = 0;a4=0;
-        a2 = 1;b1= pline->GetY();
-        a3 = 1;b2= x;
-    }
-    else
-    {
-        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
-        {
-            a2=0;a3=0;
-            a1=1,b1=pline->GetX();
-            a4 = 1;b2 = y;
-        }
-        else
-        {
-            a1 = tan(ratio) *(-1.0);
-            a2 = 1;
-            a3 = tan(ratio+M_PI/2.0)*(-1.0);
-            a4 = 1;
-            b1 = a1*pline->GetX() + a2 * pline->GetY();
-            b2 = a3*x+a4*y;
-        }
-    }
-
-    y2 = y1 + pline->GetLength() * sin(ratio);
-    x2 = x1 + pline->GetLength() * cos(ratio);
-
-    Eigen::Matrix2d A;
-    A<<a1,a2,
-            a3,a4;
-    Eigen::Vector2d B(b1,b2);
-
-    Eigen::Vector2d opoint  = A.lu().solve(B);
-
- //   std::cout<<opoint<<std::endl;
-
-    x3 = opoint(0);
-    y3 = opoint(1);
-
-    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
-    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
-    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
-
-//    std::cout<<" dis 1 is "<<dis1<<std::endl;
-//    std::cout<<" dis 2 is "<<dis2<<std::endl;
-//    std::cout<<" dis 3 is "<<dis3<<std::endl;
-
-    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
-    {
- //       std::cout<<" out line"<<std::endl;
-        if(dis1<dis2)
-        {
-            fRtn = dis1;
-            nearx = x1;neary=y1;nearhead = pline->GetHdg();
-        }
-        else
-        {
-            fRtn = dis2;
-            nearx = x2;neary=y2;nearhead = pline->GetHdg();
-        }
-    }
-    else
-    {
-        fRtn = dis3;
-        nearx = x3;neary=y3;nearhead = pline->GetHdg();
-    }
-
-
-//    std::cout<<"dis is "<<fRtn<<" x is "<<nearx<<" y is "<<neary<<" head is "<<nearhead<<std::endl;
-    return fRtn;
-
-
-
-}
-
-
-static int getcirclecrosspoint(QPointF startPos, QPointF endPos, QPointF agvPos, double R,QPointF & point1,QPointF & point2 )
-{
-     double m=0;
-     double k;
-     double b;
-
-      //计算分子
-      m=startPos.x()-endPos.x();
-
-      //求直线的方程
-      if(0==m)
-      {
-          k=100000;
-          b=startPos.y()-k*startPos.x();
-      }
-      else
-      {
-          k=(endPos.y()-startPos.y())/(endPos.x()-startPos.x());
-          b=startPos.y()-k*startPos.x();
-      }
-//      qDebug()<<k<<b;
-
-     double temp = fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b);
-      //求直线与圆的交点 前提是圆需要与直线有交点
-     if(fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b)<R)
-      {
-          point1.setX((2*agvPos.x()-2*k*(b-agvPos.y())+sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
-          point2.setX((2*agvPos.x()-2*k*(b-agvPos.y())-sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
-          point1.setY(k*point1.x()+b);
-          point2.setY(k*point2.x()+b);
-          return 0;
-      }
-
-     return -1;
-}
-
-
-/**
-  * @brief CalcHdg
-  * 计算点0到点1的航向
-  * @param p0        Point 0
-  * @param p1        Point 1
-**/
-static double CalcHdg(QPointF p0, QPointF p1)
-{
-    double x0,y0,x1,y1;
-    x0 = p0.x();
-    y0 = p0.y();
-    x1 = p1.x();
-    y1 = p1.y();
-    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;
-}
-
-
-
-static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
-{
-    double hdg = CalcHdg(poingarc,point1);
-    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
-    else hdg = hdg - M_PI/2.0;
-    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
-    if(hdg < 0)hdg = hdg + 2.0*M_PI;
-
-    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
-
-    double hdgdiff = hdg - parc->GetHdg();
-
-    if(hdgrange >= 0 )
-    {
-        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
-    }
-    else
-    {
-        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
-    }
-
-    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
-    return false;
-
-}
-
-static inline double calcpointdis(QPointF p1,QPointF p2)
-{
-    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
-}
-
-/**
-  * @brief GetArcDis
-  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
-  * @param parc        pointer to a arc geomery
-  * @param x           current x
-  * @param y           current y
-  * @param nearx       near x
-  * @param neary       near y
-  * @param nearhead    nearhead
-**/
-
-static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
-                        double & neary,double & nearhead)
-{
-
- //   double fRtn = 1000.0;
-    if(parc->GetCurvature() == 0.0)return 1000.0;
-
-    double R = fabs(1.0/parc->GetCurvature());
-
-//    if(parc->GetX() == 4.8213842690322309e+01)
-//    {
-//        int a = 1;
-//        a = 3;
-//    }
-//    if(parc->GetCurvature() == -0.0000110203)
-//    {
-//        int a = 1;
-//        a = 3;
-//    }
-
-    //calculate arc center
-    double x_center,y_center;
-    if(parc->GetCurvature() > 0)
-    {
-        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
-        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
-    }
-    else
-    {
-        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
-        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
-    }
-
-    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
-
-
-    QPointF arcpoint;
-    arcpoint.setX(x_center);arcpoint.setY(y_center);
-
-    QPointF pointnow;
-    pointnow.setX(x);pointnow.setY(y);
-    QPointF point1,point2;
-    point1.setX(x_center + (R * cos(hdgltoa)));
-    point1.setY(y_center + (R * sin(hdgltoa)));
-    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
-    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
-
-    //calculat dis
-    bool bp1inarc,bp2inarc;
-    bp1inarc =pointinarc(parc,arcpoint,point1);
-    bp2inarc =pointinarc(parc,arcpoint,point2);
-    double fdis[4];
-    fdis[0] = calcpointdis(pointnow,point1);
-    fdis[1] = calcpointdis(pointnow,point2);
-    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
-    QPointF pointend;
-    double hdgrange = parc->GetLength()*parc->GetCurvature();
-    double hdgend = parc->GetHdg() + hdgrange;
-    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
-    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
-
-    if(parc->GetCurvature() >0)
-    {
-        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
-        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
-    }
-    else
-    {
-        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
-        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
-    }
-
-    fdis[3] = calcpointdis(pointnow,pointend);
-    int indexmin = -1;
-    double fdismin = 1000000.0;
-    if(bp1inarc)
-    {
-        indexmin = 0;fdismin = fdis[0];
-    }
-    if(bp2inarc)
-    {
-        if(indexmin == -1)
-        {
-            indexmin = 1;fdismin = fdis[1];
-        }
-        else
-        {
-            if(fdis[1]<fdismin)
-            {
-                indexmin = 1;fdismin = fdis[1];
-            }
-        }
-    }
-    if(indexmin == -1)
-    {
-        indexmin = 2;fdismin = fdis[2];
-    }
-    else
-    {
-        if(fdis[2]<fdismin)
-        {
-            indexmin = 2;fdismin = fdis[2];
-        }
-    }
-    if(fdis[3]<fdismin)
-    {
-        indexmin = 3;fdismin = fdis[3];
-    }
-
-    switch (indexmin) {
-    case 0:
-        nearx = point1.x();
-        neary = point1.y();
-        if(parc->GetCurvature()<0)
-        {
-            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
-        }
-        else
-        {
-            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
-        }
-        break;
-    case 1:
-        nearx = point2.x();
-        neary = point2.y();
-        if(parc->GetCurvature()<0)
-        {
-            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
-        }
-        else
-        {
-            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
-        }
-        break;
-    case 2:
-        nearx = parc->GetX();
-        neary = parc->GetY();
-        nearhead = parc->GetHdg();
-        break;
-    case 3:
-        nearx = pointend.x();
-        neary = pointend.y();
-        nearhead = hdgend;
-        break;
-    default:
-        std::cout<<"error in arcdis "<<std::endl;
-        break;
-    }
-
-    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
-    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
-    return fdismin;
-}
-
-
-static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead)
-{
-
-    double x,y,hdg;
-    double s = 0.0;
-    double fdismin = 100000.0;
-    double s0 = pspiral->GetS();
-
-    while(s<pspiral->GetLength())
-    {
-        pspiral->GetCoords(s0+s,x,y,hdg);
-        s = s+0.1;
-
-        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
-        if(fdis<fdismin)
-        {
-            fdismin = fdis;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-        }
-    }
-
-    return fdismin;
-}
-
-/**
- * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
- * @param parc
- * @param xnow
- * @param ynow
- * @param nearx
- * @param neary
- * @param nearhead
- * @return
- */
-static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead)
-{
-
-    double s = 0.1;
-    double fdismin = 100000.0;
-
-    double xold,yold;
-    xold = parc->GetX();
-    yold = parc->GetY();
-
-    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
-    if(fdis<fdismin)
-    {
-        fdismin = fdis;
-        nearhead = parc->GetHdg();
-        nearx = parc->GetX();
-        neary = parc->GetY();
-    }
-
-    double s_start = parc->GetS();
-
-    while(s < parc->GetLength())
-    {
-
-        double x, y,hdg;//xtem,ytem;
-        parc->GetCoords(s_start+s,x,y,hdg);
-        if(fdis<fdismin)
-        {
-            fdismin = fdis;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-        }
-
-//        xold = x;
-//        yold = y;
-        s = s+ 0.1;
-    }
-
-    return fdismin;
-
-}
-
-
-static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
-                        double & neary,double & nearhead)
-{
-    double x,y,hdg;
-//    double s = 0.0;
-    double fdismin = 100000.0;
- //   double s0 = ppoly->GetS();
-
-    x = ppoly->GetX();
-    y = ppoly->GetY();
-    double A,B,C,D;
-    A = ppoly->GetA();
-    B = ppoly->GetB();
-    C = ppoly->GetC();
-    D = ppoly->GetD();
-    const double steplim = 0.3;
-    double du = steplim;
-    double u = 0;
-    double v = 0;
-    double oldx,oldy;
-    oldx = x;
-    oldy = y;
-    double xstart,ystart;
-    xstart = x;
-    ystart = y;
-
-
-    double hdgstart = ppoly->GetHdg();
-    double flen = 0;
-    u = u+du;
-    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));
-
-        if(fdis>(steplim*2.0))du = du/2.0;
-        flen = flen + fdis;
-        u = u + du;
-        hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
-
-        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
-        if(fdisnow<fdismin)
-        {
-            fdismin = fdisnow;
-            nearhead = hdg;
-            nearx = x;
-            neary = y;
-        }
-
-        oldx = x;
-        oldy = y;
-    }
-
-    return fdismin;
-}
-
-
-/**
-  * @brief GetNearPoint
-  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
-  * @param x           current x
-  * @param y           current y
-  * @param pxodr       OpenDrive
-  * @param pObjRoad    Road
-  * @param pgeo        Geometry of road
-  * @param nearx       near x
-  * @param neary       near y
-  * @param nearhead    nearhead
-  * @param nearthresh  near threshhold
-  * @retval            if == 0 successfull  <0 fail.
-**/
-int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
-                 double & neary,double & nearhead,const double nearthresh,double &froads)
-{
-
-    double dismin = std::numeric_limits<double>::infinity();
-    s = dismin;
-    int i;
-    double frels;
-    for(i=0;i<pxodr->GetRoadCount();i++)
-    {
-        int j;
-        Road * proad = pxodr->GetRoad(i);
-        double nx,ny,nh;
-
-        for(j=0;j<proad->GetGeometryBlockCount();j++)
-        {
-            GeometryBlock * pgb = proad->GetGeometryBlock(j);
-            double dis;
-            RoadGeometry * pg;
-            pg = pgb->GetGeometryAt(0);
-
-            switch (pg->GetGeomType()) {
-            case 0:   //line
-                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
- //               dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
-                break;
-            case 1:
-                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 2:  //arc
-                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
-                break;
-
-            case 3:
-                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 4:
-                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            default:
-                dis = 100000.0;
-                break;
-            }
-
-            if(dis < dismin)
-            {
-                dismin = dis;
-                nearx = nx;
-                neary = ny;
-                nearhead = nh;
-                s = dis;
-                froads = frels;
-                *pObjRoad = proad;
-                *pgeo = pgb;
-            }
-
-
-//            if(pgb->CheckIfLine())
-//            {
-//                GeometryLine * pline = (GeometryLine *)pgb->GetGeometryAt(0);
-
-//                double dis = GetLineDis(pline,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//            }
-//            else
-//            {
-//                GeometryLine * pline1 = (GeometryLine *)pgb->GetGeometryAt(0);
-//                double dis = GetLineDis(pline1,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//                GeometryArc * parc = (GeometryArc *)pgb->GetGeometryAt(1);
-//                dis = GetArcDis(parc,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//                pline1 = (GeometryLine *)pgb->GetGeometryAt(2);
-//                dis = GetLineDis(pline1,x,y,nx,ny,nh);
-//                if(dis < dismin)
-//                {
-//                    dismin = dis;
-//                    nearx = nx;
-//                    neary = ny;
-//                    nearhead = nh;
-//                    s = dis;
-//                    *pObjRoad = proad;
-//                    *pgeo = pgb;
-//                }
-//            }
-
-        }
-    }
-    if(s > nearthresh)return -1;
-    return 0;
-}
-
-
-namespace iv {
-struct nearroad
-{
-Road * pObjRoad;
-GeometryBlock * pgeob;
-double nearx;
-double neary;
-double nearhead;
-double frels;
-double fdis;
-int lr = 2; //1 left 2 right;
-int nmode = 0; //0 same direciion dis zero   1 oposite dis = 0 2 same direction dis<5 3 oposite  dis <5   4 same direction > 5 5 oposite direction;
-};
-}
-
-int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
-                         const double nearthresh,bool bhdgvalid = true)
-{
-    int i;
-    double frels;
-    int nminmode = 5;
-    for(i=0;i<pxodr->GetRoadCount();i++)
-    {
-        int j;
-        Road * proad = pxodr->GetRoad(i);
-        double nx,ny,nh;
-
-        bool bchange = false;
-        double localdismin = std::numeric_limits<double>::infinity();
-
-        double localnearx = 0;
-        double localneary = 0;
-        double localnearhead = 0;
-        double locals = 0;
-        double localfrels = 0;
-        GeometryBlock * pgeolocal;
-
-        for(j=0;j<proad->GetGeometryBlockCount();j++)
-        {
-            localdismin = std::numeric_limits<double>::infinity();
-            GeometryBlock * pgb = proad->GetGeometryBlock(j);
-            double dis;
-            RoadGeometry * pg;
-            pg = pgb->GetGeometryAt(0);
-
-            switch (pg->GetGeomType()) {
-            case 0:   //line
-                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
-                break;
-            case 1:
-                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 2:  //arc
-                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
-                break;
-
-            case 3:
-                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            case 4:
-                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
-                break;
-            default:
-                dis = 100000.0;
-                break;
-            }
-
-
-            if(dis<localdismin)
-            {
-                localdismin = dis;
-                localnearx = nx;
-                localneary = ny;
-                localnearhead = nh;
-                locals = dis;
-                localfrels = frels;
-                pgeolocal = pgb;
-                bchange = true;
-            }
-
-
-        }
-
-        if(localdismin < nearthresh)
-        {
-            iv::nearroad xnear;
-            xnear.pObjRoad = proad;
-            xnear.pgeob = pgeolocal;
-            xnear.nearx = localnearx;
-            xnear.neary = localneary;
-            xnear.fdis = localdismin;
-            xnear.nearhead = localnearhead;
-            xnear.frels = localfrels;
-            if(xnear.fdis>0)
-            {
-                double fcalhdg = xodrfunc::CalcHdg(QPointF(xnear.nearx,xnear.neary),QPointF(x,y));
-                double fhdgdiff = fcalhdg - xnear.nearhead;
-                while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
-                while(fhdgdiff>= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
-                if(fhdgdiff<M_PI)
-                {
-                    double fdisroad = proad->GetRoadLeftWidth(xnear.frels);
-                    if(fdisroad>xnear.fdis)
-                    {
-                        xnear.fdis = 0;
-                    }
-                    else
-                    {
-                        xnear.fdis = xnear.fdis - fdisroad;
-                    }
-                    xnear.lr = 1;
-                }
-                else
-                {
-                    double fdisroad = proad->GetRoadRightWidth(xnear.frels);
-                    if(fdisroad>xnear.fdis)
-                    {
-                        xnear.fdis = 0;
-                    }
-                    else
-                    {
-                        xnear.fdis = xnear.fdis - fdisroad;
-                    }
-                    xnear.lr = 2;
-                }
-
-
-            }
-            else
-            {
-                if(bhdgvalid == false)
-                {
-                    if(xnear.pObjRoad->GetLaneSectionCount()>0)
-                    {
-                        LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
-                        if(pLS->GetRightLaneCount()>0)
-                        {
-                            xnear.lr = 2;
-                        }
-                        else
-                        {
-                            xnear.lr = 1;
-                        }
-                    }
-                    else
-                    {
-                        xnear.lr = 2;
-                    }
-                }
-            }
-            LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
-            if(pLS != NULL)
-            {
-            if(xnear.fdis < 0.00001)
-            {
-                if(bhdgvalid == false)
-                {
-                    xnear.nmode = 0;
-                }
-                else
-                {
-                    double headdiff = hdg - xnear.nearhead;
-                    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
-                    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
-                    if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
-                    {
-                       xnear.nmode = 0;
-                    }
-                    else
-                    {
-                        xnear.nmode = 1;
-                    }
-                }
-
-            }
-            else
-            {
-                if(xnear.fdis<5)
-                {
-                    if(bhdgvalid == false)
-                    {
-                        xnear.nmode = 2;
-                    }
-                    else
-                    {
-                        double headdiff = hdg - xnear.nearhead;
-                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
-                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
-                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
-                        {
-                            xnear.nmode = 2;
-                        }
-                        else
-                        {
-                            xnear.nmode = 3;
-                        }
-                    }
-
-                }
-                else
-                {
-                    if(bhdgvalid == false)
-                    {
-                        xnear.nmode = 4;
-                    }
-                    else
-                    {
-                        double headdiff = hdg - xnear.nearhead;
-                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
-                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
-                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
-                        {
-                            xnear.nmode = 4;
-                        }
-                        else
-                        {
-                            xnear.nmode = 5;
-                        }
-                    }
-                }
-            }
-            if(xnear.nmode < nminmode)nminmode = xnear.nmode;
-            xvectornear.push_back(xnear);
-            }
-        }
-
-    }
-
-    if(xvectornear.size() == 0)return -1;
-
-
-    if(xvectornear.size() > 1)
-    {
-        int i;
-        for(i=0;i<(int)xvectornear.size();i++)
-        {
-            if(xvectornear[i].nmode > nminmode)
-            {
-                xvectornear.erase(xvectornear.begin() + i);
-                i = i-1;
-            }
-        }
-    }
-    if((xvectornear.size()>1)&&(nminmode>=4))  //if dis > 5 select small dis
-    {
-        int i;
-        while(xvectornear.size()>1)
-        {
-            if(xvectornear[1].fdis < xvectornear[0].fdis)
-            {
-                xvectornear.erase(xvectornear.begin());
-            }
-            else
-            {
-                xvectornear.erase(xvectornear.begin()+1);
-            }
-        }
-    }
-    return 0;
-
-}
-
-
-
-/**
-  * @brief SelectRoadLeftRight
-  * 选择左侧道路或右侧道路
-  * 1.选择角度差小于90度的道路一侧，即同侧道路
-  * 2.单行路，选择存在的那一侧道路。
-  * @param pRoad       道路
-  * @param head1       车的航向或目标点的航向
-  * @param head_road   目标点的航向
-  * @retval            1 left   2 right
-**/
-static int SelectRoadLeftRight(Road * pRoad,const double head1,const double  head_road)
-{
-    int nrtn = 2;
-
-
-    double headdiff = head1 - head_road;
-    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
-    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
-
-    bool bSel = false;
-    if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0))  //if diff is
-    {
-        if(pRoad->GetLaneSection(0)->GetLeftLaneCount()>0)
-        {
-
-            nrtn = 1;
-            bSel = true;
-        }
-    }
-    else
-    {
-        if(pRoad->GetLaneSection(0)->GetRightLaneCount()>0)
-        {
-            nrtn = 2;
-            bSel = true;
-        }
-    }
-
-    if(bSel == false)
-    {
-        if(pRoad->GetLaneSection(0)->GetLeftLaneCount() >0)
-        {
-            nrtn = 1;
-        }
-        else
-        {
-            nrtn = 2;
-        }
-    }
-
-    return nrtn;
-
-}
-
-
-
-//static double getlinereldis(GeometryLine * pline,double x,double y)
-//{
-//    double x0,y0;
-//    x0 = pline->GetX();
-//    y0 = pline->GetY();
-//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
-//    if(dis > pline->GetS())
-//    {
-//        std::cout<<"getlinereldis exceed s S is "<<pline->GetS()<<" dis is "<<dis<<std::endl;
-//        return dis;
-//    }
-//    return dis;
-//}
-
-//static double getarcreldis(GeometryArc * parc,double x,double y)
-//{
-//    double x0,y0;
-//    x0 = parc->GetX();
-//    y0 = parc->GetY();
-//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
-//    double R = fabs(1.0/parc->GetCurvature());
-//    double ang = 2.0* asin(dis/(2.0*R));
-//    double frtn = ang * R;
-//    return frtn;
-//}
-
-//static double getparampoly3reldis(GeometryParamPoly3 *parc, double xnow, double ynow)
-//{
-//    double s = 0.1;
-
-//    double xold,yold;
-//    xold = parc->GetX();
-//    yold = parc->GetY();
-
-
-//    while(s < parc->GetLength())
-//    {
-
-//        double x, y,xtem,ytem;
-//        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
-//        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
-//        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
-//        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
-
-//        if(sqrt(pow(xnow - x,2) + pow(ynow -y,2))<0.3)
-//        {
-//            break;
-//        }
-
-//        xold = x;
-//        yold = y;
-//        s = s+ 0.1;
-//    }
-
-//    return s;
-//}
-
-//static double getreldis(RoadGeometry * prg,double x,double y)
-//{
-//    int ngeotype = prg->GetGeomType();
-//    double frtn = 0;
-//    switch (ngeotype) {
-//    case 0:
-//        frtn = getlinereldis((GeometryLine *)prg,x,y);
-//        break;
-//    case 1:
-//        break;
-//    case 2:
-//        frtn = getarcreldis((GeometryArc *)prg,x,y);
-//        break;
-//    case 3:
-//        break;
-//    case 4:
-//        frtn = getparampoly3reldis((GeometryParamPoly3 *)prg,x,y);
-//        break;
-//    default:
-//        break;
-//    }
-
-//    return frtn;
-//}
-
-
-
-
-//static double getposinroad(Road * pRoad,GeometryBlock * pgeob,double x,double y)
-//{
-//    RoadGeometry* prg = pgeob->GetGeometryAt(0);
-//    double s1 = prg->GetS();
-//    double srtn = s1;
-
-//    return s1 + getreldis(prg,x,y);
-//}
-
-
-static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
-{
-    std::vector<PlanPoint> xvectorPP;
-    int i;
-    double s = pline->GetLength();
-    int nsize = s/fspace;
-    if(nsize ==0)nsize = 1;
-
-    for(i=0;i<nsize;i++)
-    {
-        double x,y;
-        x = pline->GetX() + i *fspace * cos(pline->GetHdg());
-        y = pline->GetY() + i *fspace * sin(pline->GetHdg());
-        PlanPoint pp;
-        pp.x = x;
-        pp.y = y;
-        pp.dis = i*fspace;
-        pp.hdg = pline->GetHdg();
-        pp.mS = pline->GetS() + i*fspace;
-        xvectorPP.push_back(pp);
-    }
-    return xvectorPP;
-}
-
-static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
-{
-    std::vector<PlanPoint> xvectorPP;
-
-    //   double fRtn = 1000.0;
-    if(parc->GetCurvature() == 0.0)return xvectorPP;
-
-    double R = fabs(1.0/parc->GetCurvature());
-
-    //calculate arc center
-    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
-    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
-
-    double arcdiff = fspace/R;
-    int nsize = parc->GetLength()/fspace;
-    if(nsize == 0)nsize = 1;
-    int i;
-    for(i=0;i<nsize;i++)
-    {
-        double x,y;
-        PlanPoint pp;
-        if(parc->GetCurvature() > 0)
-        {
-            x = x_center + R * cos(parc->GetHdg() + i*arcdiff - M_PI/2.0);
-            y = y_center + R * sin(parc->GetHdg() + i*arcdiff - M_PI/2.0);
-            pp.hdg = parc->GetHdg() + i*arcdiff;
-        }
-        else
-        {
-            x = x_center + R * cos(parc->GetHdg() -i*arcdiff + M_PI/2.0);
-            y = y_center + R * sin(parc->GetHdg() -i*arcdiff + M_PI/2.0);
-            pp.hdg = parc->GetHdg() - i*arcdiff;
-        }
-
-        pp.x = x;
-        pp.y = y;
-        pp.dis = i*fspace;
-        pp.mS = parc->GetS() + i*fspace;
-        xvectorPP.push_back(pp);
-    }
-    return xvectorPP;
-}
-
-
-static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
-{
-
-    double x,y,hdg;
-    double s = 0.0;
-    double s0 = pspiral->GetS();
-
-    std::vector<PlanPoint> xvectorPP;
-    PlanPoint pp;
-
-    while(s<pspiral->GetLength())
-    {
-        pspiral->GetCoords(s0+s,x,y,hdg);
-
-        pp.x = x;
-        pp.y = y;
-        pp.hdg = hdg;
-        pp.dis = s;
-        pp.mS = pspiral->GetS() + s;
-        xvectorPP.push_back(pp);
-
-        s = s+fspace;
-
-    }
-    return xvectorPP;
-}
-
-static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
-{
-    double x,y;
-    x = ppoly->GetX();
-    y = ppoly->GetY();
-    double A,B,C,D;
-    A = ppoly->GetA();
-    B = ppoly->GetB();
-    C = ppoly->GetC();
-    D = ppoly->GetD();
-    const double steplim = fspace;
-    double du = steplim;
-    double u = 0;
-    double v = 0;
-    double oldx,oldy;
-    oldx = x;
-    oldy = y;
-    double xstart,ystart;
-    xstart = x;
-    ystart = y;
-    double hdgstart = ppoly->GetHdg();
-    double flen = 0;
-    std::vector<PlanPoint> xvectorPP;
-    PlanPoint pp;
-    pp.x = xstart;
-    pp.y = ystart;
-    pp.hdg = hdgstart;
-    pp.dis = 0;
-    pp.mS = ppoly->GetS();
-    xvectorPP.push_back(pp);
-    u = du;
-    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));
-        double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
-        oldx = x;
-        oldy = y;
-        if(fdis>(steplim*2.0))du = du/2.0;
-        flen = flen + fdis;
-        u = u + du;
-
-
-        pp.x = x;
-        pp.y = y;
-        pp.hdg = hdg;
-
- //       s = s+ dt;
-        pp.dis = flen;;
-        pp.mS = ppoly->GetS() + flen;
-        xvectorPP.push_back(pp);
-    }
-
-    return xvectorPP;
-
-}
-
-static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
-{
-    double s = 0.1;
-    std::vector<PlanPoint> xvectorPP;
-    PlanPoint pp;
-
-    double xold,yold;
-    xold = parc->GetX();
-    yold = parc->GetY();
-    double hdg0 = parc->GetHdg();
-    pp.x = xold;
-    pp.y = yold;
-    pp.hdg = hdg0;
-    pp.dis = 0;
-//    xvectorPP.push_back(pp);
-
-    double dt = 1.0;
-    double tcount = parc->GetLength()/0.1;
-    if(tcount > 0)
-    {
-        dt = 1.0/tcount;
-    }
-    double t;
-    s = dt;
-    s = 0;
-
-    double ua,ub,uc,ud,va,vb,vc,vd;
-    ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
-    va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();
-
-    double s_start = parc->GetS();
-    while(s < parc->GetLength())
-    {
-        double x, y,hdg;
-        parc->GetCoords(s_start+s,x,y,hdg);
-        pp.x = x;
-        pp.y = y;
-        pp.hdg = hdg;
-        s = s+ fspace;
-        pp.dis = pp.dis + fspace;;
-        pp.mS = s_start + s;
-        xvectorPP.push_back(pp);
-    }
-    return xvectorPP;
-}
-
-
-std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
-{
-    Road * pRoad = xpath.mpRoad;
-    //s_start  s_end for select now section data.
-    double s_start,s_end;
-    LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
-    s_start = pLS->GetS();
-    if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
-    else
-    {
-        s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
-    }
-
-//    if(xpath.mroadid == 10190)
-//    {
-//        int a = 1;
-//        a++;
-//    }
-
-    std::vector<PlanPoint> xvectorPPS;
-    double s = 0;
-
-    int i;
-    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
-    {
-        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
-        RoadGeometry * prg = pgb->GetGeometryAt(0);
-        std::vector<PlanPoint> xvectorPP;
-        if(i<(pRoad->GetGeometryBlockCount() -0))
-        {
-            if(prg->GetS()>s_end)
-            {
-                continue;
-            }
-            if((prg->GetS() + prg->GetLength())<s_start)
-            {
-                continue;
-            }
-        }
-
-
-        double s1 = prg->GetLength();
-
-        switch (prg->GetGeomType()) {
-        case 0:
-            xvectorPP = getlinepoint((GeometryLine *)prg,fspace);
-
-            break;
-        case 1:
-            xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
-            break;
-        case 2:
-            xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
-            break;
-        case 3:
-
-            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
-            break;
-        case 4:
-            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
-            break;
-        default:
-            break;
-        }
-        int j;
-        if(prg->GetS()<s_start)
-        {
-            s1 = prg->GetLength() -(s_start - prg->GetS());
-        }
-        if((prg->GetS() + prg->GetLength())>s_end)
-        {
-            s1 = s_end - prg->GetS();
-        }
-        for(j=0;j<xvectorPP.size();j++)
-        {
-            PlanPoint pp = xvectorPP.at(j);
-
-            pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);
-
-            if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
-            {
-                if(s_start > prg->GetS())
-                {
-                    pp.dis = s + pp.dis -(s_start - prg->GetS());
-                }
-                else
-                {
-                    pp.dis = s+ pp.dis;
-                }
-                pp.nRoadID = atoi(pRoad->GetRoadId().data());
-                xvectorPPS.push_back(pp);
-            }
-//            if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
-//            {
-//                pp.dis = s + pp.dis;
-//                pp.nRoadID = atoi(pRoad->GetRoadId().data());
-//                xvectorPPS.push_back(pp);
-//            }
-//            else
-//            {
-//                if(prg->GetS()<s_start)
-//                {
-
-//                }
-//                else
-//                {
-//                    if(pp.dis<(s_end -prg->GetS()))
-//                    {
-//                        pp.dis = s + pp.dis;
-//                        pp.nRoadID = atoi(pRoad->GetRoadId().data());
-//                        xvectorPPS.push_back(pp);
-//                    }
-//                }
-//            }
-        }
-        s = s+ s1;
-
-    }
-    return xvectorPPS;
-}
-
-std::vector<PlanPoint> GetPoint(Road * pRoad)
-{
-    std::vector<PlanPoint> xvectorPPS;
-    double s = 0;
-
-    int i;
-    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
-    {
-        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
-        RoadGeometry * prg = pgb->GetGeometryAt(0);
-        std::vector<PlanPoint> xvectorPP;
-        double s1 = prg->GetLength();
-        switch (prg->GetGeomType()) {
-        case 0:
-            xvectorPP = getlinepoint((GeometryLine *)prg);
-            break;
-        case 1:
-            xvectorPP = getspiralpoint((GeometrySpiral *)prg);
-            break;
-        case 2:
-            xvectorPP = getarcpoint((GeometryArc *)prg);
-
-            break;
-        case 3:
-            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
-            break;
-        case 4:
-            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
-            break;
-        default:
-            break;
-        }
-        int j;
-        for(j=0;j<xvectorPP.size();j++)
-        {
-            PlanPoint pp = xvectorPP.at(j);
-            pp.dis = s + pp.dis;
-            pp.nRoadID = atoi(pRoad->GetRoadId().data());
-            xvectorPPS.push_back(pp);
-        }
-        s = s+ s1;
-
-    }
-    return xvectorPPS;
-
-}
-
-int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
-{
-    int nrtn = 0;
-    int i;
-    int dismin = 1000;
-    for(i=0;i<xvectorPP.size();i++)
-    {
-        double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
-        if(dis <dismin)
-        {
-            dismin = dis;
-            nrtn = i;
-        }
-    }
-
-    if(dismin > 10.0)
-    {
-        std::cout<<"indexinroadpoint near point is error. dis is "<<dismin<<std::endl;
-    }
-    return nrtn;
-}
-
-
-double getwidth(Road * pRoad, int nlane)
-{
-    double frtn = 0;
-
-    int i;
-    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
-    for(i=0;i<nlanecount;i++)
-    {
-        if(nlane == pRoad->GetLaneSection(0)->GetLane(i)->GetId())
-        {
-            LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
-            if(pLW != 0)
-            {
-            frtn = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0)->GetA();
-            break;
-            }
-        }
-    }
-    return frtn;
-}
-
-double getoff(Road * pRoad,int nlane)
-{
-    double off = getwidth(pRoad,nlane)/2.0;
-    if(nlane < 0)off = off * (-1.0);
- //   int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
-    int i;
-    if(nlane > 0)
-        off = off + getwidth(pRoad,0)/2.0;
-    else
-        off = off - getwidth(pRoad,0)/2.0;
-    if(nlane > 0)
-    {
-        for(i=1;i<nlane;i++)
-        {
-            off = off + getwidth(pRoad,i);
-        }
-    }
-    else
-    {
-        for(i=-1;i>nlane;i--)
-        {
-            off = off - getwidth(pRoad,i);
-        }
-    }
-//    return 0;
-    return off;
-
-}
-
-
-namespace  iv {
-
-struct lanewidthabcd
-{
-    int nlane;
-    double A;
-    double B;
-    double C;
-    double D;
-};
-}
-
-double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
-{
-    double frtn = 0;
-
-
-    int i;
-    int nlanecount = xvectorlwa.size();
-    for(i=0;i<nlanecount;i++)
-    {
-        if(nlane == xvectorlwa.at(i).nlane)
-        {
-            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
-            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
-            break;
-        }
-    }
-    return frtn;
-}
-
-
-
-std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
-{
-    std::vector<iv::lanewidthabcd> xvectorlwa;
-    if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
-    int i;
-
-
-    LaneSection * pLS = pRoad->GetLaneSection(0);
-
-//    if(pRoad->GetLaneSectionCount() > 1)
-//    {
-//        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
-//        {
-//            if(s>pRoad->GetLaneSection(i+1)->GetS())
-//            {
-//                pLS = pRoad->GetLaneSection(i+1);
-//            }
-//            else
-//            {
-//                break;
-//            }
-//        }
-//    }
-
-    int nlanecount = pLS->GetLaneCount();
-
-    for(i=0;i<nlanecount;i++)
-    {
-        iv::lanewidthabcd xlwa;
-
-
-        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
-        xlwa.nlane = pLS->GetLane(i)->GetId();
-        if(pLW != 0)
-        {
-
-            xlwa.A = pLW->GetA();
-            xlwa.B = pLW->GetB();
-            xlwa.C = pLW->GetC();
-            xlwa.D = pLW->GetD();
-
-        }
-        else
-        {
-            xlwa.A = 0;
-            xlwa.B = 0;
-            xlwa.C = 0;
-            xlwa.D = 0;
-        }
-
- //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
-        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.
-
-    }
-    return xvectorlwa;
-
-}
-
-
-inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
-{
-    int i;
-    double off = 0;
-    double a,b,c,d;
-    if(xvectorIndex.size() == 0)return off;
-
-    for(i=0;i<(xvectorIndex.size()-1);i++)
-    {
-
-        a = xvectorLWA[xvectorIndex[i]].A;
-        b = xvectorLWA[xvectorIndex[i]].B;
-        c = xvectorLWA[xvectorIndex[i]].C;
-        d = xvectorLWA[xvectorIndex[i]].D;
-        if(xvectorLWA[xvectorIndex[i]].nlane != 0)
-        {
-            off = off + a + b*s + c*s*s + d*s*s*s;
-        }
-        else
-        {
-            off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
-        }
-    }
-    i = xvectorIndex.size()-1;
-    a = xvectorLWA[xvectorIndex[i]].A;
-    b = xvectorLWA[xvectorIndex[i]].B;
-    c = xvectorLWA[xvectorIndex[i]].C;
-    d = xvectorLWA[xvectorIndex[i]].D;
-    off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
-    if(nlane < 0) off = off*(-1.0);
-//    std::cout<<"s is "<<s<<std::endl;
-//    std::cout<<" off is "<<off<<std::endl;
-    return off;
-
-}
-
-double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
-{
-    double fwidth = 0;
-    int i;
-    double a,b,c,d;
-
-    int nsize = xvectorLWA.size();
-    for(i=0;i<nsize;i++)
-    {
-        if(nlane*(xvectorLWA[i].nlane)>0)
-        {
-            a = xvectorLWA[i].A;
-            b = xvectorLWA[i].B;
-            c = xvectorLWA[i].C;
-            d = xvectorLWA[i].D;
-            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
-        }
-    }
-    return fwidth;
-
-}
-
-int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
-{
-    if(pRoad->GetLaneSectionCount() < 1)return -1;
-
-    LaneSection * pLS = pRoad->GetLaneSection(0);
-
-    int i;
-
-    if(pRoad->GetLaneSectionCount() > 1)
-    {
-        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
-        {
-            if(s>pRoad->GetLaneSection(i+1)->GetS())
-            {
-                pLS = pRoad->GetLaneSection(i+1);
-            }
-            else
-            {
-                break;
-            }
-        }
-    }
-
-    nori = 0;
-    ntotal = 0;
-    fSpeed = -1; //if -1 no speedlimit for map
-
-    pRoad->GetRoadSpeedMax(s,fSpeed);   //Get Road Speed Limit.
-
-    pRoad->GetRoadNoavoid(s,bNoavoid);
-
-    int nlanecount = pLS->GetLaneCount();
-    for(i=0;i<nlanecount;i++)
-    {
-        int nlaneid = pLS->GetLane(i)->GetId();
-
-        if(nlaneid == nlane)
-        {
-            Lane * pLane = pLS->GetLane(i);
-            if(pLane->GetLaneSpeedCount() > 0)
-            {
-                LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
-                int j;
-                int nspeedcount = pLane->GetLaneSpeedCount();
-                for(j=0;j<(nspeedcount -1);j++)
-                {
-                    if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
-                    {
-                        pLSpeed = pLane->GetLaneSpeed(j+1);
-                    }
-                    else
-                    {
-                        break;
-                    }
-                }
-                if(pLSpeed->GetS()<=(s-pLS->GetS()))fSpeed = pLSpeed->GetMax();
-
-            }
-        }
-        if(nlane<0)
-        {
-
-            if(nlaneid < 0)
-            {
-                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
-                {
-                    ntotal++;
-                    if(nlaneid<nlane)nori++;
-                }
-            }
-
-
-        }
-        else
-        {
-            if(nlaneid > 0)
-            {
-                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
-                {
-                    ntotal++;
-                    if(nlaneid > nlane)nori++;
-                }
-            }
-        }
-    }
-
-    return 0;
-}
-
-std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
-{
-    std::vector<int> xvectorindex;
-    int i;
-    if(nlane >= 0)
-    {
-    for(i=0;i<=nlane;i++)
-    {
-       int j;
-       int nsize = xvectorLWA.size();
-       for(j=0;j<nsize;j++)
-       {
-           if(i == xvectorLWA.at(j).nlane )
-           {
-               xvectorindex.push_back(j);
-               break;
-           }
-       }
-    }
-    }
-    else
-    {
-        for(i=0;i>=nlane;i--)
-        {
-           int j;
-           int nsize = xvectorLWA.size();
-           for(j=0;j<nsize;j++)
-           {
-               if(i == xvectorLWA.at(j).nlane )
-               {
-                   xvectorindex.push_back(j);
-                   break;
-               }
-           }
-        }
-    }
-    return xvectorindex;
-}
-
-void CalcBorringRoad(pathsection xps,std::vector<PlanPoint> & xvectorPP)
-{
-    if(xps.mpRoad->GetRoadBorrowCount() == 0)
-    {
-        return;
-    }
-
-    Road * pRoad = xps.mpRoad;
-    unsigned int nborrowsize = pRoad->GetRoadBorrowCount();
-    unsigned int i;
-    unsigned int nPPCount = xvectorPP.size();
-    for(i=0;i<nborrowsize;i++)
-    {
-        RoadBorrow * pborrow = pRoad->GetRoadBorrow(i);
-        if(pborrow == NULL)
-        {
-            std::cout<<"warning:can't get borrow"<<std::endl;
-            continue;
-        }
-        if((pborrow->GetMode() == "All")||((pborrow->GetMode()=="R2L")&&(xps.mainsel<0))||((pborrow->GetMode()=="L2R")&&(xps.mainsel>0)))
-        {
-            unsigned int j;
-            double soffset = pborrow->GetS();
-            double borrowlen = pborrow->GetLength();
-            for(j=0;j<xvectorPP.size();j++)
-            {
-                if((xvectorPP[j].mS>=soffset)&&(xvectorPP[j].mS<=(soffset + borrowlen)))
-                {
-                    xvectorPP[j].mbBoringRoad = true;
-                }
-            }
-        }
-    }
-}
-
-void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const double fvehiclewidth,
-                     const int nchang1,const int nchang2,const int nchangpoint)
-{
-    if(xvectorPP.size()<2)return;
-    bool bInlaneavoid = false;
-    int i;
-    if((xps.mbStartToMainChange == false) && (xps.mbMainToEndChange == false))
-    {
-        if(xps.mpRoad->GetRoadLength()<30)
-        {
-            double hdgdiff = xvectorPP.at(xvectorPP.size()-1).hdg - xvectorPP.at(0).hdg;
-            if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
-            if(hdgdiff>(M_PI/3.0)&&(hdgdiff<(5.0*M_PI/3.0)))
-                bInlaneavoid = false;
-            else
-                bInlaneavoid = true;
-        }
-        else
-        {
-            bInlaneavoid = true;
-        }
-    }
-    else
-    {
-        if(xps.mpRoad->GetRoadLength()>100)bInlaneavoid = true;
-    }
-
-    if(bInlaneavoid == false)
-    {
-        int nvpsize = xvectorPP.size();
-        for(i=0;i<nvpsize;i++)
-        {
-            xvectorPP.at(i).bInlaneAvoid = false;
-            xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
-            xvectorPP.at(i).my_left = xvectorPP.at(i).y;
-            xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
-            xvectorPP.at(i).my_right = xvectorPP.at(i).y;
-        }
-    }
-    else
-    {
-      int nvpsize = xvectorPP.size();
-      for(i=0;i<nvpsize;i++)xvectorPP.at(i).bInlaneAvoid = true;
-      if((xps.mbStartToMainChange == true)||(xps.mbMainToEndChange == true))
-      {
-          if(xps.mbStartToMainChange == true)
-          {
-              for(i=(nchang1 - nchangpoint/2);i<(nchang1 + nchangpoint/2);i++)
-              {
-                  if((i>=0)&&(i<nvpsize))
-                    xvectorPP.at(i).bInlaneAvoid = false;
-              }
-
-          }
-          if(xps.mbMainToEndChange)
-          {
-              for(i=(nchang2 - nchangpoint/2);i<(nchang2 + nchangpoint/2);i++)
-              {
-                  if((i>=0)&&(i<nvpsize))
-                    xvectorPP.at(i).bInlaneAvoid = false;
-              }
-          }
-      }
-
-      for(i=0;i<nvpsize;i++)
-      {
-          if(xvectorPP.at(i).bInlaneAvoid)
-          {
-              double foff = xvectorPP.at(i).mfDisToLaneLeft -0.3-fvehiclewidth*0.5; //30cm + 0.5vehcilewidth
-              if(foff < 0.3)
-              {
-                  xvectorPP.at(i).bInlaneAvoid = false;
-                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
-                  xvectorPP.at(i).my_left = xvectorPP.at(i).y;
-                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
-                  xvectorPP.at(i).my_right = xvectorPP.at(i).y;
-              }
-              else
-              {
-                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg + M_PI/2.0);
-                  xvectorPP.at(i).my_left = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg + M_PI/2.0);
-                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg - M_PI/2.0);
-                  xvectorPP.at(i).my_right = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg - M_PI/2.0);
-              }
-          }
-      }
-    }
-
-}
-
-
-double getoff(Road * pRoad,int nlane,const double s)
-{
-    int i;
-    int nLSCount = pRoad->GetLaneSectionCount();
-    double s_section = 0;
-
-    double foff = 0;
-
-    for(i=0;i<nLSCount;i++)
-    {
-
-        LaneSection * pLS = pRoad->GetLaneSection(i);
-        if(i<(nLSCount -1))
-        {
-            if(pRoad->GetLaneSection(i+1)->GetS()<s)
-            {
-                continue;
-            }
-        }
-        s_section = pLS->GetS();
-        int nlanecount = pLS->GetLaneCount();
-        int j;
-        for(j=0;j<nlanecount;j++)
-        {
-            Lane * pLane = pLS->GetLane(j);
-
-            int k;
-            double s_lane = s-s_section;
-
-
-            if(pLane->GetId() != 0)
-            {
-
-                for(k=0;k<pLane->GetLaneWidthCount();k++)
-                {
-                    if(k<(pLane->GetLaneWidthCount()-1))
-                    {
-                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
-                        {
-                            continue;
-                        }
-                    }
-                    s_lane = pLane->GetLaneWidth(k)->GetS();
-                    break;
-                }
-                LaneWidth * pLW  = pLane->GetLaneWidth(k);
-                if(pLW == 0)
-                {
-                    std::cout<<"not find LaneWidth"<<std::endl;
-                    break;
-                }
-
-                double fds = s - s_lane - s_section;
-                double fwidth= pLW->GetA() + pLW->GetB() * fds
-                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
-
-//                if((pRoad->GetRoadId() == "211210") &&(nlane == -1) &&(pLane->GetId() == -1))
-//                {
-//                    qDebug("fs is %f width is %f",fds,fwidth);
-//                }
-                if(nlane == pLane->GetId())
-                {
-                    foff = foff + fwidth/2.0;
-                }
-                if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
-                {
-                    foff = foff + fwidth;
-                }
-
-            }
-
-        }
-
-
-        break;
-
-    }
-
-    if(pRoad->GetLaneOffsetCount()>0)
-    {
-
-        int nLOCount = pRoad->GetLaneOffsetCount();
-        int isel = -1;
-        for(i=0;i<(nLOCount-1);i++)
-        {
-            if(pRoad->GetLaneOffset(i+1)->GetS()>s)
-            {
-                isel = i;
-                break;
-            }
-        }
-        if(isel < 0)isel = nLOCount-1;
-        LaneOffset * pLO = pRoad->GetLaneOffset(isel);
-        double s_off = s - pLO->GetS();
-        double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
-                +pLO->Getd() * s_off * s_off * s_off;
-        foff = foff - off1;
-    }
-
-    if(nlane<0)foff = foff*(-1);
-    return foff;
-}
-
-
-std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
-{
-    std::vector<PlanPoint> xvectorPP;
-
-    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(xps.mpRoad);
-    std::vector<int> xvectorindex1,xvectorindex2,xvectorindex3;
-    int nchange1,nchange2;
-    int nlane1,nlane2,nlane3;
-    if(xps.mnStartLaneSel == xps.mnEndLaneSel)
-    {
-        xps.mainsel = xps.mnStartLaneSel;
-        xps.mbStartToMainChange = false;
-        xps.mbMainToEndChange = false;
-    }
-    else
-    {
-        if(xps.mpRoad->GetRoadLength() < 100)
-        {
-            xps.mainsel = xps.mnEndLaneSel;
-            xps.mbStartToMainChange = true;
-            xps.mbMainToEndChange = false;
-        }
-    }
-
-    double off1,off2,off3;
-    if(xps.mnStartLaneSel < 0)
-    {
-    off1 = getoff(xps.mpRoad,xps.mnStartLaneSel);
-    off2 = getoff(xps.mpRoad,xps.mainsel);
-    off3 = getoff(xps.mpRoad,xps.mnEndLaneSel);
-    xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
-    xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
-    xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
-    nlane1 = xps.mnStartLaneSel;
-    nlane2 = xps.mainsel;
-    nlane3 = xps.mnEndLaneSel;
-    }
-    else
-    {
-        off3 = getoff(xps.mpRoad,xps.mnStartLaneSel);
-        off2 = getoff(xps.mpRoad,xps.mainsel);
-        off1 = getoff(xps.mpRoad,xps.mnEndLaneSel);
-        xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
-        xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
-        xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
-        nlane3 = xps.mnStartLaneSel;
-        nlane2 = xps.mainsel;
-        nlane1 = xps.mnEndLaneSel;
-    }
-
-    int nchangepoint = 300;
-    if((nchangepoint * 3) > xvPP.size())
-    {
-        nchangepoint = xvPP.size()/3;
-    }
-    int nsize = xvPP.size();
-
-    nchange1 = nsize/3;
-    if(nchange1>500)nchange1 = 500;
-    nchange2 = nsize*2/3;
-    if( (nsize-nchange2)>500)nchange2 = nsize-500;
-//    if(nsize < 1000)
-//    {
-//        std::cout<<"GetLanePoint Error. road id is "<<xps.mpRoad->GetRoadId()<<std::endl;
-//        return xvectorPP;
-//    }
-    double x,y;
-    int i;
-    if(xps.mainsel < 0)
-    {
-
-        for(i=0;i<nsize;i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-
-            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
-            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
-
-            pp.nlrchange = 1;
-
-            if(xps.mainsel != xps.secondsel)
-            {
-                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
-                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
-                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
-
-                if(xps.mainsel >xps.secondsel)
-                {
-                    pp.nlrchange = 1;
-                }
-                else
-                {
-                    pp.nlrchange = 2;
-                }
-            }
-            else
-            {
-                pp.mfSecx = pp.x ;
-                pp.mfSecy = pp.y ;
-            }
-
-            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off1*(-1);
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-            xvectorPP.push_back(pp);
-        }
-
-
-    }
-    else
-    {
-
-        for(i=0;i<nsize;i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-
-
-            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
-            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
-
-            pp.nlrchange = 1;
-
-            if(xps.mainsel != xps.secondsel)
-            {
-                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
-                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
-                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
-
-                if(xps.mainsel >xps.secondsel)
-                {
-                    pp.nlrchange = 2;
-                }
-                else
-                {
-                    pp.nlrchange = 1;
-                }
-            }
-            else
-            {
-                pp.mfSecx = pp.x ;
-                pp.mfSecy = pp.y ;
-            }
-
-            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off1;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-            pp.hdg = pp.hdg + M_PI;
-            xvectorPP.push_back(pp);
-        }
-
-//        for(i=0;i<(nchange1- nchangepoint/2);i++)
-//        {
-//            PlanPoint pp = xvPP.at(i);
-//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-//            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off1;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
-//            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
-//        {
-
-//            PlanPoint pp = xvPP.at(i);
-//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
-//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-
-//            pp.mfDisToRoadLeft = offx;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
-//            if(nlane1 == nlane2)
-//            {
-//                pp.mWidth = flanewidth1;
-//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//                pp.lanmp = 0;
-//                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//            }
-//            else
-//            {
-//                if(nlane1 < nlane2)
-//                {
-//                    pp.lanmp = -1;
-//                }
-//                else
-//                {
-//                    pp.lanmp = 1;
-//                }
-
-//                if(i<nchange1)
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
-//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-//                else
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
-//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-
-//            }
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
-//        {
-//            PlanPoint pp = xvPP.at(i);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off2;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-//            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
-//        {
-
-//            PlanPoint pp = xvPP.at(i);
-//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-//            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
-//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-
-//            pp.mfDisToRoadLeft = offx;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-//            if(nlane2 == nlane3)
-//            {
-//                pp.mWidth = flanewidth1;
-//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//                pp.lanmp = 0;
-//                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//            }
-//            else
-//            {
-//                if(nlane2 < nlane3)
-//                {
-//                    pp.lanmp = -1;
-//                }
-//                else
-//                {
-//                    pp.lanmp = 1;
-//                }
-
-//                if(i<nchange2)
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
-//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-//                else
-//                {
-//                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-//                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
-//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-//                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-//                }
-
-//            }
-
-//            xvectorPP.push_back(pp);
-
-//        }
-//        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
-//        {
-//            PlanPoint pp = xvPP.at(i);
-//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-//            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
-//            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
-//            pp.hdg = pp.hdg + M_PI;
-
-//            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-//            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off3;
-//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
-//            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-//            xvectorPP.push_back(pp);
-
-//        }
-
-    }
-
-
-
-    CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,nchange1,nchange2,nchangepoint);
-
-    if(xps.mpRoad->GetRoadBorrowCount()>0)
-    {
-        CalcBorringRoad(xps,xvectorPP);
-    }
-
-
-    if(xps.mnStartLaneSel > 0)
-    {
-        std::vector<PlanPoint> xvvPP;
-        int nvsize = xvectorPP.size();
-        for(i=0;i<nvsize;i++)
-        {
-            xvvPP.push_back(xvectorPP.at(nvsize-1-i));
-        }
-        AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
-        return xvvPP;
-    }
-
-//    pRoad->GetLaneSection(xps.msectionid)
-
-    AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
-    return xvectorPP;
-}
-
-
-static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
-{
-    if(pRoad->GetSignalCount() == 0)return;
-    int nsigcount = pRoad->GetSignalCount();
-    int i;
-    for(i=0;i<nsigcount;i++)
-    {
-        Signal * pSig = pRoad->GetSignal(i);
-        int nfromlane = -100;
-        int ntolane = 100;
-        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
-        if(pSig_laneValidity != 0)
-        {
-            nfromlane = pSig_laneValidity->GetfromLane();
-            ntolane = pSig_laneValidity->GettoLane();
-        }
-        if((nlane < 0)&&(nfromlane >= 0))
-        {
-            continue;
-        }
-        if((nlane > 0)&&(ntolane<=0))
-        {
-            continue;
-        }
-
-        double s = pSig->Gets();
-        double fpos = s/pRoad->GetRoadLength();
-        if(nlane > 0)fpos = 1.0 -fpos;
-        int npos = fpos *xvectorPP.size();
-        if(npos <0)npos = 0;
-        if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
-        while(xvectorPP.at(npos).nSignal != -1)
-        {
-            if(npos > 0)npos--;
-            else break;
-        }
-        while(xvectorPP.at(npos).nSignal != -1)
-        {
-            if(npos < (xvectorPP.size()-1))npos++;
-            else break;
-        }
-        xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());
-
-
-    }
-}
-
-static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
-                                          Road * pRoad_obj,GeometryBlock * pgeob_obj,
-                                           const double x_now,const double y_now,const double head,
-                                           double nearx,double neary, double nearhead,
-                                           double nearx_obj,double  neary_obj,const double fvehiclewidth,const double flen = 1000)
-{
-
-  std::vector<PlanPoint> xvectorPPs;
-
-  double fspace = 0.1;
-
-  if(flen<2000)fspace = 0.1;
-  else
-  {
-      if(flen<5000)fspace = 0.3;
-      else
-      {
-          if(flen<10000)fspace = 0.5;
-          else
-              fspace = 1.0;
-      }
-  }
-
-  int i;
-
-
-//  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
-  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);
-
-
-  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
-
-  std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);
-
-  if(xpathsection[0].mainsel < 0)
-  {
-  for(i=indexstart;i<xvPP.size();i++)
-  {
-      xvectorPPs.push_back(xvPP.at(i));
-  }
-  }
-  else
-  {
-
-      for(i=(xvPP.size() -indexstart);i<xvPP.size();i++)
-      {
-          PlanPoint pp;
-          pp = xvPP.at(i);
-//          pp.hdg = pp.hdg +M_PI;
-          xvectorPPs.push_back(pp);
-      }
-  }
-
-  int npathlast = xpathsection.size() - 1;
-//  while(npathlast >= 0)
-//  {
-//    if(npathlast == 0)break;
-//    if(xpathsection[npathlast].mpRoad != xpathsection[npathlast - 1].mpRoad)break;
-//  }
-  for(i=1;i<(npathlast);i++)
-  {
-//      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
-//      {
-//          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
-//          {
-//            continue;
-//          }
-//      }
-//      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
-//      {
-//          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
-//          {
-//            break;
-//          }
-//      }
- //     xvectorPP = GetPoint( xpathsection[i].mpRoad);
-      xvectorPP = GetPoint( xpathsection[i],fspace);
-      xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
-//      std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
-//              <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
-//      std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
-      int j;
-      for(j=0;j<xvPP.size();j++)
-      {
-
-          PlanPoint pp;
-          pp = xvPP.at(j);
-//          pp.hdg = pp.hdg +M_PI;
-          xvectorPPs.push_back(pp);
-      }
-  }
-
-  xvectorPP = GetPoint(xpathsection[npathlast],fspace);
-
-
-//  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
-  int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
-  xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
-  int nlastsize;
-  if(xpathsection[npathlast].mainsel<0)
-  {
-      nlastsize = indexend;
-  }
-  else
-  {
-      if(indexend>0) nlastsize = xvPP.size() - indexend;
-      else nlastsize = xvPP.size();
-  }
-  for(i=0;i<nlastsize;i++)
-  {
-      xvectorPPs.push_back(xvPP.at(i));
-
-  }
-
-  //Find StartPoint
-//  if
-
-//  int a = 1;
-
-
-
-
-  return xvectorPPs;
-}
-
-std::vector<PlanPoint> gTempPP;
-int getPoint(double q[3], double x, void* user_data) {
-//    printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
-    PlanPoint pp;
-    pp.x = q[0];
-    pp.y = q[1];
-    pp.hdg = q[2];
-    pp.dis = x;
-    pp.speed = *((double *)user_data);
-    gTempPP.push_back(pp);
-//    std::cout<<pp.x<<" "<<" "<<pp.y<<" "<<pp.hdg<<std::endl;
-    return 0;
-}
-
-/**
- * @brief getlanesel
- * @param nSel
- * @param pLaneSection
- * @param nlr
- * @return
- */
-int getlanesel(int nSel,LaneSection * pLaneSection,int nlr)
-{
-    int nlane = nSel;
-    int mainselindex = 0;
-    if(nlr == 2)nlane = nlane*(-1);
-    int nlanecount = pLaneSection->GetLaneCount();
-
-    int i;
-    int nTrueSel = nSel;
-    if(nTrueSel <= 1)nTrueSel= 1;
-    int nCurSel = 1;
-    if(nlr ==  2)nCurSel = nCurSel *(-1);
-    bool bOK = false;
-    int nxsel = 1;
-    int nlasttid = 0;
-    bool bfindlast = false;
-    while(bOK == false)
-    {
-        bool bHaveDriving = false;
-        int ncc = 0;
-        for(i=0;i<nlanecount;i++)
-        {
-            Lane * pLane = pLaneSection->GetLane(i);
-            if(strncmp(pLane->GetType().data(),"driving",255) == 0 )
-            {
-                if((nlr == 1)&&(pLane->GetId()>0))
-                {
-                    ncc++;
-                }
-                if((nlr == 2)&&(pLane->GetId()<0))
-                {
-                    ncc++;
-                }
-                if(ncc == nxsel)
-                {
-                    bHaveDriving = true;
-                    bfindlast = true;
-                    nlasttid = pLane->GetId();
-                    break;
-                }
-            }
-        }
-        if(bHaveDriving == true)
-        {
-            if(nxsel == nTrueSel)
-            {
-                return nlasttid;
-            }
-            else
-            {
-                nxsel++;
-            }
-        }
-        else
-        {
-            if(bfindlast)
-            {
-                return nlasttid;
-            }
-            else
-            {
-                std::cout<<"can't find lane."<<std::endl;
-                return 0;
-            }
-            //Use Last
-        }
-
-    }
-
-    return 0;
-
-    int k;
-    bool bFind = false;
-    while(bFind == false)
-    {
-        for(k=0;k<nlanecount;k++)
-        {
-            Lane * pLane = pLaneSection->GetLane(k);
-            if((nlane == pLane->GetId())&&(strncmp(pLane->GetType().data(),"driving",255) == 0))
-            {
-                bFind = true;
-                mainselindex = k;
-                break;
-            }
-        }
-        if(bFind)continue;
-        if(nlr == 1)nlane--;
-        else nlane++;
-        if(nlane == 0)
-        {
-            std::cout<<" Fail. can't find lane"<<std::endl;
-            break;
-        }
-    }
-    return nlane;
-}
-
-
-
-void checktrace(std::vector<PlanPoint> & xPlan)
-{
-    int i;
-    int nsize = xPlan.size();
-    for(i=1;i<nsize;i++)
-    {
-        double dis = sqrt(pow(xPlan.at(i).x - xPlan.at(i-1).x,2) + pow(xPlan.at(i).y - xPlan.at(i-1).y,2));
-        if(dis > 0.3)
-        {
-            double hdg =  CalcHdg(QPointF(xPlan.at(i-1).x,xPlan.at(i-1).y),QPointF(xPlan.at(i).x,xPlan.at(i).y));
-
-            int ncount = dis/0.1;
-            int j;
-            for(j=1;j<ncount;j++)
-            {
-                double x, y;
-
-                PlanPoint pp;
-                pp.x = xPlan.at(i-1).x + j*0.1 *cos(hdg);
-                pp.y = xPlan.at(i-1).y + j*0.1 *sin(hdg);
-                pp.hdg = hdg;
-                xPlan.insert(xPlan.begin()+i+j-1,pp);
-
-            }
-            qDebug("dis is %f",dis);
-        }
-    }
-}
-
-
-
-void ChangeLane(std::vector<PlanPoint> & xvectorPP)
-{
-    int i = 0;
-    int nsize = xvectorPP.size();
-    for(i=0;i<nsize;i++)
-    {
-        if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
-        {
-
-        }
-        else
-        {
-            int k;
-            for(k=i;k<(nsize-1);k++)
-            {
-                if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
-                {
-                    break;
-                }
-            }
-            int nnum = k-i;
-            int nchangepoint = 300;
-            double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
-                                   +pow(xvectorPP[k].y - xvectorPP[i].y,2));
-            const double fMAXCHANGE = 100;
-            if(froadlen<fMAXCHANGE)
-            {
-                nchangepoint = nnum;
-            }
-            else
-            {
-                nchangepoint = (fMAXCHANGE/froadlen) * nnum;
-            }
-
-            qDebug(" road %d len is %f changepoint is %d nnum is %d",
-                   xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);
-
-            int nstart = i + nnum/2 -nchangepoint/2;
-            int nend = i+nnum/2 + nchangepoint/2;
-            if(nnum<300)
-            {
-                nstart = i;
-                nend = k;
-            }
-            int j;
-            for(j=i;j<nstart;j++)
-            {
-                xvectorPP[j].x = xvectorPP[j].mfSecx;
-                xvectorPP[j].y = xvectorPP[j].mfSecy;
-            }
-            nnum = nend - nstart;
-            for(j=nstart;j<nend;j++)
-            {
-                double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
-                                   +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
-                double foff = fdis *(j-nstart)/nnum;
-                if(xvectorPP[j].nlrchange == 1)
-                {
-//                    std::cout<<"foff is "<<foff<<std::endl;
-                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
-                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
-                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft + fdis - foff;
-                }
-                else
-                {
-                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
-                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
-                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  - fdis +foff;
-                }
-            }
-            i =k;
-
-        }
-    }
-}
-
-#include <QFile>
-
-/**
- * @brief MakePlan         全局路径规划
- * @param pxd              有向图
- * @param pxodr            OpenDrive地图
- * @param x_now            当前x坐标
- * @param y_now            当前y坐标
- * @param head             当前航向
- * @param x_obj            目标点x坐标
- * @param y_obj            目标点y坐标
- * @param obj_dis          目标点到路径的距离
- * @param srcnearthresh    当前点离最近路径的阈值，如果不在这个范围，寻找失败
- * @param dstnearthresh    目标点离最近路径的阈值，如果不在这个范围，寻找失败
- * @param nlanesel         车道偏好，1为内车道，数值越大越偏外，如果数值不满足，会选择最靠外的。
- * @param xPlan            返回的轨迹点
- * @return                 0 成功  <0 失败
- */
-int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
-             const double x_obj,const double y_obj,const double & obj_dis,
-             const double srcnearthresh,const double dstnearthresh,
-             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth )
-{
-    double  s;double nearx;
-    double  neary;double  nearhead;
-    Road * pRoad;GeometryBlock * pgeob;
-    Road * pRoad_obj;GeometryBlock * pgeob_obj;
-    double  s_obj;double nearx_obj;
-    double  neary_obj;double  nearhead_obj;
-    int lr_end = 2;
-    int lr_start = 2;
-    double fs1,fs2;
-//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
-
-    std::vector<iv::nearroad> xvectornearstart;
-    std::vector<iv::nearroad> xvectornearend;
-
-    GetNearPointWithHead(x_now,y_now,head,pxodr,xvectornearstart,srcnearthresh,true);
-    GetNearPointWithHead(x_obj,y_obj,0,pxodr,xvectornearend,dstnearthresh,false);
-
-    if(xvectornearstart.size()<1)
-    {
-        std::cout<<" plan fail. can't find start point."<<std::endl;
-        return -1;
-    }
-    if(xvectornearend.size() < 1)
-    {
-        std::cout<<" plan fail. can't find end point."<<std::endl;
-        return -2;
-    }
-
-    std::vector<std::vector<PlanPoint>> xvectorplans;
-    std::vector<double> xvectorroutedis;
-
-    int i;
-    int j;
-    for(i=0;i<(int)xvectornearstart.size();i++)
-    {
-        for(j=0;j<(int)xvectornearend.size();j++)
-        {
-            iv::nearroad * pnearstart = &xvectornearstart[i];
-            iv::nearroad * pnearend = &xvectornearend[j];
-            pRoad = pnearstart->pObjRoad;
-            pgeob = pnearstart->pgeob;
-            pRoad_obj = pnearend->pObjRoad;
-            pgeob_obj = pnearend->pgeob;
-            lr_start = pnearstart->lr;
-            lr_end = pnearend->lr;
-
-            nearx = pnearstart->nearx;
-            neary = pnearstart->neary;
-
-            nearx_obj = pnearend->nearx;
-            neary_obj = pnearend->neary;
-
-            bool bNeedDikstra = true;
-            if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
-            {
-                std::vector<PlanPoint> xvPP = GetPoint(pRoad);
-                int nindexstart = indexinroadpoint(xvPP,nearx,neary);
-                int nindexend =  indexinroadpoint(xvPP,nearx_obj,neary_obj);
-                int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
-                AddSignalMark(pRoad,nlane,xvPP);
-                if((nindexend >nindexstart)&&(lr_start == 2))
-                {
-                    bNeedDikstra = false;
-                }
-                if((nindexend <nindexstart)&&(lr_start == 1))
-                {
-                    bNeedDikstra = false;
-                }
-                if(bNeedDikstra == false)
-                {
-
-                    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
-                    std::vector<int> xvectorindex1;
-
-                    xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
-
-                    double foff = getoff(pRoad,nlane);
-
-                    foff = foff + 0.0;
-                    std::vector<PlanPoint> xvectorPP;
-                    int i = nindexstart;
-                    while(i!=nindexend)
-                    {
-
-                        if(lr_start == 2)
-                        {
-                            PlanPoint pp = xvPP.at(i);
-                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
-                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
-                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
-                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
-                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                            pp.lanmp = 0;
-                            pp.mfDisToRoadLeft = foff *(-1.0);
-                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
-                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-                            xvectorPP.push_back(pp);
-                            i++;
-                        }
-                        else
-                        {
-                            PlanPoint pp = xvPP.at(i);
-                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
-                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
-                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
-                            pp.hdg = pp.hdg + M_PI;
-
-                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
-                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                            pp.lanmp = 0;
-                            pp.mfDisToRoadLeft = foff;
-                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
-                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
-
-
-                            xvectorPP.push_back(pp);
-                            i--;
-                        }
-                    }
-
-                    for(i=0;i<(int)xvectorPP.size();i++)
-                    {
-                        xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
-                    }
-
-                    pathsection xps;
-                    xps.mbStartToMainChange = false;
-                    xps.mbMainToEndChange = false;
-         //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
-                    xPlan = xvectorPP;
-
-                    xvectorplans.push_back(xvectorPP);
-                    xvectorroutedis.push_back(xvectorPP.size() * 0.1);
-
-                }
-            }
-
-            if(bNeedDikstra)
-            {
-                bool bSuc = false;
-            std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
-            if(xpath.size()<1)
-            {
-                continue;
-            }
-            if(bSuc == false)
-            {
-                continue;
-            }
-            double flen = pxd->getpathlength(xpath);
-            std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
-
-            std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
-                         head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
-
-            ChangeLane(xvectorPP);
-            xvectorplans.push_back(xvectorPP);
-            xvectorroutedis.push_back(flen);
-
-            }
-
-        }
-    }
-
-    if(xvectorplans.size() > 0)
-    {
-        if(xvectorplans.size() == 1)
-        {
-            xPlan = xvectorplans[0];
-        }
-        else
-        {
-            int nsel = 0;
-            double fdismin = xvectorroutedis[0];
-            for(i=1;i<(int)xvectorroutedis.size();i++)
-            {
-                if(xvectorroutedis[i]<fdismin)
-                {
-                    nsel = i;
-                }
-            }
-            xPlan = xvectorplans[nsel];
-        }
-        return 0;
-    }
-    std::cout<<" plan fail. can't find path."<<std::endl;
-    return -1000;
-
-}
+#include "globalplan.h"
+#include "limits"
+#include <iostream>
+
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+#include <Eigen/LU>
+#include <Eigen/QR>
+#include <Eigen/SVD>
+
+#include <QDebug>
+#include <QPointF>
+
+
+using namespace  Eigen;
+
+extern "C"
+{
+#include "dubins.h"
+}
+
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP);
+/**
+ * @brief GetLineDis 获得点到直线Geometry的距离。
+ * @param pline
+ * @param x
+ * @param y
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+static double GetLineDis(GeometryLine * pline,const double x,const double y,double & nearx,
+                         double & neary,double & nearhead)
+{
+    double fRtn = 1000.0;
+
+    double a1,a2,a3,a4,b1,b2;
+    double ratio = pline->GetHdg();
+    while(ratio >= 2.0* M_PI)ratio = ratio-2.0*M_PI;
+    while(ratio<0)ratio = ratio+2.0*M_PI;
+
+    double dis1,dis2,dis3;
+    double x1,x2,x3,y1,y2,y3;
+    x1 = pline->GetX();y1=pline->GetY();
+    if((ratio == 0)||(ratio == M_PI))
+    {
+        a1 = 0;a4=0;
+        a2 = 1;b1= pline->GetY();
+        a3 = 1;b2= x;
+    }
+    else
+    {
+        if((ratio == 0.5*M_PI)||(ratio == 1.5*M_PI))
+        {
+            a2=0;a3=0;
+            a1=1,b1=pline->GetX();
+            a4 = 1;b2 = y;
+        }
+        else
+        {
+            a1 = tan(ratio) *(-1.0);
+            a2 = 1;
+            a3 = tan(ratio+M_PI/2.0)*(-1.0);
+            a4 = 1;
+            b1 = a1*pline->GetX() + a2 * pline->GetY();
+            b2 = a3*x+a4*y;
+        }
+    }
+
+    y2 = y1 + pline->GetLength() * sin(ratio);
+    x2 = x1 + pline->GetLength() * cos(ratio);
+
+    Eigen::Matrix2d A;
+    A<<a1,a2,
+            a3,a4;
+    Eigen::Vector2d B(b1,b2);
+
+    Eigen::Vector2d opoint  = A.lu().solve(B);
+
+ //   std::cout<<opoint<<std::endl;
+
+    x3 = opoint(0);
+    y3 = opoint(1);
+
+    dis1 = sqrt(pow(x1-x,2)+pow(y1-y,2));
+    dis2 = sqrt(pow(x2-x,2)+pow(y2-y,2));
+    dis3 = sqrt(pow(x3-x,2)+pow(y3-y,2));
+
+//    std::cout<<" dis 1 is "<<dis1<<std::endl;
+//    std::cout<<" dis 2 is "<<dis2<<std::endl;
+//    std::cout<<" dis 3 is "<<dis3<<std::endl;
+
+    if((dis1>pline->GetLength())||(dis2>pline->GetLength()))  //Outoff line
+    {
+ //       std::cout<<" out line"<<std::endl;
+        if(dis1<dis2)
+        {
+            fRtn = dis1;
+            nearx = x1;neary=y1;nearhead = pline->GetHdg();
+        }
+        else
+        {
+            fRtn = dis2;
+            nearx = x2;neary=y2;nearhead = pline->GetHdg();
+        }
+    }
+    else
+    {
+        fRtn = dis3;
+        nearx = x3;neary=y3;nearhead = pline->GetHdg();
+    }
+
+
+//    std::cout<<"dis is "<<fRtn<<" x is "<<nearx<<" y is "<<neary<<" head is "<<nearhead<<std::endl;
+    return fRtn;
+
+
+
+}
+
+
+static int getcirclecrosspoint(QPointF startPos, QPointF endPos, QPointF agvPos, double R,QPointF & point1,QPointF & point2 )
+{
+     double m=0;
+     double k;
+     double b;
+
+      //计算分子
+      m=startPos.x()-endPos.x();
+
+      //求直线的方程
+      if(0==m)
+      {
+          k=100000;
+          b=startPos.y()-k*startPos.x();
+      }
+      else
+      {
+          k=(endPos.y()-startPos.y())/(endPos.x()-startPos.x());
+          b=startPos.y()-k*startPos.x();
+      }
+//      qDebug()<<k<<b;
+
+     double temp = fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b);
+      //求直线与圆的交点 前提是圆需要与直线有交点
+     if(fabs(k*agvPos.x()-agvPos.y()+b)/sqrt(k*k+b*b)<R)
+      {
+          point1.setX((2*agvPos.x()-2*k*(b-agvPos.y())+sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
+          point2.setX((2*agvPos.x()-2*k*(b-agvPos.y())-sqrt(pow((2*k*(b-agvPos.y())-2*agvPos.x()),2)-4*(k*k+1)*((b-agvPos.y())*(b-agvPos.y())+agvPos.x()*agvPos.x()-R*R)))/(2*k*k+2));
+          point1.setY(k*point1.x()+b);
+          point2.setY(k*point2.x()+b);
+          return 0;
+      }
+
+     return -1;
+}
+
+
+/**
+  * @brief CalcHdg
+  * 计算点0到点1的航向
+  * @param p0        Point 0
+  * @param p1        Point 1
+**/
+static double CalcHdg(QPointF p0, QPointF p1)
+{
+    double x0,y0,x1,y1;
+    x0 = p0.x();
+    y0 = p0.y();
+    x1 = p1.x();
+    y1 = p1.y();
+    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;
+}
+
+
+
+static bool pointinarc(GeometryArc * parc,QPointF poingarc,QPointF point1)
+{
+    double hdg = CalcHdg(poingarc,point1);
+    if(parc->GetCurvature() >0)hdg = hdg + M_PI/2.0;
+    else hdg = hdg - M_PI/2.0;
+    if(hdg >= 2.0*M_PI)hdg = hdg - 2.0*M_PI;
+    if(hdg < 0)hdg = hdg + 2.0*M_PI;
+
+    double hdgrange = parc->GetLength()/(1.0/parc->GetCurvature());
+
+    double hdgdiff = hdg - parc->GetHdg();
+
+    if(hdgrange >= 0 )
+    {
+        if(hdgdiff < 0)hdgdiff = hdgdiff + M_PI*2.0;
+    }
+    else
+    {
+        if(hdgdiff > 0)hdgdiff = hdgdiff - M_PI*2.0;
+    }
+
+    if(fabs(hdgdiff ) < fabs(hdgrange))return true;
+    return false;
+
+}
+
+static inline double calcpointdis(QPointF p1,QPointF p2)
+{
+    return sqrt(pow(p1.x()-p2.x(),2)+pow(p1.y()-p2.y(),2));
+}
+
+/**
+  * @brief GetArcDis
+  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
+  * @param parc        pointer to a arc geomery
+  * @param x           current x
+  * @param y           current y
+  * @param nearx       near x
+  * @param neary       near y
+  * @param nearhead    nearhead
+**/
+
+static double GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+ //   double fRtn = 1000.0;
+    if(parc->GetCurvature() == 0.0)return 1000.0;
+
+    double R = fabs(1.0/parc->GetCurvature());
+
+//    if(parc->GetX() == 4.8213842690322309e+01)
+//    {
+//        int a = 1;
+//        a = 3;
+//    }
+//    if(parc->GetCurvature() == -0.0000110203)
+//    {
+//        int a = 1;
+//        a = 3;
+//    }
+
+    //calculate arc center
+    double x_center,y_center;
+    if(parc->GetCurvature() > 0)
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() + M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg()+ M_PI/2.0);
+    }
+    else
+    {
+        x_center = parc->GetX() + R * cos(parc->GetHdg() - M_PI/2.0);
+        y_center = parc->GetY() + R * sin(parc->GetHdg() - M_PI/2.0);
+    }
+
+    double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
+
+
+    QPointF arcpoint;
+    arcpoint.setX(x_center);arcpoint.setY(y_center);
+
+    QPointF pointnow;
+    pointnow.setX(x);pointnow.setY(y);
+    QPointF point1,point2;
+    point1.setX(x_center + (R * cos(hdgltoa)));
+    point1.setY(y_center + (R * sin(hdgltoa)));
+    point2.setX(x_center + (R * cos(hdgltoa + M_PI)));
+    point2.setY(y_center + (R * sin(hdgltoa + M_PI)));
+
+    //calculat dis
+    bool bp1inarc,bp2inarc;
+    bp1inarc =pointinarc(parc,arcpoint,point1);
+    bp2inarc =pointinarc(parc,arcpoint,point2);
+    double fdis[4];
+    fdis[0] = calcpointdis(pointnow,point1);
+    fdis[1] = calcpointdis(pointnow,point2);
+    fdis[2] = calcpointdis(pointnow,QPointF(parc->GetX(),parc->GetY()));
+    QPointF pointend;
+    double hdgrange = parc->GetLength()*parc->GetCurvature();
+    double hdgend = parc->GetHdg() + hdgrange;
+    while(hdgend <0.0)hdgend = hdgend + 2.0 *M_PI;
+    while(hdgend >= 2.0*M_PI) hdgend = hdgend -2.0*M_PI;
+
+    if(parc->GetCurvature() >0)
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend -M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend -M_PI/2.0) );
+    }
+    else
+    {
+        pointend.setX(arcpoint.x() + R*cos(hdgend +M_PI/2.0 ));
+        pointend.setY(arcpoint.y() + R*sin(hdgend +M_PI/2.0) );
+    }
+
+    fdis[3] = calcpointdis(pointnow,pointend);
+    int indexmin = -1;
+    double fdismin = 1000000.0;
+    if(bp1inarc)
+    {
+        indexmin = 0;fdismin = fdis[0];
+    }
+    if(bp2inarc)
+    {
+        if(indexmin == -1)
+        {
+            indexmin = 1;fdismin = fdis[1];
+        }
+        else
+        {
+            if(fdis[1]<fdismin)
+            {
+                indexmin = 1;fdismin = fdis[1];
+            }
+        }
+    }
+    if(indexmin == -1)
+    {
+        indexmin = 2;fdismin = fdis[2];
+    }
+    else
+    {
+        if(fdis[2]<fdismin)
+        {
+            indexmin = 2;fdismin = fdis[2];
+        }
+    }
+    if(fdis[3]<fdismin)
+    {
+        indexmin = 3;fdismin = fdis[3];
+    }
+
+    switch (indexmin) {
+    case 0:
+        nearx = point1.x();
+        neary = point1.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point1) - M_PI/2.0;
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point1) + M_PI/2.0;
+        }
+        break;
+    case 1:
+        nearx = point2.x();
+        neary = point2.y();
+        if(parc->GetCurvature()<0)
+        {
+            nearhead = CalcHdg(arcpoint,point2) - M_PI/2.0;
+        }
+        else
+        {
+            nearhead = CalcHdg(arcpoint,point2) + M_PI/2.0;
+        }
+        break;
+    case 2:
+        nearx = parc->GetX();
+        neary = parc->GetY();
+        nearhead = parc->GetHdg();
+        break;
+    case 3:
+        nearx = pointend.x();
+        neary = pointend.y();
+        nearhead = hdgend;
+        break;
+    default:
+        std::cout<<"error in arcdis "<<std::endl;
+        break;
+    }
+
+    while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
+    while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
+    return fdismin;
+}
+
+
+static double GetSpiralDis(GeometrySpiral * pspiral,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+    double x,y,hdg;
+    double s = 0.0;
+    double fdismin = 100000.0;
+    double s0 = pspiral->GetS();
+
+    while(s<pspiral->GetLength())
+    {
+        pspiral->GetCoords(s0+s,x,y,hdg);
+        s = s+0.1;
+
+        double fdis = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+    }
+
+    return fdismin;
+}
+
+/**
+ * @brief GetParamPoly3Dis 获得点到贝塞尔曲线的距离。
+ * @param parc
+ * @param xnow
+ * @param ynow
+ * @param nearx
+ * @param neary
+ * @param nearhead
+ * @return
+ */
+static double GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+
+    double s = 0.1;
+    double fdismin = 100000.0;
+
+    double xold,yold;
+    xold = parc->GetX();
+    yold = parc->GetY();
+
+    double fdis = calcpointdis(QPointF(parc->GetX(),parc->GetY()),QPointF(xnow,ynow));
+    if(fdis<fdismin)
+    {
+        fdismin = fdis;
+        nearhead = parc->GetHdg();
+        nearx = parc->GetX();
+        neary = parc->GetY();
+    }
+
+    double s_start = parc->GetS();
+
+    while(s < parc->GetLength())
+    {
+
+        double x, y,hdg;//xtem,ytem;
+        parc->GetCoords(s_start+s,x,y,hdg);
+        if(fdis<fdismin)
+        {
+            fdismin = fdis;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+
+//        xold = x;
+//        yold = y;
+        s = s+ 0.1;
+    }
+
+    return fdismin;
+
+}
+
+
+static double GetPoly3Dis(GeometryPoly3 * ppoly,double xnow,double ynow,double & nearx,
+                        double & neary,double & nearhead)
+{
+    double x,y,hdg;
+//    double s = 0.0;
+    double fdismin = 100000.0;
+ //   double s0 = ppoly->GetS();
+
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = 0.3;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+
+
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    u = u+du;
+    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));
+
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+        hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+
+        double fdisnow = calcpointdis(QPointF(x,y),QPointF(xnow,ynow));
+        if(fdisnow<fdismin)
+        {
+            fdismin = fdisnow;
+            nearhead = hdg;
+            nearx = x;
+            neary = y;
+        }
+
+        oldx = x;
+        oldy = y;
+    }
+
+    return fdismin;
+}
+
+
+/**
+  * @brief GetNearPoint
+  * 计算点到圆弧的最短距离，首先用点和圆弧中心点的直线与圆的交点，计算点到交点的距离，如果交点在圆弧上，则最短距离是交点之一，否则计算点到圆弧两个端点的距离。
+  * @param x           current x
+  * @param y           current y
+  * @param pxodr       OpenDrive
+  * @param pObjRoad    Road
+  * @param pgeo        Geometry of road
+  * @param nearx       near x
+  * @param neary       near y
+  * @param nearhead    nearhead
+  * @param nearthresh  near threshhold
+  * @retval            if == 0 successfull  <0 fail.
+**/
+int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
+                 double & neary,double & nearhead,const double nearthresh,double &froads)
+{
+
+    double dismin = std::numeric_limits<double>::infinity();
+    s = dismin;
+    int i;
+    double frels;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh;
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+ //               dis = GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh);
+                break;
+            case 1:
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 2:  //arc
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+                break;
+
+            case 3:
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 4:
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+            if(dis < dismin)
+            {
+                dismin = dis;
+                nearx = nx;
+                neary = ny;
+                nearhead = nh;
+                s = dis;
+                froads = frels;
+                *pObjRoad = proad;
+                *pgeo = pgb;
+            }
+
+
+//            if(pgb->CheckIfLine())
+//            {
+//                GeometryLine * pline = (GeometryLine *)pgb->GetGeometryAt(0);
+
+//                double dis = GetLineDis(pline,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//            }
+//            else
+//            {
+//                GeometryLine * pline1 = (GeometryLine *)pgb->GetGeometryAt(0);
+//                double dis = GetLineDis(pline1,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//                GeometryArc * parc = (GeometryArc *)pgb->GetGeometryAt(1);
+//                dis = GetArcDis(parc,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//                pline1 = (GeometryLine *)pgb->GetGeometryAt(2);
+//                dis = GetLineDis(pline1,x,y,nx,ny,nh);
+//                if(dis < dismin)
+//                {
+//                    dismin = dis;
+//                    nearx = nx;
+//                    neary = ny;
+//                    nearhead = nh;
+//                    s = dis;
+//                    *pObjRoad = proad;
+//                    *pgeo = pgb;
+//                }
+//            }
+
+        }
+    }
+    if(s > nearthresh)return -1;
+    return 0;
+}
+
+
+
+
+int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDrive * pxodr,std::vector<iv::nearroad> & xvectornear,
+                         const double nearthresh,bool bhdgvalid = true)
+{
+    std::cout<<" near thresh "<<nearthresh<<std::endl;
+    int i;
+    double frels;
+    int nminmode = 6;
+    for(i=0;i<pxodr->GetRoadCount();i++)
+    {
+        int j;
+        Road * proad = pxodr->GetRoad(i);
+        double nx,ny,nh;
+
+        bool bchange = false;
+        double localdismin = std::numeric_limits<double>::infinity();
+
+        double localnearx = 0;
+        double localneary = 0;
+        double localnearhead = 0;
+        double locals = 0;
+        double localfrels = 0;
+        GeometryBlock * pgeolocal;
+        localdismin = std::numeric_limits<double>::infinity();
+
+        for(j=0;j<proad->GetGeometryBlockCount();j++)
+        {
+
+            GeometryBlock * pgb = proad->GetGeometryBlock(j);
+            double dis;
+            RoadGeometry * pg;
+            pg = pgb->GetGeometryAt(0);
+
+            switch (pg->GetGeomType()) {
+            case 0:   //line
+                dis = xodrfunc::GetLineDis((GeometryLine *) pg,x,y,nx,ny,nh,frels);
+                break;
+            case 1:
+                dis = xodrfunc::GetSpiralDis((GeometrySpiral *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 2:  //arc
+                dis = xodrfunc::GetArcDis((GeometryArc *)pg,x,y,nx,ny,nh,frels);
+                break;
+
+            case 3:
+                dis = xodrfunc::GetPoly3Dis((GeometryPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            case 4:
+                dis = xodrfunc::GetParamPoly3Dis((GeometryParamPoly3 *)pg,x,y,nx,ny,nh,frels);
+                break;
+            default:
+                dis = 100000.0;
+                break;
+            }
+
+
+            if(dis<localdismin)
+            {
+                localdismin = dis;
+                localnearx = nx;
+                localneary = ny;
+                localnearhead = nh;
+                locals = dis;
+                localfrels = frels;
+                pgeolocal = pgb;
+                bchange = true;
+            }
+
+
+        }
+
+        std::cout<<" local dismin "<<localdismin<<std::endl;
+        if(localdismin < nearthresh)
+        {
+            iv::nearroad xnear;
+            xnear.pObjRoad = proad;
+            xnear.pgeob = pgeolocal;
+            xnear.nearx = localnearx;
+            xnear.neary = localneary;
+            xnear.fdis = localdismin;
+            xnear.nearhead = localnearhead;
+            xnear.frels = localfrels;
+            if((xnear.fdis>0)&&(xnear.frels>0.00001)&&(xnear.frels<(proad->GetRoadLength()-0.00001)))
+            {
+                double fcalhdg = xodrfunc::CalcHdg(QPointF(xnear.nearx,xnear.neary),QPointF(x,y));
+                double fhdgdiff = fcalhdg - xnear.nearhead;
+                while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
+                while(fhdgdiff>= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
+                if(fhdgdiff<M_PI)
+                {
+                    double fdisroad = proad->GetRoadLeftWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 1;
+                }
+                else
+                {
+                    double fdisroad = proad->GetRoadRightWidth(xnear.frels);
+                    if(fdisroad>xnear.fdis)
+                    {
+                        xnear.fdis = 0;
+                    }
+                    else
+                    {
+                        xnear.fdis = xnear.fdis - fdisroad;
+                    }
+                    xnear.lr = 2;
+                }
+
+
+            }
+            else
+            {
+                if(bhdgvalid == false)
+                {
+                    if(xnear.pObjRoad->GetLaneSectionCount()>0)
+                    {
+                        LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+                        if(pLS->GetRightLaneCount()>0)
+                        {
+                            xnear.lr = 2;
+                        }
+                        else
+                        {
+                            xnear.lr = 1;
+                        }
+                    }
+                    else
+                    {
+                        xnear.lr = 2;
+                    }
+                }
+            }
+            LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+            if(pLS != NULL)
+            {
+            if(xnear.fdis < 0.00001)
+            {
+                if(bhdgvalid == false)
+                {
+                    xnear.nmode = 0;
+                }
+                else
+                {
+                    double headdiff = hdg - xnear.nearhead;
+                    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                    if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                    {
+                       xnear.nmode = 0;
+                    }
+                    else
+                    {
+                        xnear.nmode = 1;
+                    }
+                }
+
+            }
+            else
+            {
+                if(xnear.fdis<5)
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 2;
+                    }
+                    else
+                    {
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                        {
+                            xnear.nmode = 2;
+                        }
+                        else
+                        {
+                            xnear.nmode = 3;
+                        }
+                    }
+
+                }
+                else
+                {
+                    if(bhdgvalid == false)
+                    {
+                        xnear.nmode = 4;
+                    }
+                    else
+                    {
+                        double headdiff = hdg - xnear.nearhead;
+                        while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+                        while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+                        if(((headdiff <M_PI/2.0)||(headdiff > M_PI*3.0/2.0))&&(pLS->GetRightLaneCount()>0))
+                        {
+                            xnear.nmode = 4;
+                        }
+                        else
+                        {
+                            xnear.nmode = 5;
+                        }
+                    }
+                }
+            }
+            if(xnear.nmode < nminmode)nminmode = xnear.nmode;
+
+            if(xnear.pObjRoad->GetLaneSectionCount()>0)
+            {
+                LaneSection * pLS = xnear.pObjRoad->GetLaneSection(0);
+                if((pLS->GetRightLaneCount()>0)&& ((pLS->GetLeftLaneCount()==0)) && (xnear.lr == 1))
+                {
+                    xnear.lr = 2;
+                }
+                if((pLS->GetRightLaneCount() == 0)&& ((pLS->GetLeftLaneCount()>0)) && (xnear.lr == 2))
+                {
+                    xnear.lr = 1;
+                }
+            }
+
+            xvectornear.push_back(xnear);
+            }
+        }
+
+    }
+
+    if(xvectornear.size() == 0)
+    {
+        std::cout<<" no near. "<<std::endl;
+        return -1;
+    }
+
+
+//    std::cout<<" near size: "<<xvectornear.size()<<std::endl;
+    if(xvectornear.size() > 1)
+    {
+        int i;
+        for(i=0;i<(int)xvectornear.size();i++)
+        {
+            if(xvectornear[i].nmode > nminmode)
+            {
+                xvectornear.erase(xvectornear.begin() + i);
+                i = i-1;
+            }
+        }
+    }
+//    std::cout<<" after size: "<<xvectornear.size()<<std::endl;
+    if((xvectornear.size()>1)&&(nminmode>=4))  //if dis > 5 select small dis
+    {
+        int i;
+        while(xvectornear.size()>1)
+        {
+            if(xvectornear[1].fdis < xvectornear[0].fdis)
+            {
+                xvectornear.erase(xvectornear.begin());
+            }
+            else
+            {
+                xvectornear.erase(xvectornear.begin()+1);
+            }
+        }
+    }
+    return 0;
+
+}
+
+
+
+/**
+  * @brief SelectRoadLeftRight
+  * 选择左侧道路或右侧道路
+  * 1.选择角度差小于90度的道路一侧，即同侧道路
+  * 2.单行路，选择存在的那一侧道路。
+  * @param pRoad       道路
+  * @param head1       车的航向或目标点的航向
+  * @param head_road   目标点的航向
+  * @retval            1 left   2 right
+**/
+static int SelectRoadLeftRight(Road * pRoad,const double head1,const double  head_road)
+{
+    int nrtn = 2;
+
+
+    double headdiff = head1 - head_road;
+    while(headdiff < 2.0*M_PI) headdiff = headdiff + 2.0*M_PI;
+    while(headdiff >= 2.0*M_PI)headdiff = headdiff - 2.0*M_PI;
+
+    bool bSel = false;
+    if((headdiff >=M_PI/2.0)&&(headdiff <= M_PI*3.0/2.0))  //if diff is
+    {
+        if(pRoad->GetLaneSection(0)->GetLeftLaneCount()>0)
+        {
+
+            nrtn = 1;
+            bSel = true;
+        }
+    }
+    else
+    {
+        if(pRoad->GetLaneSection(0)->GetRightLaneCount()>0)
+        {
+            nrtn = 2;
+            bSel = true;
+        }
+    }
+
+    if(bSel == false)
+    {
+        if(pRoad->GetLaneSection(0)->GetLeftLaneCount() >0)
+        {
+            nrtn = 1;
+        }
+        else
+        {
+            nrtn = 2;
+        }
+    }
+
+    return nrtn;
+
+}
+
+
+
+//static double getlinereldis(GeometryLine * pline,double x,double y)
+//{
+//    double x0,y0;
+//    x0 = pline->GetX();
+//    y0 = pline->GetY();
+//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
+//    if(dis > pline->GetS())
+//    {
+//        std::cout<<"getlinereldis exceed s S is "<<pline->GetS()<<" dis is "<<dis<<std::endl;
+//        return dis;
+//    }
+//    return dis;
+//}
+
+//static double getarcreldis(GeometryArc * parc,double x,double y)
+//{
+//    double x0,y0;
+//    x0 = parc->GetX();
+//    y0 = parc->GetY();
+//    double dis = sqrt(pow(x-x0,2) + pow(y-y0,2));
+//    double R = fabs(1.0/parc->GetCurvature());
+//    double ang = 2.0* asin(dis/(2.0*R));
+//    double frtn = ang * R;
+//    return frtn;
+//}
+
+//static double getparampoly3reldis(GeometryParamPoly3 *parc, double xnow, double ynow)
+//{
+//    double s = 0.1;
+
+//    double xold,yold;
+//    xold = parc->GetX();
+//    yold = parc->GetY();
+
+
+//    while(s < parc->GetLength())
+//    {
+
+//        double x, y,xtem,ytem;
+//        xtem = parc->GetuA() + parc->GetuB() * s + parc->GetuC() * s*s + parc->GetuD() * s*s*s;
+//        ytem = parc->GetvA() + parc->GetvB() * s + parc->GetvC() * s*s + parc->GetvD() * s*s*s;
+//        x = xtem*cos(parc->GetHdg()) - ytem * sin(parc->GetHdg()) + parc->GetX();
+//        y = xtem*sin(parc->GetHdg()) + ytem * cos(parc->GetHdg()) + parc->GetY();
+
+//        if(sqrt(pow(xnow - x,2) + pow(ynow -y,2))<0.3)
+//        {
+//            break;
+//        }
+
+//        xold = x;
+//        yold = y;
+//        s = s+ 0.1;
+//    }
+
+//    return s;
+//}
+
+//static double getreldis(RoadGeometry * prg,double x,double y)
+//{
+//    int ngeotype = prg->GetGeomType();
+//    double frtn = 0;
+//    switch (ngeotype) {
+//    case 0:
+//        frtn = getlinereldis((GeometryLine *)prg,x,y);
+//        break;
+//    case 1:
+//        break;
+//    case 2:
+//        frtn = getarcreldis((GeometryArc *)prg,x,y);
+//        break;
+//    case 3:
+//        break;
+//    case 4:
+//        frtn = getparampoly3reldis((GeometryParamPoly3 *)prg,x,y);
+//        break;
+//    default:
+//        break;
+//    }
+
+//    return frtn;
+//}
+
+
+
+
+//static double getposinroad(Road * pRoad,GeometryBlock * pgeob,double x,double y)
+//{
+//    RoadGeometry* prg = pgeob->GetGeometryAt(0);
+//    double s1 = prg->GetS();
+//    double srtn = s1;
+
+//    return s1 + getreldis(prg,x,y);
+//}
+
+
+static std::vector<PlanPoint> getlinepoint(GeometryLine * pline,const double fspace = 0.1)
+{
+    std::vector<PlanPoint> xvectorPP;
+    int i;
+    double s = pline->GetLength();
+    int nsize = s/fspace;
+    if(nsize ==0)nsize = 1;
+
+    for(i=0;i<nsize;i++)
+    {
+        double x,y;
+        x = pline->GetX() + i *fspace * cos(pline->GetHdg());
+        y = pline->GetY() + i *fspace * sin(pline->GetHdg());
+        PlanPoint pp;
+        pp.x = x;
+        pp.y = y;
+        pp.dis = i*fspace;
+        pp.hdg = pline->GetHdg();
+        pp.mS = pline->GetS() + i*fspace;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace = 0.1)
+{
+    std::vector<PlanPoint> xvectorPP;
+
+    //   double fRtn = 1000.0;
+    if(parc->GetCurvature() == 0.0)return xvectorPP;
+
+    double R = fabs(1.0/parc->GetCurvature());
+
+    //calculate arc center
+    double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
+    double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
+
+    double arcdiff = fspace/R;
+    int nsize = parc->GetLength()/fspace;
+    if(nsize == 0)nsize = 1;
+    int i;
+    for(i=0;i<nsize;i++)
+    {
+        double x,y;
+        PlanPoint pp;
+        if(parc->GetCurvature() > 0)
+        {
+            x = x_center + R * cos(parc->GetHdg() + i*arcdiff - M_PI/2.0);
+            y = y_center + R * sin(parc->GetHdg() + i*arcdiff - M_PI/2.0);
+            pp.hdg = parc->GetHdg() + i*arcdiff;
+        }
+        else
+        {
+            x = x_center + R * cos(parc->GetHdg() -i*arcdiff + M_PI/2.0);
+            y = y_center + R * sin(parc->GetHdg() -i*arcdiff + M_PI/2.0);
+            pp.hdg = parc->GetHdg() - i*arcdiff;
+        }
+
+        pp.x = x;
+        pp.y = y;
+        pp.dis = i*fspace;
+        pp.mS = parc->GetS() + i*fspace;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+
+static std::vector<PlanPoint> getspiralpoint(GeometrySpiral * pspiral,const double fspace = 0.1)
+{
+
+    double x,y,hdg;
+    double s = 0.0;
+    double s0 = pspiral->GetS();
+
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+
+    while(s<pspiral->GetLength())
+    {
+        pspiral->GetCoords(s0+s,x,y,hdg);
+
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+        pp.dis = s;
+        pp.mS = pspiral->GetS() + s;
+        xvectorPP.push_back(pp);
+
+        s = s+fspace;
+
+    }
+    return xvectorPP;
+}
+
+static std::vector<PlanPoint> getpoly3dpoint(GeometryPoly3 * ppoly,const double fspace = 0.1)
+{
+    double x,y;
+    x = ppoly->GetX();
+    y = ppoly->GetY();
+    double A,B,C,D;
+    A = ppoly->GetA();
+    B = ppoly->GetB();
+    C = ppoly->GetC();
+    D = ppoly->GetD();
+    const double steplim = fspace;
+    double du = steplim;
+    double u = 0;
+    double v = 0;
+    double oldx,oldy;
+    oldx = x;
+    oldy = y;
+    double xstart,ystart;
+    xstart = x;
+    ystart = y;
+    double hdgstart = ppoly->GetHdg();
+    double flen = 0;
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+    pp.x = xstart;
+    pp.y = ystart;
+    pp.hdg = hdgstart;
+    pp.dis = 0;
+    pp.mS = ppoly->GetS();
+    xvectorPP.push_back(pp);
+    u = du;
+    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));
+        double hdg = CalcHdg(QPointF(oldx,oldy),QPointF(x,y));
+        oldx = x;
+        oldy = y;
+        if(fdis>(steplim*2.0))du = du/2.0;
+        flen = flen + fdis;
+        u = u + du;
+
+
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+
+ //       s = s+ dt;
+        pp.dis = flen;;
+        pp.mS = ppoly->GetS() + flen;
+        xvectorPP.push_back(pp);
+    }
+
+    return xvectorPP;
+
+}
+
+static std::vector<PlanPoint> getparampoly3dpoint(GeometryParamPoly3 * parc,const double fspace = 0.1)
+{
+    double s = 0.1;
+    std::vector<PlanPoint> xvectorPP;
+    PlanPoint pp;
+
+    double xold,yold;
+    xold = parc->GetX();
+    yold = parc->GetY();
+    double hdg0 = parc->GetHdg();
+    pp.x = xold;
+    pp.y = yold;
+    pp.hdg = hdg0;
+    pp.dis = 0;
+//    xvectorPP.push_back(pp);
+
+    double dt = 1.0;
+    double tcount = parc->GetLength()/0.1;
+    if(tcount > 0)
+    {
+        dt = 1.0/tcount;
+    }
+    double t;
+    s = dt;
+    s = 0;
+
+    double ua,ub,uc,ud,va,vb,vc,vd;
+    ua = parc->GetuA();ub= parc->GetuB();uc= parc->GetuC();ud = parc->GetuD();
+    va = parc->GetvA();vb= parc->GetvB();vc= parc->GetvC();vd = parc->GetvD();
+
+    double s_start = parc->GetS();
+    while(s < parc->GetLength())
+    {
+        double x, y,hdg;
+        parc->GetCoords(s_start+s,x,y,hdg);
+        pp.x = x;
+        pp.y = y;
+        pp.hdg = hdg;
+        s = s+ fspace;
+        pp.dis = pp.dis + fspace;;
+        pp.mS = s_start + s;
+        xvectorPP.push_back(pp);
+    }
+    return xvectorPP;
+}
+
+
+std::vector<PlanPoint> GetPoint(pathsection xpath,const double fspace = 0.1)
+{
+    Road * pRoad = xpath.mpRoad;
+    //s_start  s_end for select now section data.
+    double s_start,s_end;
+    LaneSection * pLS = pRoad->GetLaneSection(xpath.msectionid);
+    s_start = pLS->GetS();
+    if(xpath.msectionid == (pRoad->GetLaneSectionCount()-1))s_end = pRoad->GetRoadLength();
+    else
+    {
+        s_end = pRoad->GetLaneSection(xpath.msectionid+1)->GetS();
+    }
+
+//    if(xpath.mroadid == 10190)
+//    {
+//        int a = 1;
+//        a++;
+//    }
+
+    std::vector<PlanPoint> xvectorPPS;
+    double s = 0;
+
+
+    int i;
+    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
+    {
+        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
+        RoadGeometry * prg = pgb->GetGeometryAt(0);
+        std::vector<PlanPoint> xvectorPP;
+        if(i<(pRoad->GetGeometryBlockCount() -0))
+        {
+            if(prg->GetS()>s_end)
+            {
+                continue;
+            }
+            if((prg->GetS() + prg->GetLength())<s_start)
+            {
+                continue;
+            }
+        }
+
+
+        double s1 = prg->GetLength();
+
+        switch (prg->GetGeomType()) {
+        case 0:
+            xvectorPP = getlinepoint((GeometryLine *)prg,fspace);
+
+            break;
+        case 1:
+            xvectorPP = getspiralpoint((GeometrySpiral *)prg,fspace);
+            break;
+        case 2:
+            xvectorPP = getarcpoint((GeometryArc *)prg,fspace);
+            break;
+        case 3:
+
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg,fspace);
+            break;
+        case 4:
+            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg,fspace);
+            break;
+        default:
+            break;
+        }
+        int j;
+        if(prg->GetS()<s_start)
+        {
+            s1 = prg->GetLength() -(s_start - prg->GetS());
+        }
+        if((prg->GetS() + prg->GetLength())>s_end)
+        {
+            s1 = s_end - prg->GetS();
+        }
+        for(j=0;j<xvectorPP.size();j++)
+        {
+            PlanPoint pp = xvectorPP.at(j);
+
+//            double foffset = pRoad->GetLaneOffsetValue(pp.mS);
+
+//            if(fabs(foffset)>0.001)
+//            {
+//                pp.x = pp.x + foffset * cos(pp.hdg + M_PI/2.0);
+//                pp.y = pp.y + foffset * sin(pp.hdg + M_PI/2.0);
+//            }
+
+            pp.mfCurvature = pRoad->GetRoadCurvature(pp.mS);
+
+
+
+            if(((pp.dis+prg->GetS()) >= s_start) &&((pp.dis+prg->GetS()) <= s_end))
+            {
+                if(s_start > prg->GetS())
+                {
+                    pp.dis = s + pp.dis -(s_start - prg->GetS());
+                }
+                else
+                {
+                    pp.dis = s+ pp.dis;
+                }
+                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+                xvectorPPS.push_back(pp);
+            }
+//            if((prg->GetS()>s_start)&&((prg->GetS() + prg->GetLength())<s_end))
+//            {
+//                pp.dis = s + pp.dis;
+//                pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                xvectorPPS.push_back(pp);
+//            }
+//            else
+//            {
+//                if(prg->GetS()<s_start)
+//                {
+
+//                }
+//                else
+//                {
+//                    if(pp.dis<(s_end -prg->GetS()))
+//                    {
+//                        pp.dis = s + pp.dis;
+//                        pp.nRoadID = atoi(pRoad->GetRoadId().data());
+//                        xvectorPPS.push_back(pp);
+//                    }
+//                }
+//            }
+        }
+        s = s+ s1;
+
+    }
+    return xvectorPPS;
+}
+
+std::vector<PlanPoint> GetPoint(Road * pRoad)
+{
+    std::vector<PlanPoint> xvectorPPS;
+    double s = 0;
+
+    int i;
+    for(i=0;i<pRoad->GetGeometryBlockCount();i++)
+    {
+        GeometryBlock * pgb =  pRoad->GetGeometryBlock(i);
+        RoadGeometry * prg = pgb->GetGeometryAt(0);
+        std::vector<PlanPoint> xvectorPP;
+        double s1 = prg->GetLength();
+        switch (prg->GetGeomType()) {
+        case 0:
+            xvectorPP = getlinepoint((GeometryLine *)prg);
+            break;
+        case 1:
+            xvectorPP = getspiralpoint((GeometrySpiral *)prg);
+            break;
+        case 2:
+            xvectorPP = getarcpoint((GeometryArc *)prg);
+
+            break;
+        case 3:
+            xvectorPP = getpoly3dpoint((GeometryPoly3 *)prg);
+            break;
+        case 4:
+            xvectorPP = getparampoly3dpoint((GeometryParamPoly3 *)prg);
+            break;
+        default:
+            break;
+        }
+        int j;
+        for(j=0;j<xvectorPP.size();j++)
+        {
+            PlanPoint pp = xvectorPP.at(j);
+            pp.dis = s + pp.dis;
+            pp.nRoadID = atoi(pRoad->GetRoadId().data());
+            xvectorPPS.push_back(pp);
+        }
+        s = s+ s1;
+
+    }
+
+    unsigned int j;
+    for(j=0;j<xvectorPPS.size();j++)
+    {
+        xvectorPPS.at(j).mfCurvature = pRoad->GetRoadCurvature(xvectorPPS.at(j).mS);
+    }
+    return xvectorPPS;
+
+}
+
+int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
+{
+    int nrtn = 0;
+    int i;
+    int dismin = 1000;
+    for(i=0;i<xvectorPP.size();i++)
+    {
+        double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
+        if(dis <dismin)
+        {
+            dismin = dis;
+            nrtn = i;
+        }
+    }
+
+    if(dismin > 10.0)
+    {
+        std::cout<<"indexinroadpoint near point is error. dis is "<<dismin<<std::endl;
+    }
+    return nrtn;
+}
+
+
+double getwidth(Road * pRoad, int nlane)
+{
+    double frtn = 0;
+
+    int i;
+    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        if(nlane == pRoad->GetLaneSection(0)->GetLane(i)->GetId())
+        {
+            LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
+            if(pLW != 0)
+            {
+            frtn = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0)->GetA();
+            break;
+            }
+        }
+    }
+    return frtn;
+}
+
+double getoff(Road * pRoad,int nlane)
+{
+    double off = getwidth(pRoad,nlane)/2.0;
+    if(nlane < 0)off = off * (-1.0);
+ //   int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+    int i;
+    if(nlane > 0)
+        off = off + getwidth(pRoad,0)/2.0;
+    else
+        off = off - getwidth(pRoad,0)/2.0;
+    if(nlane > 0)
+    {
+        for(i=1;i<nlane;i++)
+        {
+            off = off + getwidth(pRoad,i);
+        }
+    }
+    else
+    {
+        for(i=-1;i>nlane;i--)
+        {
+            off = off - getwidth(pRoad,i);
+        }
+    }
+//    return 0;
+    return off;
+
+}
+
+
+namespace  iv {
+
+struct lanewidthabcd
+{
+    int nlane;
+    double A;
+    double B;
+    double C;
+    double D;
+};
+}
+
+double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
+{
+    double frtn = 0;
+
+
+    int i;
+    int nlanecount = xvectorlwa.size();
+    for(i=0;i<nlanecount;i++)
+    {
+        if(nlane == xvectorlwa.at(i).nlane)
+        {
+            iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
+            frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
+            break;
+        }
+    }
+    return frtn;
+}
+
+
+
+std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
+{
+    std::vector<iv::lanewidthabcd> xvectorlwa;
+    if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
+    int i;
+
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+//    if(pRoad->GetLaneSectionCount() > 1)
+//    {
+//        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+//        {
+//            if(s>pRoad->GetLaneSection(i+1)->GetS())
+//            {
+//                pLS = pRoad->GetLaneSection(i+1);
+//            }
+//            else
+//            {
+//                break;
+//            }
+//        }
+//    }
+
+    int nlanecount = pLS->GetLaneCount();
+
+    for(i=0;i<nlanecount;i++)
+    {
+        iv::lanewidthabcd xlwa;
+
+
+        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
+        xlwa.nlane = pLS->GetLane(i)->GetId();
+        if(pLW != 0)
+        {
+
+            xlwa.A = pLW->GetA();
+            xlwa.B = pLW->GetB();
+            xlwa.C = pLW->GetC();
+            xlwa.D = pLW->GetD();
+
+        }
+        else
+        {
+            xlwa.A = 0;
+            xlwa.B = 0;
+            xlwa.C = 0;
+            xlwa.D = 0;
+        }
+
+ //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.
+
+    }
+    return xvectorlwa;
+
+}
+
+
+inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
+{
+    int i;
+    double off = 0;
+    double a,b,c,d;
+    if(xvectorIndex.size() == 0)return off;
+
+    for(i=0;i<(xvectorIndex.size()-1);i++)
+    {
+
+        a = xvectorLWA[xvectorIndex[i]].A;
+        b = xvectorLWA[xvectorIndex[i]].B;
+        c = xvectorLWA[xvectorIndex[i]].C;
+        d = xvectorLWA[xvectorIndex[i]].D;
+        if(xvectorLWA[xvectorIndex[i]].nlane != 0)
+        {
+            off = off + a + b*s + c*s*s + d*s*s*s;
+        }
+        else
+        {
+            off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
+        }
+    }
+    i = xvectorIndex.size()-1;
+    a = xvectorLWA[xvectorIndex[i]].A;
+    b = xvectorLWA[xvectorIndex[i]].B;
+    c = xvectorLWA[xvectorIndex[i]].C;
+    d = xvectorLWA[xvectorIndex[i]].D;
+    off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
+    if(nlane < 0) off = off*(-1.0);
+//    std::cout<<"s is "<<s<<std::endl;
+//    std::cout<<" off is "<<off<<std::endl;
+    return off;
+
+}
+
+double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+{
+    double fwidth = 0;
+    int i;
+    double a,b,c,d;
+
+    int nsize = xvectorLWA.size();
+    for(i=0;i<nsize;i++)
+    {
+        if(nlane*(xvectorLWA[i].nlane)>0)
+        {
+            a = xvectorLWA[i].A;
+            b = xvectorLWA[i].B;
+            c = xvectorLWA[i].C;
+            d = xvectorLWA[i].D;
+            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
+        }
+    }
+    return fwidth;
+
+}
+
+int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed,bool & bNoavoid)
+{
+    if(pRoad->GetLaneSectionCount() < 1)return -1;
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+    int i;
+
+    if(pRoad->GetLaneSectionCount() > 1)
+    {
+        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+        {
+            if(s>pRoad->GetLaneSection(i+1)->GetS())
+            {
+                pLS = pRoad->GetLaneSection(i+1);
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+
+    nori = 0;
+    ntotal = 0;
+    fSpeed = -1; //if -1 no speedlimit for map
+
+    pRoad->GetRoadSpeedMax(s,fSpeed);   //Get Road Speed Limit.
+
+    pRoad->GetRoadNoavoid(s,bNoavoid);
+
+    int nlanecount = pLS->GetLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        int nlaneid = pLS->GetLane(i)->GetId();
+
+        if(nlaneid == nlane)
+        {
+            Lane * pLane = pLS->GetLane(i);
+            if(pLane->GetLaneSpeedCount() > 0)
+            {
+                LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
+                int j;
+                int nspeedcount = pLane->GetLaneSpeedCount();
+                for(j=0;j<(nspeedcount -1);j++)
+                {
+                    if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
+                    {
+                        pLSpeed = pLane->GetLaneSpeed(j+1);
+                    }
+                    else
+                    {
+                        break;
+                    }
+                }
+                if(pLSpeed->GetS()<=(s-pLS->GetS()))fSpeed = pLSpeed->GetMax();
+
+            }
+        }
+        if(nlane<0)
+        {
+
+            if(nlaneid < 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid<nlane)nori++;
+                }
+            }
+
+
+        }
+        else
+        {
+            if(nlaneid > 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid > nlane)nori++;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
+std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
+{
+    std::vector<int> xvectorindex;
+    int i;
+    if(nlane >= 0)
+    {
+    for(i=0;i<=nlane;i++)
+    {
+       int j;
+       int nsize = xvectorLWA.size();
+       for(j=0;j<nsize;j++)
+       {
+           if(i == xvectorLWA.at(j).nlane )
+           {
+               xvectorindex.push_back(j);
+               break;
+           }
+       }
+    }
+    }
+    else
+    {
+        for(i=0;i>=nlane;i--)
+        {
+           int j;
+           int nsize = xvectorLWA.size();
+           for(j=0;j<nsize;j++)
+           {
+               if(i == xvectorLWA.at(j).nlane )
+               {
+                   xvectorindex.push_back(j);
+                   break;
+               }
+           }
+        }
+    }
+    return xvectorindex;
+}
+
+void CalcBorringRoad(pathsection xps,std::vector<PlanPoint> & xvectorPP)
+{
+    if(xps.mpRoad->GetRoadBorrowCount() == 0)
+    {
+        return;
+    }
+
+    Road * pRoad = xps.mpRoad;
+    unsigned int nborrowsize = pRoad->GetRoadBorrowCount();
+    unsigned int i;
+    unsigned int nPPCount = xvectorPP.size();
+    for(i=0;i<nborrowsize;i++)
+    {
+        RoadBorrow * pborrow = pRoad->GetRoadBorrow(i);
+        if(pborrow == NULL)
+        {
+            std::cout<<"warning:can't get borrow"<<std::endl;
+            continue;
+        }
+        if((pborrow->GetMode() == "All")||((pborrow->GetMode()=="R2L")&&(xps.mainsel<0))||((pborrow->GetMode()=="L2R")&&(xps.mainsel>0)))
+        {
+            unsigned int j;
+            double soffset = pborrow->GetS();
+            double borrowlen = pborrow->GetLength();
+            for(j=0;j<xvectorPP.size();j++)
+            {
+                if((xvectorPP[j].mS>=soffset)&&(xvectorPP[j].mS<=(soffset + borrowlen)))
+                {
+                    xvectorPP[j].mbBoringRoad = true;
+                }
+            }
+        }
+    }
+}
+
+void CalcInLaneAvoid(pathsection xps,std::vector<PlanPoint> & xvectorPP,const double fvehiclewidth,
+                     const int nchang1,const int nchang2,const int nchangpoint)
+{
+    if(xvectorPP.size()<2)return;
+    bool bInlaneavoid = false;
+    int i;
+    if((xps.mbStartToMainChange == false) && (xps.mbMainToEndChange == false))
+    {
+        if(xps.mpRoad->GetRoadLength()<30)
+        {
+            double hdgdiff = xvectorPP.at(xvectorPP.size()-1).hdg - xvectorPP.at(0).hdg;
+            if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
+            if(hdgdiff>(M_PI/3.0)&&(hdgdiff<(5.0*M_PI/3.0)))
+                bInlaneavoid = false;
+            else
+                bInlaneavoid = true;
+        }
+        else
+        {
+            bInlaneavoid = true;
+        }
+    }
+    else
+    {
+        if(xps.mpRoad->GetRoadLength()>100)bInlaneavoid = true;
+    }
+
+    if(bInlaneavoid == false)
+    {
+        int nvpsize = xvectorPP.size();
+        for(i=0;i<nvpsize;i++)
+        {
+            xvectorPP.at(i).bInlaneAvoid = false;
+            xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
+            xvectorPP.at(i).my_left = xvectorPP.at(i).y;
+            xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
+            xvectorPP.at(i).my_right = xvectorPP.at(i).y;
+        }
+    }
+    else
+    {
+      int nvpsize = xvectorPP.size();
+      for(i=0;i<nvpsize;i++)xvectorPP.at(i).bInlaneAvoid = true;
+      if((xps.mbStartToMainChange == true)||(xps.mbMainToEndChange == true))
+      {
+          if(xps.mbStartToMainChange == true)
+          {
+              for(i=(nchang1 - nchangpoint/2);i<(nchang1 + nchangpoint/2);i++)
+              {
+                  if((i>=0)&&(i<nvpsize))
+                    xvectorPP.at(i).bInlaneAvoid = false;
+              }
+
+          }
+          if(xps.mbMainToEndChange)
+          {
+              for(i=(nchang2 - nchangpoint/2);i<(nchang2 + nchangpoint/2);i++)
+              {
+                  if((i>=0)&&(i<nvpsize))
+                    xvectorPP.at(i).bInlaneAvoid = false;
+              }
+          }
+      }
+
+      for(i=0;i<nvpsize;i++)
+      {
+          if(xvectorPP.at(i).bInlaneAvoid)
+          {
+              double foff = xvectorPP.at(i).mfDisToLaneLeft -0.3-fvehiclewidth*0.5; //30cm + 0.5vehcilewidth
+              if(foff < 0.3)
+              {
+                  xvectorPP.at(i).bInlaneAvoid = false;
+                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x;
+                  xvectorPP.at(i).my_left = xvectorPP.at(i).y;
+                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x;
+                  xvectorPP.at(i).my_right = xvectorPP.at(i).y;
+              }
+              else
+              {
+                  xvectorPP.at(i).mx_left = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg + M_PI/2.0);
+                  xvectorPP.at(i).my_left = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg + M_PI/2.0);
+                  xvectorPP.at(i).mx_right = xvectorPP.at(i).x + foff * cos(xvectorPP.at(i).hdg - M_PI/2.0);
+                  xvectorPP.at(i).my_right = xvectorPP.at(i).y + foff * sin(xvectorPP.at(i).hdg - M_PI/2.0);
+              }
+          }
+      }
+    }
+
+}
+
+
+double getoff(Road * pRoad,int nlane,const double s)
+{
+    int i;
+    int nLSCount = pRoad->GetLaneSectionCount();
+    double s_section = 0;
+
+    double foff = 0;
+
+    for(i=0;i<nLSCount;i++)
+    {
+
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(i<(nLSCount -1))
+        {
+            if(pRoad->GetLaneSection(i+1)->GetS()<s)
+            {
+                continue;
+            }
+        }
+        s_section = pLS->GetS();
+        int nlanecount = pLS->GetLaneCount();
+        int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            Lane * pLane = pLS->GetLane(j);
+
+            int k;
+            double s_lane = s-s_section;
+
+
+            if(pLane->GetId() != 0)
+            {
+
+                for(k=0;k<pLane->GetLaneWidthCount();k++)
+                {
+                    if(k<(pLane->GetLaneWidthCount()-1))
+                    {
+                        if((pLane->GetLaneWidth(k+1)->GetS()+s_section)<s)
+                        {
+                            continue;
+                        }
+                    }
+                    s_lane = pLane->GetLaneWidth(k)->GetS();
+                    break;
+                }
+                LaneWidth * pLW  = pLane->GetLaneWidth(k);
+                if(pLW == 0)
+                {
+                    std::cout<<"not find LaneWidth"<<std::endl;
+                    break;
+                }
+
+                double fds = s - s_lane - s_section;
+                double fwidth= pLW->GetA() + pLW->GetB() * fds
+                        +pLW->GetC() * pow(fds,2) + pLW->GetD() * pow(fds,3);
+
+//                if((pRoad->GetRoadId() == "211210") &&(nlane == -1) &&(pLane->GetId() == -1))
+//                {
+//                    qDebug("fs is %f width is %f",fds,fwidth);
+//                }
+                if(nlane == pLane->GetId())
+                {
+                    foff = foff + fwidth/2.0;
+                }
+                if((nlane*(pLane->GetId())>0)&&(abs(nlane)>abs(pLane->GetId())))
+                {
+                    foff = foff + fwidth;
+                }
+
+            }
+
+        }
+
+
+        break;
+
+    }
+
+    if(nlane<0)foff = foff*(-1);
+
+    if(pRoad->GetLaneOffsetCount()>0)
+    {
+
+        int nLOCount = pRoad->GetLaneOffsetCount();
+        int isel = -1;
+        for(i=0;i<(nLOCount-1);i++)
+        {
+            if(pRoad->GetLaneOffset(i+1)->GetS()>s)
+            {
+                isel = i;
+                break;
+            }
+        }
+        if(isel < 0)isel = nLOCount-1;
+        LaneOffset * pLO = pRoad->GetLaneOffset(isel);
+        double s_off = s - pLO->GetS();
+        double off1 = pLO->Geta() + pLO->Getb()*s_off + pLO->Getc() * s_off * s_off
+                +pLO->Getd() * s_off * s_off * s_off;
+        foff = foff + off1;
+    }
+
+
+    return foff;
+}
+
+
+std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
+{
+    std::vector<PlanPoint> xvectorPP;
+
+    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(xps.mpRoad);
+    std::vector<int> xvectorindex1,xvectorindex2,xvectorindex3;
+    int nchange1,nchange2;
+    int nlane1,nlane2,nlane3;
+    if(xps.mnStartLaneSel == xps.mnEndLaneSel)
+    {
+        xps.mainsel = xps.mnStartLaneSel;
+        xps.mbStartToMainChange = false;
+        xps.mbMainToEndChange = false;
+    }
+    else
+    {
+        if(xps.mpRoad->GetRoadLength() < 100)
+        {
+            xps.mainsel = xps.mnEndLaneSel;
+            xps.mbStartToMainChange = true;
+            xps.mbMainToEndChange = false;
+        }
+    }
+
+    double off1,off2,off3;
+    if(xps.mnStartLaneSel < 0)
+    {
+    off1 = getoff(xps.mpRoad,xps.mnStartLaneSel);
+    off2 = getoff(xps.mpRoad,xps.mainsel);
+    off3 = getoff(xps.mpRoad,xps.mnEndLaneSel);
+    xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
+    xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
+    xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
+    nlane1 = xps.mnStartLaneSel;
+    nlane2 = xps.mainsel;
+    nlane3 = xps.mnEndLaneSel;
+    }
+    else
+    {
+        off3 = getoff(xps.mpRoad,xps.mnStartLaneSel);
+        off2 = getoff(xps.mpRoad,xps.mainsel);
+        off1 = getoff(xps.mpRoad,xps.mnEndLaneSel);
+        xvectorindex3 = GetLWIndex(xvectorLWA,xps.mnStartLaneSel);
+        xvectorindex2 = GetLWIndex(xvectorLWA,xps.mainsel);
+        xvectorindex1 = GetLWIndex(xvectorLWA,xps.mnEndLaneSel);
+        nlane3 = xps.mnStartLaneSel;
+        nlane2 = xps.mainsel;
+        nlane1 = xps.mnEndLaneSel;
+    }
+
+    int nchangepoint = 300;
+    if((nchangepoint * 3) > xvPP.size())
+    {
+        nchangepoint = xvPP.size()/3;
+    }
+    int nsize = xvPP.size();
+
+    nchange1 = nsize/3;
+    if(nchange1>500)nchange1 = 500;
+    nchange2 = nsize*2/3;
+    if( (nsize-nchange2)>500)nchange2 = nsize-500;
+//    if(nsize < 1000)
+//    {
+//        std::cout<<"GetLanePoint Error. road id is "<<xps.mpRoad->GetRoadId()<<std::endl;
+//        return xvectorPP;
+//    }
+    double x,y;
+    int i;
+    if(xps.mainsel < 0)
+    {
+
+        for(i=0;i<nsize;i++)
+        {
+            PlanPoint pp = xvPP.at(i);
+    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+
+            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
+            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+
+            pp.nlrchange = 0;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
+
+                if(xps.mainsel >xps.secondsel)
+                {
+                    pp.nlrchange = 1;
+                }
+                else
+                {
+                    pp.nlrchange = 2;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
+            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+            xvectorPP.push_back(pp);
+        }
+
+
+    }
+    else
+    {
+
+        for(i=0;i<nsize;i++)
+        {
+            PlanPoint pp = xvPP.at(i);
+    //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+
+
+            off1 = getoff(xps.mpRoad,nlane2,pp.mS);
+            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+
+            pp.nlrchange = 0;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
+
+                if(xps.mainsel >xps.secondsel)
+                {
+                    pp.nlrchange = 2;
+                }
+                else
+                {
+                    pp.nlrchange = 1;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
+            pp.mWidth = getwidth(xps.mpRoad,xps.mainsel,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1;
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+            pp.hdg = pp.hdg + M_PI;
+            xvectorPP.push_back(pp);
+        }
+
+//        for(i=0;i<(nchange1- nchangepoint/2);i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off1;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
+//        {
+
+//            PlanPoint pp = xvPP.at(i);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+
+//            pp.mfDisToRoadLeft = offx;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+//            if(nlane1 == nlane2)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane1 < nlane2)
+//                {
+//                    pp.lanmp = -1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = 1;
+//                }
+
+//                if(i<nchange1)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+
+//            }
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off2;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
+//        {
+
+//            PlanPoint pp = xvPP.at(i);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
+//            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+
+//            pp.mfDisToRoadLeft = offx;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+//            if(nlane2 == nlane3)
+//            {
+//                pp.mWidth = flanewidth1;
+//                pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//                pp.lanmp = 0;
+//                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//            }
+//            else
+//            {
+//                if(nlane2 < nlane3)
+//                {
+//                    pp.lanmp = -1;
+//                }
+//                else
+//                {
+//                    pp.lanmp = 1;
+//                }
+
+//                if(i<nchange2)
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+//                else
+//                {
+//                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
+//                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
+//                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
+//                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+//                }
+
+//            }
+
+//            xvectorPP.push_back(pp);
+
+//        }
+//        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
+//        {
+//            PlanPoint pp = xvPP.at(i);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
+//            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
+//            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
+
+//            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+//            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+//            pp.lanmp = 0;
+//            pp.mfDisToRoadLeft = off3;
+//            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
+//            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
+//            xvectorPP.push_back(pp);
+
+//        }
+
+    }
+
+
+
+    CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,nchange1,nchange2,nchangepoint);
+
+    if(xps.mpRoad->GetRoadBorrowCount()>0)
+    {
+        CalcBorringRoad(xps,xvectorPP);
+    }
+
+
+    if(xps.mnStartLaneSel > 0)
+    {
+        std::vector<PlanPoint> xvvPP;
+        int nvsize = xvectorPP.size();
+        for(i=0;i<nvsize;i++)
+        {
+            xvvPP.push_back(xvectorPP.at(nvsize-1-i));
+        }
+        AddSignalMark(xps.mpRoad,xps.mainsel,xvvPP);
+        return xvvPP;
+    }
+
+//    pRoad->GetLaneSection(xps.msectionid)
+
+    AddSignalMark(xps.mpRoad,xps.mainsel,xvectorPP);
+    return xvectorPP;
+}
+
+
+static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvectorPP)
+{
+    if(pRoad->GetSignalCount() == 0)return;
+    int nsigcount = pRoad->GetSignalCount();
+    int i;
+    for(i=0;i<nsigcount;i++)
+    {
+        Signal * pSig = pRoad->GetSignal(i);
+        int nfromlane = -100;
+        int ntolane = 100;
+
+        if(pSig->GetlaneValidityCount()>0)
+        {
+            bool bValid = false;
+            unsigned int j;
+            std::vector<signal_laneValidity> * pvectorlanevalid = pSig->GetlaneValidityVector();
+            unsigned int nsize = static_cast<unsigned int>(pvectorlanevalid->size());
+            for(j=0;j<nsize;j++)
+            {
+                signal_laneValidity * pSig_laneValidity = &pvectorlanevalid->at(j);
+                nfromlane = pSig_laneValidity->GetfromLane();
+                ntolane = pSig_laneValidity->GettoLane();
+                if((nlane >= nfromlane)&&(nlane<=ntolane))
+                {
+                    bValid = true;
+                    break;
+                }
+            }
+            if(bValid == false)continue;
+        }
+
+//        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
+//        if(pSig_laneValidity != 0)
+//        {
+//            nfromlane = pSig_laneValidity->GetfromLane();
+//            ntolane = pSig_laneValidity->GettoLane();
+//        }
+//        if((nlane < 0)&&(nfromlane >= 0))
+//        {
+//            continue;
+//        }
+//        if((nlane > 0)&&(ntolane<=0))
+//        {
+//            continue;
+//        }
+
+        double s = pSig->Gets();
+        double fpos = s/pRoad->GetRoadLength();
+        if(nlane > 0)fpos = 1.0 -fpos;
+        int npos = fpos *xvectorPP.size();
+        if(npos <0)npos = 0;
+        if(npos>=xvectorPP.size())npos = xvectorPP.size()-1;
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos > 0)npos--;
+            else break;
+        }
+        while(xvectorPP.at(npos).nSignal != -1)
+        {
+            if(npos < (xvectorPP.size()-1))npos++;
+            else break;
+        }
+        xvectorPP.at(npos).nSignal = atoi(pSig->Gettype().data());
+
+
+    }
+}
+
+static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection,Road * pRoad,GeometryBlock * pgeob,
+                                          Road * pRoad_obj,GeometryBlock * pgeob_obj,
+                                           const double x_now,const double y_now,const double head,
+                                           double nearx,double neary, double nearhead,
+                                           double nearx_obj,double  neary_obj,const double fvehiclewidth,const double flen = 1000)
+{
+
+  std::vector<PlanPoint> xvectorPPs;
+
+  double fspace = 0.1;
+
+  if(flen<2000)fspace = 0.1;
+  else
+  {
+      if(flen<5000)fspace = 0.3;
+      else
+      {
+          if(flen<10000)fspace = 0.5;
+          else
+              fspace = 1.0;
+      }
+  }
+
+  int i;
+
+
+//  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0].mpRoad);
+  std::vector<PlanPoint> xvectorPP = GetPoint( xpathsection[0],fspace);
+
+
+  int indexstart = indexinroadpoint(xvectorPP,nearx,neary);
+
+  std::vector<PlanPoint> xvPP = GetLanePoint(xpathsection[0],xvectorPP,fvehiclewidth);
+
+  if(xpathsection[0].mainsel < 0)
+  {
+  for(i=indexstart;i<xvPP.size();i++)
+  {
+      xvectorPPs.push_back(xvPP.at(i));
+  }
+  }
+  else
+  {
+
+      for(i=(xvPP.size() -indexstart);i<xvPP.size();i++)
+      {
+          PlanPoint pp;
+          pp = xvPP.at(i);
+//          pp.hdg = pp.hdg +M_PI;
+          xvectorPPs.push_back(pp);
+      }
+  }
+
+  int npathlast = xpathsection.size() - 1;
+//  while(npathlast >= 0)
+//  {
+//    if(npathlast == 0)break;
+//    if(xpathsection[npathlast].mpRoad != xpathsection[npathlast - 1].mpRoad)break;
+//  }
+  for(i=1;i<(npathlast);i++)
+  {
+//      if(xpathsection[i].mpRoad == xpathsection[i-1].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[i-1].mainsel >0)
+//          {
+//            continue;
+//          }
+//      }
+//      if(xpathsection[i].mpRoad == xpathsection[npathlast].mpRoad)
+//      {
+//          if(xpathsection[i].mainsel * xpathsection[npathlast].mainsel > 0)
+//          {
+//            break;
+//          }
+//      }
+ //     xvectorPP = GetPoint( xpathsection[i].mpRoad);
+      xvectorPP = GetPoint( xpathsection[i],fspace);
+      xvPP = GetLanePoint(xpathsection[i],xvectorPP,fvehiclewidth);
+//      std::cout<<" road id: "<<xpathsection[i].mroadid<<" section: "
+//              <<xpathsection[i].msectionid<<" size is "<<xvectorPP.size()<<std::endl;
+//      std::cout<<" road id is "<<xpathsection[i].mpRoad->GetRoadId().data()<<" size is "<<xvPP.size()<<std::endl;
+      int j;
+      for(j=0;j<xvPP.size();j++)
+      {
+
+          PlanPoint pp;
+          pp = xvPP.at(j);
+//          pp.hdg = pp.hdg +M_PI;
+          xvectorPPs.push_back(pp);
+      }
+  }
+
+  xvectorPP = GetPoint(xpathsection[npathlast],fspace);
+
+
+//  xvectorPP = GetPoint(xpathsection[npathlast].mpRoad);
+  int indexend = indexinroadpoint(xvectorPP,nearx_obj,neary_obj);
+  xvPP = GetLanePoint(xpathsection[npathlast],xvectorPP,fvehiclewidth);
+  int nlastsize;
+  if(xpathsection[npathlast].mainsel<0)
+  {
+      nlastsize = indexend;
+  }
+  else
+  {
+      if(indexend>0) nlastsize = xvPP.size() - indexend;
+      else nlastsize = xvPP.size();
+  }
+  for(i=0;i<nlastsize;i++)
+  {
+      xvectorPPs.push_back(xvPP.at(i));
+
+  }
+
+  //Find StartPoint
+//  if
+
+//  int a = 1;
+
+
+
+
+  return xvectorPPs;
+}
+
+std::vector<PlanPoint> gTempPP;
+int getPoint(double q[3], double x, void* user_data) {
+//    printf("%f, %f, %f, %f\n", q[0], q[1], q[2], x);
+    PlanPoint pp;
+    pp.x = q[0];
+    pp.y = q[1];
+    pp.hdg = q[2];
+    pp.dis = x;
+    pp.speed = *((double *)user_data);
+    gTempPP.push_back(pp);
+//    std::cout<<pp.x<<" "<<" "<<pp.y<<" "<<pp.hdg<<std::endl;
+    return 0;
+}
+
+/**
+ * @brief getlanesel
+ * @param nSel
+ * @param pLaneSection
+ * @param nlr
+ * @return
+ */
+int getlanesel(int nSel,LaneSection * pLaneSection,int nlr)
+{
+    int nlane = nSel;
+    int mainselindex = 0;
+    if(nlr == 2)nlane = nlane*(-1);
+    int nlanecount = pLaneSection->GetLaneCount();
+
+    int i;
+    int nTrueSel = nSel;
+    if(nTrueSel <= 1)nTrueSel= 1;
+    int nCurSel = 1;
+    if(nlr ==  2)nCurSel = nCurSel *(-1);
+    bool bOK = false;
+    int nxsel = 1;
+    int nlasttid = 0;
+    bool bfindlast = false;
+    while(bOK == false)
+    {
+        bool bHaveDriving = false;
+        int ncc = 0;
+        for(i=0;i<nlanecount;i++)
+        {
+            Lane * pLane = pLaneSection->GetLane(i);
+            if(strncmp(pLane->GetType().data(),"driving",255) == 0 )
+            {
+                if((nlr == 1)&&(pLane->GetId()>0))
+                {
+                    ncc++;
+                }
+                if((nlr == 2)&&(pLane->GetId()<0))
+                {
+                    ncc++;
+                }
+                if(ncc == nxsel)
+                {
+                    bHaveDriving = true;
+                    bfindlast = true;
+                    nlasttid = pLane->GetId();
+                    break;
+                }
+            }
+        }
+        if(bHaveDriving == true)
+        {
+            if(nxsel == nTrueSel)
+            {
+                return nlasttid;
+            }
+            else
+            {
+                nxsel++;
+            }
+        }
+        else
+        {
+            if(bfindlast)
+            {
+                return nlasttid;
+            }
+            else
+            {
+                std::cout<<"can't find lane."<<std::endl;
+                return 0;
+            }
+            //Use Last
+        }
+
+    }
+
+    return 0;
+
+    int k;
+    bool bFind = false;
+    while(bFind == false)
+    {
+        for(k=0;k<nlanecount;k++)
+        {
+            Lane * pLane = pLaneSection->GetLane(k);
+            if((nlane == pLane->GetId())&&(strncmp(pLane->GetType().data(),"driving",255) == 0))
+            {
+                bFind = true;
+                mainselindex = k;
+                break;
+            }
+        }
+        if(bFind)continue;
+        if(nlr == 1)nlane--;
+        else nlane++;
+        if(nlane == 0)
+        {
+            std::cout<<" Fail. can't find lane"<<std::endl;
+            break;
+        }
+    }
+    return nlane;
+}
+
+
+
+void checktrace(std::vector<PlanPoint> & xPlan)
+{
+    int i;
+    int nsize = xPlan.size();
+    for(i=1;i<nsize;i++)
+    {
+        double dis = sqrt(pow(xPlan.at(i).x - xPlan.at(i-1).x,2) + pow(xPlan.at(i).y - xPlan.at(i-1).y,2));
+        if(dis > 0.3)
+        {
+            double hdg =  CalcHdg(QPointF(xPlan.at(i-1).x,xPlan.at(i-1).y),QPointF(xPlan.at(i).x,xPlan.at(i).y));
+
+            int ncount = dis/0.1;
+            int j;
+            for(j=1;j<ncount;j++)
+            {
+                double x, y;
+
+                PlanPoint pp;
+                pp.x = xPlan.at(i-1).x + j*0.1 *cos(hdg);
+                pp.y = xPlan.at(i-1).y + j*0.1 *sin(hdg);
+                pp.hdg = hdg;
+                xPlan.insert(xPlan.begin()+i+j-1,pp);
+
+            }
+            qDebug("dis is %f",dis);
+        }
+    }
+}
+
+
+
+void ChangeLane(std::vector<PlanPoint> & xvectorPP)
+{
+    int i = 0;
+    int nsize = xvectorPP.size();
+    int nstart = -1;
+    int nend = -1;
+    for(i=0;i<nsize;i++)
+    {
+        if((xvectorPP[i].mfSecx == xvectorPP[i].x)&&(xvectorPP[i].mfSecy == xvectorPP[i].y))
+        {
+
+        }
+        else
+        {
+            int k;
+            for(k=i;k<=(nsize-1);k++)
+            {
+                if((fabs(xvectorPP[k].mfSecx - xvectorPP[k].x)<0.001)&&(fabs(xvectorPP[k].mfSecy - xvectorPP[k].y)<0.001))
+                {
+                    break;
+                }
+            }
+            if(k>(nsize-1))k=nsize-1;
+            int nnum = k-i;
+            int nchangepoint = 300;
+            double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
+                                   +pow(xvectorPP[k].y - xvectorPP[i].y,2));
+            const double fMAXCHANGE = 50;
+
+            if(froadlen<fMAXCHANGE)
+            {
+                nchangepoint = nnum;
+            }
+            else
+            {
+                nchangepoint = (fMAXCHANGE/froadlen) * nnum;
+            }
+
+            qDebug(" road %d len is %f changepoint is %d nnum is %d",
+                   xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);
+
+            nstart = i + nnum/2 -nchangepoint/2;
+            nend = i+nnum/2 + nchangepoint/2;
+            if(nnum<300)
+            {
+                nstart = i;
+                nend = k;
+            }
+            int j;
+            for(j=i;j<nstart;j++)
+            {
+                xvectorPP[j].x = xvectorPP[j].mfSecx;
+                xvectorPP[j].y = xvectorPP[j].mfSecy;
+            }
+            nnum = nend - nstart;
+            int nlast = k-1;
+            if(k==(nsize-1))nlast = k;
+            for(j=nstart;j<=nlast;j++)
+            {
+                double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
+                                   +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
+                double foff = 0;
+                if(j<nend)
+                    foff = fdis *(j-nstart)/nnum;
+                else
+                    foff = fdis;
+                if(xvectorPP[j].nlrchange == 1)
+                {
+//                    std::cout<<"foff is "<<foff<<std::endl;
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft - foff; //+ fdis
+                }
+                else
+                {
+                    xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg - M_PI/2.0);
+                    xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg - M_PI/2.0);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  +foff;//- fdis
+                }
+            }
+            i =k;
+
+        }
+    }
+
+    if(nstart != -1)
+    {
+        for(i=0;i<nstart;i++)
+        {
+            xvectorPP[i].nlrchange = 0;
+            if(i>nsize)break;
+        }
+    }
+    if(nend >0)
+    {
+        for(i=nend;i<nsize;i++)
+        {
+            xvectorPP[i].nlrchange = 0;
+            if(i>nsize)break;
+        }
+    }
+
+}
+
+#include <QFile>
+
+
+int OptimizeStart(std::vector<PlanPoint> & xPlan,const double x_now,const double y_now,const double head)
+{
+    if(xPlan.size()<1)return -1;
+
+    double hdgdiff = head - xPlan[0].hdg;
+    while(hdgdiff>=(2.0*M_PI))hdgdiff = hdgdiff - 2.0*M_PI;
+    while(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
+
+
+    PlanPoint ppz = xPlan[0];
+
+
+
+    double fdis = sqrt(pow(x_now - xPlan[0].x,2)+pow(y_now - xPlan[0].y,2));
+
+    if(fdis>30)return -2;
+    if(fdis<0.3)return -3;
+
+    double fhdg = CalcHdg(QPointF(x_now,y_now),QPointF(ppz.x,ppz.y));
+    double hdgdiff2 = fhdg - xPlan[0].hdg;
+    while(hdgdiff2>=(2.0*M_PI))hdgdiff2 = hdgdiff2 - 2.0*M_PI;
+    while(hdgdiff2<0)hdgdiff2 = hdgdiff2 + 2.0*M_PI;
+
+    if((hdgdiff >(0.7*M_PI/2.0)) && (hdgdiff<(1.3*M_PI/2.0)))
+    {
+
+
+        double fspace = 0.1;
+        double fS = 0;
+        double R = 4.5;
+        if(R*tan(( hdgdiff2)/2.0)>fdis)
+        {
+            R = fdis/(tan((hdgdiff2)/2.0));
+        }
+        double fCurveDis = R* hdgdiff2;
+        double fLineDis = fdis - R*tan(hdgdiff2/2.0);
+        double fSTotal = fLineDis + fCurveDis ;
+        std::vector<PlanPoint>  xPlanBefore;
+        while(fS < fLineDis)
+        {
+
+            PlanPoint pp = ppz;
+            pp.hdg = fhdg;
+            pp.x = x_now + fS*cos(fhdg);
+            pp.y = y_now + fS*sin(fhdg);
+            pp.mS = fS;
+            pp.speed = 1.0;
+            xPlanBefore.push_back(pp);
+            fS = fS + fspace;
+        }
+
+        while(fS<fSTotal)
+        {
+            double fRel = fS - fLineDis;
+            double fangle = fRel/R;
+            double xrelraw = R*cos(M_PI/2.0 - fangle);
+            double yrelraw = R*sin(M_PI/2.0 - fangle) - R;
+            double xrel = xrelraw * cos(fhdg) - yrelraw * sin(fhdg);
+            double yrel = xrelraw * sin(fhdg) + yrelraw * cos(fhdg);
+            
+            PlanPoint pp = ppz;
+            pp.hdg = fhdg - fangle;
+            pp.x = x_now + fLineDis*cos(fhdg) + xrel;
+            pp.y = y_now + fLineDis*sin(fhdg) + yrel;
+            pp.mS = fS;
+            pp.speed = 1.0;
+            xPlanBefore.push_back(pp);
+
+            fS = fS + fspace;
+        }
+//        while(fS<fdis)
+//        {
+//            PlanPoint pp = ppz;
+//            pp.hdg = fhdg;
+//            pp.x = x_now + fS*cos(fhdg);
+//            pp.y = y_now + fS*sin(fhdg);
+//            pp.mS = fS;
+//            pp.speed = 3.0;
+//            xPlanBefore.push_back(pp);
+//            fS = fS + fspace;
+//        }
+
+        double fEraseDis = 0;
+        double fEraseT  = R;
+        if(hdgdiff2>(M_PI/2.0))fEraseT = R*cos(hdgdiff2 - M_PI/2.0);
+        else fEraseT = R*sin(hdgdiff2);
+        while((fEraseDis<fEraseT) &&(xPlan.size()>=2))
+        {
+            double fPDis = sqrt(pow(xPlan[0].x - xPlan[1].x,2) + pow(xPlan[0].y - xPlan[1].y,2));
+            if((fEraseDis+fPDis)>(fEraseT))
+            {
+                break;
+            }
+            xPlan.erase(xPlan.begin());
+            fEraseDis = fEraseDis + fPDis;
+
+        }
+
+        if(xPlanBefore.size()>0)
+        {
+            int nsize = xPlanBefore.size();
+            int i;
+            for(i=(nsize-1);i>=0;i--)
+            {
+                xPlan.insert(xPlan.begin(),xPlanBefore[i]);
+            }
+        }
+
+        double fFixS = fSTotal - fEraseT;
+
+        int i;
+        int nsizeplan = xPlan.size();
+        int nsizebefore = xPlanBefore.size();
+        if(nsizeplan > nsizebefore)
+        {
+            for(i=nsizebefore;i<nsizeplan;i++)
+            {
+                xPlan[i].mS = xPlan[i].mS + fFixS;
+            }
+        }
+    }
+
+    return 0;
+}
+
+/**
+ * @brief MakePlan         全局路径规划
+ * @param pxd              有向图
+ * @param pxodr            OpenDrive地图
+ * @param x_now            当前x坐标
+ * @param y_now            当前y坐标
+ * @param head             当前航向
+ * @param x_obj            目标点x坐标
+ * @param y_obj            目标点y坐标
+ * @param obj_dis          目标点到路径的距离
+ * @param srcnearthresh    当前点离最近路径的阈值，如果不在这个范围，寻找失败
+ * @param dstnearthresh    目标点离最近路径的阈值，如果不在这个范围，寻找失败
+ * @param nlanesel         车道偏好，1为内车道，数值越大越偏外，如果数值不满足，会选择最靠外的。
+ * @param xPlan            返回的轨迹点
+ * @return                 0 成功  <0 失败
+ */
+int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+             const double x_obj,const double y_obj,const double & obj_dis,
+             const double srcnearthresh,const double dstnearthresh,
+             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth )
+{
+    double  s;double nearx;
+    double  neary;double  nearhead;
+    Road * pRoad;GeometryBlock * pgeob;
+    Road * pRoad_obj;GeometryBlock * pgeob_obj;
+    double  s_obj;double nearx_obj;
+    double  neary_obj;double  nearhead_obj;
+    int lr_end = 2;
+    int lr_start = 2;
+    double fs1,fs2;
+//    int nfind = GetNearPoint(x_now,y_now,pxodr,&pRoad,&pgeob,s,nearx,neary,nearhead,srcnearthresh);
+
+    std::vector<iv::nearroad> xvectornearstart;
+    std::vector<iv::nearroad> xvectornearend;
+
+    GetNearPointWithHead(x_now,y_now,head,pxodr,xvectornearstart,srcnearthresh,true);
+    GetNearPointWithHead(x_obj,y_obj,0,pxodr,xvectornearend,dstnearthresh,false);
+
+    if(xvectornearstart.size()<1)
+    {
+        std::cout<<" plan fail. can't find start point."<<std::endl;
+        return -1;
+    }
+    if(xvectornearend.size() < 1)
+    {
+        std::cout<<" plan fail. can't find end point."<<std::endl;
+        return -2;
+    }
+
+    std::cout<<" start size: "<<xvectornearstart.size()<<std::endl;
+    std::cout<<" end size: "<<xvectornearend.size()<<std::endl;
+
+    std::vector<std::vector<PlanPoint>> xvectorplans;
+    std::vector<double> xvectorroutedis;
+
+    int i;
+    int j;
+    for(i=0;i<(int)xvectornearstart.size();i++)
+    {
+        for(j=0;j<(int)xvectornearend.size();j++)
+        {
+            iv::nearroad * pnearstart = &xvectornearstart[i];
+            iv::nearroad * pnearend = &xvectornearend[j];
+            pRoad = pnearstart->pObjRoad;
+            pgeob = pnearstart->pgeob;
+            pRoad_obj = pnearend->pObjRoad;
+            pgeob_obj = pnearend->pgeob;
+            lr_start = pnearstart->lr;
+            lr_end = pnearend->lr;
+
+            std::cout<<" lr start: "<<lr_start<<" lr end "<<lr_end<<std::endl;
+
+            nearx = pnearstart->nearx;
+            neary = pnearstart->neary;
+
+            nearx_obj = pnearend->nearx;
+            neary_obj = pnearend->neary;
+
+            bool bNeedDikstra = true;
+            if((atoi(pRoad->GetRoadId().data()) == atoi(pRoad_obj->GetRoadId().data()))&&(lr_start == lr_end) &&(pRoad->GetLaneSectionCount() == 1))
+            {
+                std::vector<PlanPoint> xvPP = GetPoint(pRoad);
+                int nindexstart = indexinroadpoint(xvPP,nearx,neary);
+                int nindexend =  indexinroadpoint(xvPP,nearx_obj,neary_obj);
+                int nlane = getlanesel(nlanesel,pRoad->GetLaneSection(0),lr_start);
+                AddSignalMark(pRoad,nlane,xvPP);
+                if((nindexend >nindexstart)&&(lr_start == 2))
+                {
+                    bNeedDikstra = false;
+                }
+                if((nindexend <nindexstart)&&(lr_start == 1))
+                {
+                    bNeedDikstra = false;
+                }
+                if(bNeedDikstra == false)
+                {
+
+                    std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(pRoad);
+                    std::vector<int> xvectorindex1;
+
+                    xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
+
+                    double foff = getoff(pRoad,nlane);
+
+                    foff = foff + 0.0;
+                    std::vector<PlanPoint> xvectorPP;
+                    int i = nindexstart;
+                    while(i!=nindexend)
+                    {
+
+                        if(lr_start == 2)
+                        {
+                            PlanPoint pp = xvPP.at(i);
+                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
+                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
+                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                            pp.lanmp = 0;
+                            pp.mfDisToRoadLeft = foff *(-1.0);
+                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mLaneCur = abs(nlane);
+                            pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
+                            pp.mLaneCount = pRoad->GetRightDrivingLaneCount(pp.mS);
+                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+                            xvectorPP.push_back(pp);
+                            i++;
+                        }
+                        else
+                        {
+                            PlanPoint pp = xvPP.at(i);
+                            foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
+                            pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
+                            pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
+                            pp.hdg = pp.hdg + M_PI;
+
+                            pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                            pp.lanmp = 0;
+                            pp.mfDisToRoadLeft = foff;
+                            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                            pp.mLaneCur = abs(nlane);
+                            pp.mVectorLaneWidth = pRoad->GetDrivingLaneWidthVector(pp.mS,lr_start);
+                            pp.mLaneCount = pRoad->GetLeftDrivingLaneCount(pp.mS);
+                            GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed,pp.mbNoavoid);
+
+
+                            xvectorPP.push_back(pp);
+                            i--;
+                        }
+                    }
+
+                    for(i=0;i<(int)xvectorPP.size();i++)
+                    {
+                        xvectorPP[i].mfCurvature = pRoad->GetRoadCurvature(xvectorPP[i].mS);
+                    }
+
+                    pathsection xps;
+                    xps.mbStartToMainChange = false;
+                    xps.mbMainToEndChange = false;
+         //           CalcInLaneAvoid(xps,xvectorPP,fvehiclewidth,0,0,0);
+                    xPlan = xvectorPP;
+
+                    xvectorplans.push_back(xvectorPP);
+                    xvectorroutedis.push_back(xvectorPP.size() * 0.1);
+
+                }
+            }
+
+            if(bNeedDikstra)
+            {
+                bool bSuc = false;
+            std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,bSuc,fs1,fs2);
+            if(xpath.size()<1)
+            {
+                continue;
+            }
+            if(bSuc == false)
+            {
+                continue;
+            }
+            double flen = pxd->getpathlength(xpath);
+            std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
+
+            std::vector<PlanPoint> xvectorPP = GetPlanPoint(xpathsection,pRoad,pgeob,pRoad_obj,pgeob_obj,x_now,y_now,
+                         head,nearx,neary,nearhead,nearx_obj,neary_obj,fvehiclewidth,flen);
+
+            ChangeLane(xvectorPP);
+            xvectorplans.push_back(xvectorPP);
+            xvectorroutedis.push_back(flen);
+
+            }
+
+        }
+    }
+
+    if(xvectorplans.size() > 0)
+    {
+        if(xvectorplans.size() == 1)
+        {
+            xPlan = xvectorplans[0];
+        }
+        else
+        {
+            int nsel = 0;
+            double fdismin = xvectorroutedis[0];
+            for(i=1;i<(int)xvectorroutedis.size();i++)
+            {
+                if(xvectorroutedis[i]<fdismin)
+                {
+                    nsel = i;
+                }
+            }
+            xPlan = xvectorplans[nsel];
+        }
+        OptimizeStart(xPlan,x_now,y_now,head);
+        return 0;
+    }
+    std::cout<<" plan fail. can't find path."<<std::endl;
+    return -1000;
+
+}

src/driver/driver_map_xodrload/globalplan.h

@@ -1,26 +1,26 @@
-#ifndef GLOBALPLAN_H
-#define GLOBALPLAN_H
-#include "OpenDrive/OpenDrive.h"
-
-#include <vector>
-
-#include "xodrfunc.h"
-
-#include "xodrdijkstra.h"
-
-#include "planpoint.h"
-
-class LaneChangePoint
-{
-    int nRoadID = -1;
-    double mS = 0;
-};
-
-int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
-                 double & neary,double & nearhead,const double nearthresh);
-
-int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
-             const double x_obj,const double y_obj,const double & obj_dis,
-             const double srcnearthresh,const double dstnearthresh,
-             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
-#endif // GLOBALPLAN_H
+#ifndef GLOBALPLAN_H
+#define GLOBALPLAN_H
+#include "OpenDrive/OpenDrive.h"
+
+#include <vector>
+
+#include "xodrfunc.h"
+
+#include "xodrdijkstra.h"
+
+#include "planpoint.h"
+
+class LaneChangePoint
+{
+    int nRoadID = -1;
+    double mS = 0;
+};
+
+int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,
+                 double & neary,double & nearhead,const double nearthresh);
+
+int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+             const double x_obj,const double y_obj,const double & obj_dis,
+             const double srcnearthresh,const double dstnearthresh,
+             const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
+#endif // GLOBALPLAN_H

src/driver/driver_map_xodrload/gnss_coordinate_convert.cpp

@@ -1,153 +1,189 @@
-#include <gnss_coordinate_convert.h>
-
-void gps2xy(double J4, double K4, double *x, double *y)
-{
-    int L4 = (int)((K4 - 1.5) / 3) + 1;
-    double M4 = K4 - (L4 * 3);
-    double N4 = sin(J4 * 3.1415926536 / 180);
-    double O4 = cos(J4 * 3.1415926536 / 180);
-    double P4 = tan(J4 * 3.1415926536 / 180);
-    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
-    double R4 = sqrt(0.006738525414683) * O4;
-    double S4 = sqrt(1 + R4 * R4);
-    double T4 = 6399698.901783 / S4;
-    double U4 = (T4 / S4) / S4;
-    double V4 = O4 * M4 * 3.1415926536 / 180;
-    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
-    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
-    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
-    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
-
-    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
-    *x = 500000 + T4 * V4 * (Y4 + Z4);
-}
-
-//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
-void GaussProjCal(double longitude, double latitude, double *X, double *Y)
-{
-    int ProjNo = 0; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double a, f, e2, ee, NN, T, C, A, M, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    ZoneWide = 6; ////6度带宽
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ProjNo = (int)(longitude / ZoneWide);
-    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI;
-    latitude0 = 0;
-    longitude1 = longitude * iPI; //经度转换为弧度
-    latitude1 = latitude * iPI; //纬度转换为弧度
-    e2 = 2 * f - f * f;
-    ee = e2 * (1.0 - e2);
-    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
-    T = tan(latitude1)*tan(latitude1);
-    C = ee * cos(latitude1)*cos(latitude1);
-    A = (longitude1 - longitude0)*cos(latitude1);
-    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
-        *e2 / 1024)*sin(2 * latitude1)
-        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
-    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
-    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
-        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
-    X0 = 1000000L * (ProjNo + 1) + 500000L;
-    Y0 = 0;
-    xval = xval + X0; yval = yval + Y0;
-    *X = xval;
-    *Y = yval;
-}
-
-
-
-//=======================zhaobo0904
-#define PI  3.14159265358979
-void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
-{
-    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
-    double a = 6378140.0;
-    double e2 = 0.006694384999588;
-    double ep2 = e2/(1-e2);
-
-    // 原点所在经度
-    double lon_origin = 6.0*int(lon/6) + 3.0;
-    lon_origin *= PI / 180.0;
-
-    double k0 = 0.9996;
-
-    // 角度转弧度
-    double lat1 = lat * PI / 180.0;
-    double lon1 = lon * PI / 180.0;
-
-
-    // 经线在该点处的曲率半径,
-    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
-
-
-    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
-    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
-    // 首先计算前四项的系数 a1~a4.
-    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
-    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
-    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
-    double a4 = (35*e2*e2*e2)/3072;
-    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
-
-    // 辅助量
-    double T = tan(lat1)*tan(lat1);
-    double C = ep2*cos(lat1)*cos(lat1);
-    double A = (lon1 - lon_origin)*cos(lat1);
-
-    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
-    *Y = M + N * tan(lat1) * ((A*A)/2 +
-                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
-                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
-
-
-    *Y *= k0;
-    return;
-}
-//==========================================================
-
-
-
-
-
-//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
-{
-    int ProjNo; int ZoneWide; ////带宽
-    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
-    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-    ZoneWide = 6; ////6度带宽
-    ProjNo = (int)(X / 1000000L); //查找带号
-    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
-    longitude0 = longitude0 * iPI; //中央经线
-    X0 = ProjNo * 1000000L + 500000L;
-    Y0 = 0;
-    xval = X - X0; yval = Y - Y0; //带内大地坐标
-    e2 = 2 * f - f * f;
-    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
-    ee = e2 / (1 - e2);
-    M = yval;
-    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
-    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
-                4 * u)
-            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
-    C = ee * cos(fai)*cos(fai);
-    T = tan(fai)*tan(fai);
-    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
-    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
-                                                                                       (fai)*sin(fai)));
-    D = xval / NN;
-    //计算经度(Longitude) 纬度(Latitude)
-    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
-                               *D*D*D*D / 120) / cos(fai);
-    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
-                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
-    //转换为度 DD
-    *longitude = longitude1 / iPI;
-    *latitude = latitude1 / iPI;
-}
+#include <gnss_coordinate_convert.h>
+
+
+
+#ifdef USE_UTM
+#include <GeographicLib/UTMUPS.hpp>
+#include <iostream>
+#endif
+
+void gps2xy(double J4, double K4, double *x, double *y)
+{
+    int L4 = (int)((K4 - 1.5) / 3) + 1;
+    double M4 = K4 - (L4 * 3);
+    double N4 = sin(J4 * 3.1415926536 / 180);
+    double O4 = cos(J4 * 3.1415926536 / 180);
+    double P4 = tan(J4 * 3.1415926536 / 180);
+    double Q4 = 111134.8611 * J4 - N4 * O4 * (32005.7799 + 133.9238 * N4 * N4 + 0.6973 * N4 * N4 * N4 * N4 + 0.0039 * N4 * N4 * N4 * N4 * N4 * N4);
+    double R4 = sqrt(0.006738525414683) * O4;
+    double S4 = sqrt(1 + R4 * R4);
+    double T4 = 6399698.901783 / S4;
+    double U4 = (T4 / S4) / S4;
+    double V4 = O4 * M4 * 3.1415926536 / 180;
+    double W4 = 0.5 + (5 - P4 * P4 + 9 * R4 * R4 + 4 * R4 * R4 * R4 * R4) * V4 * V4 / 24;
+    double X4 = V4 * V4 * V4 * V4 / 720 * (61 + (P4 * P4 - 58) * P4 * P4);
+    double Y4 = 1 + V4 * V4 * (1 - P4 * P4 + R4 * R4) / 6;
+    double Z4 = V4 * V4 * V4 * V4 * (5 - 18 * P4 * P4 * P4 * P4 * P4 * P4 + 14 * R4 * R4 - 58 * R4 * R4 * P4 * P4) / 120;
+
+    *y = Q4 + T4 * P4 * V4 * V4 * (W4 + X4);
+    *x = 500000 + T4 * V4 * (Y4 + Z4);
+}
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+{
+
+#ifdef USE_UTM
+
+    int zone{};
+    bool northp{};
+    try {
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, *X,*Y);
+      zone = zone;
+    } catch (GeographicLib::GeographicErr& e) {
+  //    throw ForwardProjectionError(e.what());
+        std::cout<<"GeographicLib::UTMUPS::Forward Fail. what: "<<e.what()<<std::endl;
+    }
+#else
+    int ProjNo = 0; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double a, f, e2, ee, NN, T, C, A, M, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    ZoneWide = 6; ////6度带宽
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ProjNo = (int)(longitude / ZoneWide);
+    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI;
+    latitude0 = 0;
+    longitude1 = longitude * iPI; //经度转换为弧度
+    latitude1 = latitude * iPI; //纬度转换为弧度
+    e2 = 2 * f - f * f;
+    ee = e2 * (1.0 - e2);
+    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+    T = tan(latitude1)*tan(latitude1);
+    C = ee * cos(latitude1)*cos(latitude1);
+    A = (longitude1 - longitude0)*cos(latitude1);
+    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+        *e2 / 1024)*sin(2 * latitude1)
+        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+    X0 = 1000000L * (ProjNo + 1) + 500000L;
+    Y0 = 0;
+    xval = xval + X0; yval = yval + Y0;
+    *X = xval;
+    *Y = yval;
+#endif
+}
+
+
+
+//=======================zhaobo0904
+#define PI  3.14159265358979
+void ZBGaussProjCal(double lon, double lat, double *X, double *Y)
+{
+    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
+    double a = 6378140.0;
+    double e2 = 0.006694384999588;
+    double ep2 = e2/(1-e2);
+
+    // 原点所在经度
+    double lon_origin = 6.0*int(lon/6) + 3.0;
+    lon_origin *= PI / 180.0;
+
+    double k0 = 0.9996;
+
+    // 角度转弧度
+    double lat1 = lat * PI / 180.0;
+    double lon1 = lon * PI / 180.0;
+
+
+    // 经线在该点处的曲率半径,
+    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));
+
+
+    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
+    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
+    // 首先计算前四项的系数 a1~a4.
+    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
+    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
+    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
+    double a4 = (35*e2*e2*e2)/3072;
+    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));
+
+    // 辅助量
+    double T = tan(lat1)*tan(lat1);
+    double C = ep2*cos(lat1)*cos(lat1);
+    double A = (lon1 - lon_origin)*cos(lat1);
+
+    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
+    *Y = M + N * tan(lat1) * ((A*A)/2 +
+                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
+                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);
+
+
+    *Y *= k0;
+    return;
+}
+//==========================================================
+
+
+
+
+
+//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
+{
+
+#ifdef USE_UTM
+    int zone = 50;
+    bool isInNorthernHemisphere_ = true;
+    try {
+      GeographicLib::UTMUPS::Reverse(zone, isInNorthernHemisphere_, X,
+                                     Y, *latitude, *longitude);
+    } catch (GeographicLib::GeographicErr& e) {
+      std::cout<<"GeographicLib::UTMUPS::Reverse what:  "<<e.what()<<std::endl;
+    }
+
+#else
+
+    int ProjNo; int ZoneWide; ////带宽
+    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
+    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+    ZoneWide = 6; ////6度带宽
+    ProjNo = (int)(X / 1000000L); //查找带号
+    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
+    longitude0 = longitude0 * iPI; //中央经线
+    X0 = ProjNo * 1000000L + 500000L;
+    Y0 = 0;
+    xval = X - X0; yval = Y - Y0; //带内大地坐标
+    e2 = 2 * f - f * f;
+    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
+    ee = e2 / (1 - e2);
+    M = yval;
+    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
+    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
+                4 * u)
+            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
+    C = ee * cos(fai)*cos(fai);
+    T = tan(fai)*tan(fai);
+    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
+    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
+                                                                                       (fai)*sin(fai)));
+    D = xval / NN;
+    //计算经度(Longitude) 纬度(Latitude)
+    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
+                               *D*D*D*D / 120) / cos(fai);
+    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
+                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
+    //转换为度 DD
+    *longitude = longitude1 / iPI;
+    *latitude = latitude1 / iPI;
+
+#endif
+}

src/driver/driver_map_xodrload/gnss_coordinate_convert.h

@@ -1,27 +1,27 @@
-#pragma once
-#ifndef _IV_PERCEPTION_GNSS_CONVERT_
-#define _IV_PERCEPTION_GNSS_CONVERT_
-
-#include <math.h>
-//double nmeaConvert2Lat(string lat)
-//{
-//	Console.WriteLine(lat);
-//	double nmea_d = double.Parse(lat.Substring(0, 2));
-//	double nmea_m = double.Parse(lat.Substring(2, 6));
-//	return nmea_d + nmea_m / 60;
-//}
-//
-//double nmeaConvert2Lon(string lon)
-//{
-//	Console.WriteLine(lon);
-//	double nmea_d = double.Parse(lon.Substring(0, 3));
-//	double nmea_m = double.Parse(lon.Substring(3, 7));
-//	return nmea_d + nmea_m / 60;
-//}
-
-void gps2xy(double , double , double *, double *);
-void GaussProjCal(double longitude, double latitude, double *X, double *Y);
-void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
-void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
-
-#endif // !_IV_PERCEPTION_GNSS_CONVERT_
+#pragma once
+#ifndef _IV_PERCEPTION_GNSS_CONVERT_
+#define _IV_PERCEPTION_GNSS_CONVERT_
+
+#include <math.h>
+//double nmeaConvert2Lat(string lat)
+//{
+//	Console.WriteLine(lat);
+//	double nmea_d = double.Parse(lat.Substring(0, 2));
+//	double nmea_m = double.Parse(lat.Substring(2, 6));
+//	return nmea_d + nmea_m / 60;
+//}
+//
+//double nmeaConvert2Lon(string lon)
+//{
+//	Console.WriteLine(lon);
+//	double nmea_d = double.Parse(lon.Substring(0, 3));
+//	double nmea_m = double.Parse(lon.Substring(3, 7));
+//	return nmea_d + nmea_m / 60;
+//}
+
+void gps2xy(double , double , double *, double *);
+void GaussProjCal(double longitude, double latitude, double *X, double *Y);
+void ZBGaussProjCal(double longitude, double latitude, double *X, double *Y);
+void GaussProjInvCal(double X, double Y, double *longitude, double *latitude);
+
+#endif // !_IV_PERCEPTION_GNSS_CONVERT_

src/driver/driver_map_xodrload/main.cpp

@@ -1,1252 +1,1606 @@
-#include <QCoreApplication>
-#include <math.h>
-#include <string>
-#include <thread>
-
-#include <QFile>
-
-#include "OpenDrive/OpenDrive.h"
-#include "OpenDrive/OpenDriveXmlParser.h"
-
-#include "globalplan.h"
-
-#include "gpsimu.pb.h"
-
-#include "mapdata.pb.h"
-
-#include "v2x.pb.h"
-
-#include "modulecomm.h"
-
-#include "xmlparam.h"
-
-#include "gps_type.h"
-
-#include "xodrdijkstra.h"
-
-#include "gnss_coordinate_convert.h"
-
-#include "ivexit.h"
-
-#include "ivversion.h"
-
-#include "ivbacktrace.h"
-
-#include <signal.h>
-
-#include "service_roi_xodr.h"
-#include "ivservice.h"
-
-OpenDrive mxodr;
-xodrdijkstra * gpxd;
-bool gmapload = false;
-
-double glat0,glon0,ghead0;
-
-double gvehiclewidth = 2.0;
-
-bool gbExtendMap = true;
-
-static bool gbSideEnable = false;
-static bool gbSideLaneEnable = false;
-
-void * gpa;
-void * gpasrc;
-void * gpmap;
-void * gpagps;
-void * gpasimple;
-void * gpav2x;
-static void * gpanewtrace;
-
-iv::Ivfault *gfault = nullptr;
-iv::Ivlog *givlog = nullptr;
-
-static QCoreApplication * gApp;
-
-service_roi_xodr gsrx;
-
-
-namespace iv {
-struct simpletrace
-{
-    double gps_lat = 0;//纬度
-    double gps_lng = 0;//经度
-
-    double gps_x = 0;
-    double gps_y = 0;
-    double gps_z = 0;
-
-
-    double ins_heading_angle = 0;	//航向角
-};
-
-}
-/**
- *
- *
- *
- *
- * */
-bool LoadXODR(std::string strpath)
-{
-    OpenDriveXmlParser xp(&mxodr);
-    xp.ReadFile(strpath);
-    std::cout<<"road cout is "<<mxodr.GetRoadCount()<<std::endl;
-    if(mxodr.GetRoadCount() < 1)
-    {
-        gmapload = true;
-        return false;
-    }
-
-
-    xodrdijkstra * pxd = new xodrdijkstra(&mxodr);
-    gpxd = pxd;
-//    std::vector<int> xpath = pxd->getpath(10001,2,30012,2);
-
-//    pxd->getgpspoint(10001,2,30012,2,xpath);
-
-    int i;
-    double  nlenth = 0;
-    int nroadsize = mxodr.GetRoadCount();
-    for(i=0;i<nroadsize;i++)
-    {
-        Road * px = mxodr.GetRoad(i);
-        nlenth = nlenth + px->GetRoadLength();
-        int bloksize = px->GetGeometryBlockCount();
-        if(px->GetGeometryBlock(0)->GetGeometryAt(0)->GetGeomType() == 4)
-        {
-            GeometryParamPoly3 * p = (GeometryParamPoly3 *) px->GetGeometryBlock(0)->GetGeometryAt(0);
-            double a = p->GetuA();
-            a = p->GetuB();
-            a = p->GetuC();
-            a = p->GetuD();
-            a = p->GetvA();
-        }
-
-
-    }
-
-//    void Header::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)
-
-
-    unsigned short int revMajor,revMinor;
-    std::string name,date;
-    float version;
-    double north,south,east,west,lat0,lon0,hdg0;
-    if(mxodr.GetHeader() != 0)
-    {
-        mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
-        glat0 = lat0;
-        glon0 = lon0;
-    }
-
-
-
-    Road * proad1 = mxodr.GetRoad(0);
-    givlog->info("road name is %s",proad1->GetRoadName().data());
-    std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
-
-    gsrx.SetXODR(&mxodr);
-}
-
-class roadx
-{
-  public:
-    roadx * para;
-    std::vector<roadx> child;
-    int nlen;
-};
-
-
-#define EARTH_RADIUS 6370856.0
-
-//从1到2的距离和方向
-int CalcDisAngle(double lat1,double lon1,double lat2,double lon2,double * pLatDis,double * pLonDis,double * pDis,double * pangle)
-{
-    double a,b;
-    double LonDis,LatDis;
-    double LonRadius;
-    double Dis;
-    double angle;
-    if((lat1 == lat2)&&(lon1 == lon2))return -1;
-
-    LonRadius = EARTH_RADIUS * cos(lat2/(180.0/M_PI));
-    a = (EARTH_RADIUS * M_PI*2.0/360.0)/(100000.0);
-    b = lat2 - lat1; b = b*100000.0;
-    LatDis = a*b;
-    a = (LonRadius * M_PI*2.0/360.0)/100000.0;
-    b = lon2 - lon1; b = b*100000.0;
-    LonDis = a*b;
-    Dis = sqrt(LatDis*LatDis + LonDis *LonDis);
-    angle = acos(fabs(LonDis)/Dis);
-    angle = angle * 180.0/M_PI;
-
-    if(LonDis > 0)
-    {
-        if(LatDis > 0)angle = 90.0 - angle;
-        else angle= 90.0+angle;
-    }
-    else
-    {
-        if(LatDis > 0)angle = 270.0+angle;
-        else angle = 270.0-angle;
-    }
-
-    if(pLatDis != 0)*pLatDis = LatDis;
-    if(pLonDis != 0)*pLonDis = LonDis;
-    if(pDis != 0)*pDis = Dis;
-    if(pangle != 0)*pangle = angle;
-}
-
-
-
-//==========================================================
-
-//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
-//void GaussProjCal(double longitude, double latitude, double *X, double *Y)
-//{
-//    int ProjNo = 0; int ZoneWide; ////带宽
-//    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
-//    double a, f, e2, ee, NN, T, C, A, M, iPI;
-//    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
-//    ZoneWide = 6; ////6度带宽
-//    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
-//                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
-//    ProjNo = (int)(longitude / ZoneWide);
-//    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
-//    longitude0 = longitude0 * iPI;
-//    latitude0 = 0;
-//    longitude1 = longitude * iPI; //经度转换为弧度
-//    latitude1 = latitude * iPI; //纬度转换为弧度
-//    e2 = 2 * f - f * f;
-//    ee = e2 * (1.0 - e2);
-//    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
-//    T = tan(latitude1)*tan(latitude1);
-//    C = ee * cos(latitude1)*cos(latitude1);
-//    A = (longitude1 - longitude0)*cos(latitude1);
-//    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
-//        *e2 / 1024)*sin(2 * latitude1)
-//        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
-//    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
-//    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
-//        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
-//    X0 = 1000000L * (ProjNo + 1) + 500000L;
-//    Y0 = 0;
-//    xval = xval + X0; yval = yval + Y0;
-//    *X = xval;
-//    *Y = yval;
-//}
-
-#include <math.h>
-static void OffLngLat(double fLat0,double fLng0,double & fLat,double & fLng,double fHeading,double x,double y)
-{
-    double fxdiff,fydiff;
-
-    double xoff = y*(-1);
-    double yoff = x;
-
-    fxdiff = 0+xoff * cos(fHeading*M_PI/180.0)+ yoff*sin(fHeading*M_PI/180.0);  //East
-    fydiff = 0-xoff * sin(fHeading*M_PI/180.0)+ yoff*cos(fHeading*M_PI/180.0);  //South
-
-    double fEarthRadius = 6378245.0;
-
-    double ns1d = fEarthRadius*2*M_PI/360.0;
-    double fewRadius = fEarthRadius * cos(fLat0*M_PI/180.0);
-    double ew1d = fewRadius * 2*M_PI/360.0;
-
-    fLat = fLat0 + fydiff/ns1d;
-    fLng = fLng0 + fxdiff/ew1d;
-
-
-}
-
-void CalcXY(const double lat0,const double lon0,const double hdg0,
-              const double lat,const double lon,
-              double & x,double & y)
-{
-
-    double x0,y0;
-    GaussProjCal(lon0,lat0,&x0,&y0);
-    GaussProjCal(lon,lat,&x,&y);
-    x = x - x0;
-    y = y-  y0;
-
-//    double ang,dis;
-//    CalcDisAngle(lat0,lon0,lat,lon,0,0,&dis,&ang);
-//    double xang = hdg0 - ang;
-//    while(xang<0)xang = xang + 360.0;
-//    x = dis * cos(xang * M_PI/180.0);
-//    y = dis * sin(xang * M_PI/180.0);
-}
-
-//void CalcLatLon(const double lat0,const double lon0,const double hdg0,
-//                const double x,const double y,const double xyhdg,
-//                double &lat,double & lon, double & hdg)
-//{
-//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
-//    hdg = hdg0 - xyhdg * 180.0/M_PI;
-//    while(hdg < 0)hdg = hdg + 360;
-//    while(hdg >= 360)hdg = hdg - 360;
-//}
-
-
-void CalcLatLon(const double lat0,const double lon0,
-                const double x,const double y,
-                double &lat,double & lon)
-{
-    double x0,y0;
-    GaussProjCal(lon0,lat0,&x0,&y0);
-    double x_gps,y_gps;
-    x_gps = x0+x;
-    y_gps = y0+y;
-    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
-}
-
-
-void CalcLatLon(const double lat0,const double lon0,const double hdg0,
-                const double x,const double y,const double xyhdg,
-                double &lat,double & lon, double & hdg)
-{
-    double x0,y0;
-    GaussProjCal(lon0,lat0,&x0,&y0);
-    double x_gps,y_gps;
-    x_gps = x0+x;
-    y_gps = y0+y;
-    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
- //   hdg = hdg0 -xyhdg * 270/M_PI;
-    hdg = 90 -  xyhdg* 180.0/M_PI;
-
-//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
-//    hdg = hdg0 - xyhdg * 180.0/M_PI;
-    while(hdg < 0)hdg = hdg + 360;
-    while(hdg >= 360)hdg = hdg - 360;
-}
-
-class xodrobj
-{
-public:
-    double flatsrc;
-    double flonsrc;
-    double fhgdsrc;
-    double flat;
-    double flon;
-    int lane;
-};
-
-xodrobj gsrc;
-
-void ShareMap(std::vector<iv::GPSData> navigation_data)
-{
-    if(navigation_data.size()<1)return;
-    iv::GPS_INS x;
-    x = *(navigation_data.at(0));
-    char * data = new char[sizeof(iv::GPS_INS)*navigation_data.size()];
-    int gpssize = sizeof(iv::GPS_INS);
-    int i;
-    for(i=0;i<navigation_data.size();i++)
-    {
-        x = *(navigation_data.at(i));
-        memcpy(data+i*gpssize,&x,gpssize);
-    }
-
-    iv::modulecomm::ModuleSendMsg(gpmap,data,navigation_data.size()*gpssize);
-
-
-    int nsize = 100;
-    int nstep = 1;
-    if(navigation_data.size() < 100)
-    {
-        nsize = navigation_data.size();
-
-    }
-    else
-    {
-        nstep = navigation_data.size()/100;
-    }
-
-    iv::simpletrace psim[100];
-    for(i=0;i<nsize;i++)
-    {
-        x = *(navigation_data.at(i*nstep));
-        psim[i].gps_lat = x.gps_lat;
-        psim[i].gps_lng = x.gps_lng;
-        psim[i].gps_z = x.gps_z;
-        psim[i].gps_x = x.gps_x;
-        psim[i].gps_y = x.gps_y;
-        psim[i].ins_heading_angle = x.ins_heading_angle;
-    }
-    if(navigation_data.size()>100)
-    {
-        int nlast = 99;
-        x = *(navigation_data.at(navigation_data.size()-1));
-        psim[nlast].gps_lat = x.gps_lat;
-        psim[nlast].gps_lng = x.gps_lng;
-        psim[nlast].gps_z = x.gps_z;
-        psim[nlast].gps_x = x.gps_x;
-        psim[nlast].gps_y = x.gps_y;
-        psim[nlast].ins_heading_angle = x.ins_heading_angle;
-    }
-
-    iv::modulecomm::ModuleSendMsg(gpasimple,(char *)psim,nsize * sizeof(iv::simpletrace));
-
-    delete data;
-}
-
-static void ToGPSTrace(std::vector<PlanPoint> xPlan)
-{
-//    double x_src,y_src,x_dst,y_dst;
-
-
-
-//    x_src = -26;y_src = 10;
-//    x_dst = -50;y_dst = -220;
-
-    int i;
-    int nSize = xPlan.size();
-
-    std::vector<iv::GPSData> mapdata;
-
-
-    QFile xfile;
-    QString strpath;
-    strpath = getenv("HOME");
-    strpath = strpath + "/map/maptrace.txt";
-    xfile.setFileName(strpath);
-    bool bFileOpen = xfile.open(QIODevice::ReadWrite);
-
-    for(i=0;i<nSize;i++)
-    {
-        iv::GPSData data(new iv::GPS_INS);
-        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
-                   data->gps_lng,data->ins_heading_angle);
-
-        data->index = i;
-        data->speed = xPlan[i].speed;
-        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-
-
-        givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
-               data->gps_lng,data->ins_heading_angle);
-
-        data->roadSum = xPlan[i].mnLaneTotal;
-        data->roadMode = 0;
-        data->roadOri = xPlan[i].mnLaneori;
-
-        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
-        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
-        data->mfLaneWidth = xPlan[i].mWidth;
-        data->mfRoadWidth = xPlan[i].mfRoadWidth;
-        data->mnLaneChangeMark = xPlan[i].lanmp;
-
-        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-        if(xPlan[i].lanmp == 1)data->roadMode = 14;
-
-        mapdata.push_back(data);
-
-        char strline[255];
-        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
-                 i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,0,0);
-        if(bFileOpen)  xfile.write(strline);
-    //                             fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<"\t" <<road_width <<std::endl;
-
-    }
-    if(bFileOpen)xfile.close();
-
-    ShareMap(mapdata);
-}
-
-int avoidroadid[] = {10002,10019,10003,10098,10099,10063,10099,10100,10104,10110,10111};
-
-inline bool isboringroad(int nroadid)
-{
-    int i;
-    bool brtn = false;
-    for(i=0;i<11;i++)
-    {
-        if(avoidroadid[i] == nroadid)
-        {
-            brtn = true;
-            break;
-        }
-    }
-    return brtn;
-}
-
-
-void CalcLaneSide(std::vector<PlanPoint> & xPlan)
-{
-    const double fsidedis = 0.3;
-    const int nChangePoint = 150;
-    const int nStopPoint = 50;
-    if(xPlan.size()<nChangePoint)return;
-
-    int i;
-    int nsize = xPlan.size();
-
-
-    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
-    {
-        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
-   //     double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
-        double xold = xPlan[i].x;
-        double yold = xPlan[i].y;
-        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
-    }
-
-    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
-    {
-        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
-//        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
-        double xold = xPlan[i].x;
-        double yold = xPlan[i].y;
-        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
-        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
-    }
-    return;
-}
-
-void CalcSide(std::vector<PlanPoint> & xPlan)
-{
-    const double fsidedis = 0.3;
-    const int nChangePoint = 150;
-    const int nStopPoint = 50;
-    if(xPlan.size()<nChangePoint)return;
-    bool bChange = true;
-    int i;
-    int nsize = xPlan.size();
-    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
-    {
-        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
-        {
-            std::cout<<" Because Lane is narrow, not change."<<std::endl;
-            bChange = false;
-            break;
-        }
-    }
-
-    if(bChange == false)return;
-
-    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
-    {
-        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
-        double xold = xPlan[i].x;
-        double yold = xPlan[i].y;
-        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
-    }
-
-    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
-    {
-        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
-        double xold = xPlan[i].x;
-        double yold = xPlan[i].y;
-        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
-        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
-        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
-    }
-    return;
-}
-
-void SetPlan(xodrobj xo)
-{
-
-    double x_src,y_src,x_dst,y_dst;
-
-    CalcXY(glat0,glon0,ghead0,xo.flatsrc,xo.flonsrc,x_src,y_src);
-    CalcXY(glat0,glon0,ghead0,xo.flat,xo.flon,x_dst,y_dst);
-
-
-    std::vector<PlanPoint> xPlan;
-
-    double s;
-
-//    x_src = -5;y_src = 6;
-//    x_dst = -60;y_src = -150;
-    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - xo.fhgdsrc)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,xo.lane,xPlan);
-
-    if(nRtn < 0)
-    {
-        qDebug("plan fail.");
-        return;
-    }
-
-    int i;
-    int nSize = xPlan.size();
-
-    if(gbSideLaneEnable)
-    {
-        CalcLaneSide(xPlan);
-    }
-    else
-    {
-        if(gbSideEnable)
-        {
-            CalcSide(xPlan);
-        }
-    }
-
-    if(nSize<1)
-    {
-        qDebug("plan fail.");
-        return;
-    }
-    PlanPoint xLastPoint = xPlan[nSize -1];
-    for(i=0;i<600;i++)
-    {
-        PlanPoint pp = xLastPoint;
-        double fdis = 0.1*(i+1);
-        pp.mS = pp.mS + i*0.1;
-        pp.x = pp.x + fdis * cos(pp.hdg);
-        pp.y = pp.y + fdis * sin(pp.hdg);
-        pp.nSignal = 23;
-        if(gbExtendMap)
-        {
-            xPlan.push_back(pp);
-        }
-
-    }
-    
-
-   iv::map::tracemap xtrace;
-
-    nSize = xPlan.size();
-
-    std::vector<iv::GPSData> mapdata;
-    //maptrace
-    QFile xfile;
-    QString strpath;
-    strpath = getenv("HOME");
-    strpath = strpath + "/map/maptrace.txt";
-    xfile.setFileName(strpath);
-    xfile.open(QIODevice::ReadWrite);
-
-    //maptrace_add
-    QFile xfile_1;
-    QString strpath_1;
-    strpath_1 = getenv("HOME");
-    strpath_1 = strpath_1 + "/map/mapadd.txt";
-    xfile_1.setFileName(strpath_1);
-    xfile_1.open(QIODevice::ReadWrite);
-
-    for(i=0;i<nSize;i++)
-    {
-        iv::map::mappoint * pmappoint =  xtrace.add_point();
-        double gps_lon,gps_lat,gps_lon_avoidleft,gps_lat_avoidleft,gps_lat_avoidright,gps_lon_avoidright,gps_heading;
-        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,gps_lat,
-                  gps_lon,gps_heading);
-        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
-                   gps_lat_avoidleft,gps_lon_avoidleft);
-        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
-                   gps_lat_avoidright,gps_lon_avoidright);
-
-        pmappoint->set_gps_lat(gps_lat);
-        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
-        pmappoint->set_gps_lat_avoidright(gps_lat_avoidright);
-        pmappoint->set_gps_lng(gps_lon);
-        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
-        pmappoint->set_gps_lng_avoidright(gps_lon_avoidright);
-        pmappoint->set_ins_heading_angle(gps_heading);
-
-        double gps_x,gps_y,gps_x_avoidleft,gps_y_avoidleft,gps_x_avoidright,gps_y_avoidright;
-
-        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
-        GaussProjCal(gps_lon_avoidleft,gps_lat_avoidleft,&gps_x_avoidleft,&gps_y_avoidleft);
-        GaussProjCal(gps_lon_avoidright,gps_lat_avoidright,&gps_x_avoidright,&gps_y_avoidright);
-
-        pmappoint->set_gps_x(gps_x);
-        pmappoint->set_gps_x_avoidleft(gps_x_avoidleft);
-        pmappoint->set_gps_x_avoidright(gps_x_avoidright);
-        pmappoint->set_gps_y(gps_y);
-        pmappoint->set_gps_y_avoidleft(gps_y_avoidleft);
-        pmappoint->set_gps_y_avoidright(gps_y_avoidright);
-
-        pmappoint->set_speed(xPlan[i].speed);
-        pmappoint->set_index(i);
-
-        pmappoint->set_roadori(xPlan[i].mnLaneori);
-        pmappoint->set_roadsum(xPlan[i].mnLaneTotal);
-        pmappoint->set_mfdistolaneleft(xPlan[i].mfDisToLaneLeft);
-        pmappoint->set_mfdistoroadleft(xPlan[i].mfDisToRoadLeft);
-        pmappoint->set_mflanewidth(xPlan[i].mWidth);
-        pmappoint->set_mfroadwidth(xPlan[i].mfRoadWidth);
-        pmappoint->set_mbinlaneavoid(xPlan[i].bInlaneAvoid);
-
-
-        iv::GPSData data(new iv::GPS_INS);
-        data->gps_lat = gps_lat;
-        data->gps_lat_avoidleft = gps_lat_avoidleft;
-        data->gps_lat_avoidright = gps_lat_avoidright;
-        data->gps_lng = gps_lon;
-        data->gps_lng_avoidleft = gps_lon_avoidleft;
-        data->gps_lng_avoidright =  gps_lon_avoidright;
-        data->ins_heading_angle = gps_heading;
-        data->gps_x = gps_x;
-        data->gps_x_avoidleft = gps_x_avoidleft;
-        data->gps_x_avoidright = gps_x_avoidright;
-        data->gps_y = gps_y;
-        data->gps_y_avoidleft = gps_y_avoidleft;
-        data->gps_y_avoidright = gps_y_avoidright;
-        pmappoint->set_mbnoavoid(xPlan[i].mbNoavoid);
-        pmappoint->set_mfcurvature(xPlan[i].mfCurvature);
-
-
-//        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
-//                   data->gps_lng,data->ins_heading_angle);
-//        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
-//                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
-//        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
-//                   data->gps_lat_avoidright,data->gps_lng_avoidright);
-        data->index = i;
-        data->speed = xPlan[i].speed;
- //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-//        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-//        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
-//        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);
-
-        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
-               data->gps_lng,data->ins_heading_angle);
-
-        data->roadOri = xPlan[i].mnLaneori;
-        data->roadSum = xPlan[i].mnLaneTotal;
-        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
-        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
-        data->mfLaneWidth = xPlan[i].mWidth;
-        data->mfRoadWidth = xPlan[i].mfRoadWidth;
-
-
-        data->mbInLaneAvoid = xPlan[i].bInlaneAvoid;
-
-        if(xPlan[i].mbBoringRoad)
-        {
-            data->roadOri = 0;
-            data->roadSum = 2;
-            pmappoint->set_roadori(0);
-            pmappoint->set_roadsum(2);
-        }
-
-
-        data->mbnoavoid = xPlan[i].mbNoavoid;
-        if(data->mbnoavoid)
-        {
-            qDebug("no avoid i = %d",i);
-        }
-        data->mfCurvature = xPlan[i].mfCurvature;
-
-        data->mode2 = xPlan[i].nSignal;
-        if(data->mode2 == 3000)
-        {
-            int k;
-            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
-            {
-                if(k<0)break;
-                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=3000))
-                {
-                 continue;
-                }
-                mapdata.at(k)->mode2 = 3000;
-            }
-         }
-        if(data->mode2 == 1000001)
-        {
-            int k;
-            for(k=(mapdata.size()-1);k>(mapdata.size()-10);k--)
-            {
-                if(k<0)break;
-                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=1000001))
-                {
-                 continue;
-                }
-                mapdata.at(k)->mode2 = 1000001;
-            }
-         }
-
-       pmappoint->set_mode2(data->mode2);
-       pmappoint->set_rel_x(xPlan[i].x);
-       pmappoint->set_rel_y(xPlan[i].y);
-       pmappoint->set_rel_z(0);
-       pmappoint->set_rel_yaw(xPlan[i].hdg);
-       pmappoint->set_laneid(-1);
-       pmappoint->set_roadid(xPlan[i].nRoadID);
-
-#ifdef BOAVOID
-    if(isboringroad(xPlan[i].nRoadID))
-    {
-        const int nrangeavoid = 300;
-         if((i+(nrangeavoid + 10))<nSize)
-         {
-             double fhdg1 = xPlan[i].hdg;
-             bool bavoid = true;
-//             int k;
-//             for(k=0;k<=10;k++)
-//             {
-//                 double fhdg5 = xPlan[i+nrangeavoid*k/10].hdg;
-//                 double fhdgdiff1 = fhdg5 - fhdg1;
-//                 while(fhdgdiff1<0)fhdgdiff1 = fhdgdiff1 + 2.0*M_PI;
-//                 if((fhdgdiff1>(M_PI/3.0))&&(fhdgdiff1<(5.0*M_PI/3.0)))
-//                 {
-//                     bavoid = false;
-//                     break;
-//                 }
-
-//             }
-             if(bavoid)
-             {
-                 data->roadSum = 2;
-                 data->roadOri = 0;
-             }
-
-         }
-         else
-         {
-             int a = 1;
-             a++;
-         }
-    }
-#endif
-
-        mapdata.push_back(data);
-
-
-
-
-//        char strline[255];
-//  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
-//  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
-//              snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
-//                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri,data->mode2);
-//        xfile.write(strline);
-    }
-
-
-    for(int j=0;j<nSize;j++)
-    {
-        char strline[255];
-        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
-                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri);
-        xfile.write(strline);
-     //mapttrace1
-        char strline_1[255];
-        snprintf(strline_1,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
-                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri,mapdata.at(j)->mode2);
-        xfile_1.write(strline_1);
-    }
-
-    xfile.close();
-    xfile_1.close();
-    ShareMap(mapdata);
-
-    int nnewmapsize = xtrace.ByteSize();
-    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
-    if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
-    {
-        iv::modulecomm::ModuleSendMsg(gpanewtrace ,str_ptr.get(),nnewmapsize);
-    }
-    else
-    {
-        std::cout<<" new trace map serialize fail."<<std::endl;
-    }
-
-
-
-
-    s = 1;
-}
-
-void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane)
-{
-
-    std::vector<PlanPoint> xPlan;
-
-    int i;
-
-    double fLastHdg = 0;
-
-    int ndeflane =nlane;
-
-    for(i=0;i<pxv2x->stgps_size();i++)
-    {
-
-        double x_src,y_src,x_dst,y_dst;
-
-        if(i==0)
-        {
-            CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src);
-        }
-        else
-        {
-            CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src);
-        }
-        CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst);
-
-
-        std::vector<PlanPoint> xPlanPart;
-
-        double s;
-
-        //    x_src = -5;y_src = 6;
-        //    x_dst = -60;y_src = -150;
-        int nRtn = -1;
-        if(i==0)
-        {
-            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
-        }
-        else
-        {
-            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
-        }
-
-
-
-        if(nRtn < 0)
-        {
-            qDebug("plan fail.");
-            return;
-        }
-
-        int j;
-        for(j=0;j<xPlanPart.size();j++)xPlan.push_back(xPlanPart.at(j));
-        fLastHdg = xPlanPart.at(xPlanPart.size()-1).hdg;
-
-    }
-
-
-
-    int nSize = xPlan.size();
-
-    std::vector<iv::GPSData> mapdata;
-
-    QFile xfile;
-    QString strpath;
-    strpath = getenv("HOME");
-    strpath = strpath + "/map/maptrace.txt";
-    xfile.setFileName(strpath);
-    xfile.open(QIODevice::ReadWrite);
-
-    for(i=0;i<nSize;i++)
-    {
-        iv::GPSData data(new iv::GPS_INS);
-        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
-                   data->gps_lng,data->ins_heading_angle);
-        data->index = i;
-        data->speed = xPlan[i].speed;
- //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
-
-        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
-               data->gps_lng,data->ins_heading_angle);
-
-//        data->roadSum = 1;
-//        data->roadMode = 0;
-//        data->roadOri = 0;
-
-//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
-//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
-        data->roadOri = xPlan[i].mnLaneori;
-        data->roadSum = xPlan[i].mnLaneTotal;
-        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
-        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
-        data->mfLaneWidth = xPlan[i].mWidth;
-        data->mfRoadWidth = xPlan[i].mfRoadWidth;
-
-        data->mode2 = xPlan[i].nSignal;
-        if(data->mode2 == 3000)
-        {
-            int k;
-            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
-            {
-                if(k<0)break;
-                mapdata.at(k)->mode2 = 3000;
-            }
-        }
-
-        mapdata.push_back(data);
-
-        char strline[255];
-        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
-                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
-        xfile.write(strline);
-
-    }
-
-    xfile.close();
-
-    ShareMap(mapdata);
-
-}
-
-void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-    if(nSize<sizeof(xodrobj))
-    {
-        std::cout<<"ListenCmd small."<<std::endl;
-        return;
-    }
-
-    xodrobj xo;
-    memcpy(&xo,strdata,sizeof(xodrobj));
-
-    givlog->debug("lat is %f", xo.flat);
-
-    xo.fhgdsrc = gsrc.fhgdsrc;
-    xo.flatsrc = gsrc.flatsrc;
-    xo.flonsrc = gsrc.flonsrc;
-
-    SetPlan(xo);
-
-
-}
-
-void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    iv::v2x::v2x xv2x;
-    if(!xv2x.ParseFromArray(strdata,nSize))
-    {
-        givlog->warn("ListernV2X Parse Error.");
-        std::cout<<"ListenV2X Parse Error."<<std::endl;
-        return;
-    }
-    if(xv2x.stgps_size()<1)
-    {
-        givlog->debug("ListenV2X no gps station.");
-        std::cout<<"ListenV2X no gps station."<<std::endl;
-        return;
-    }
-
-    MultiStationPlan(&xv2x,gsrc.flatsrc,gsrc.flonsrc,gsrc.fhgdsrc,1);
-}
-
-void ListenSrc(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-    if(nSize<sizeof(xodrobj))
-    {
-        givlog->warn("ListenSrc small");
-        std::cout<<"ListenSrc small."<<std::endl;
-        return;
-    }
-
-    memcpy(&gsrc,strdata,sizeof(xodrobj));
-
-    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
-    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
-
-}
-
-void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-
-    iv::gps::gpsimu  xgpsimu;
-    if(!xgpsimu.ParseFromArray(strdata,nSize))
-    {
-        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
-        return;
-    }
-
-    xodrobj xo;
-    xo.fhgdsrc = xgpsimu.heading();
-    xo.flatsrc = xgpsimu.lat();
-    xo.flonsrc = xgpsimu.lon();
-
-    gsrc = xo;
-
-    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
-    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
-}
-
-void ExitFunc()
-{
-//    gApp->quit();
-    iv::modulecomm::Unregister(gpasimple);
-    iv::modulecomm::Unregister(gpav2x);
-    iv::modulecomm::Unregister(gpagps);
-    iv::modulecomm::Unregister(gpasrc);
-    iv::modulecomm::Unregister(gpmap);
-    iv::modulecomm::Unregister(gpa);
-    std::cout<<"driver_map_xodrload exit."<<std::endl;
-    gApp->quit();
-    std::this_thread::sleep_for(std::chrono::milliseconds(100));
-//    std::this_thread::sleep_for(std::chrono::milliseconds(900));
-}
-
-void signal_handler(int sig)
-{
-    if(sig == SIGINT)
-    {
-        ExitFunc();
-    }
-}
-
-void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
-{
-    (void)pstr_req;
-    (void)nreqsize;
-    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
-    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
-    {
-        std::shared_ptr<iv::roi::resultarray> pstr_roires;
-        gsrx.GetROI(pstr_roireq,pstr_roires);
-        int nbytesize = pstr_roires->ByteSize();
-        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
-        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
-        {
-            nressize = nbytesize;
-            std::cout<<"return res."<<std::endl;
-            return;
-        }
-        else
-        {
-            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
-        }
-    }
-    else
-    {
-        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
-        return;
-    }
-}
-
-int main(int argc, char *argv[])
-{
-    showversion("driver_map_xodrload");
-    QCoreApplication a(argc, argv);
-    gApp = &a;
-
-    RegisterIVBackTrace();
-
-    gfault = new iv::Ivfault("driver_map_xodrload");
-    givlog = new iv::Ivlog("driver_map_xodrload");
-
-    std::string strmapth,strparapath;
-    if(argc<3)
-    {
-//        strmapth = "./map.xodr";
-        strmapth = getenv("HOME");
-        strmapth = strmapth + "/map/map.xodr";
-//        strmapth = "/home/yuchuli/1226.xodr";
-        strparapath = "./driver_map_xodrload.xml";
-    }
-    else
-    {
-        strmapth = argv[1];
-        strparapath = argv[2];
-    }
-
-
-    iv::xmlparam::Xmlparam xp(strparapath);
-    xp.GetParam(std::string("he"),std::string("1"));
-    std::string strlat0 = xp.GetParam("lat0","39");
-    std::string strlon0 = xp.GetParam("lon0","117.0");
-    std::string strhdg0 = xp.GetParam("hdg0","360.0");
-
-    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
-
-    std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");
-
-    std::string strvehiclewidth = xp.GetParam("vehiclewidth","2.0");
-
-    std::string strextendmap = xp.GetParam("extendmap","false");
-
-    std::string strsideenable = xp.GetParam("sideenable","false");
-
-    std::string strsidelaneenable = xp.GetParam("sidelaneenable","false");
-
-
-    glat0 = atof(strlat0.data());
-    glon0 = atof(strlon0.data());
-    ghead0 = atof(strhdg0.data());
-
-    gvehiclewidth = atof(strvehiclewidth.data());
-
-    if(strextendmap == "true")
-    {
-        gbExtendMap = true;
-    }
-    else
-    {
-        gbExtendMap = false;
-    }
-
-    if(strsideenable == "true")
-    {
-        gbSideEnable = true;
-    }
-
-    if(strsidelaneenable == "true")
-    {
-        gbSideLaneEnable = true;
-    }
-
-
-    LoadXODR(strmapth);
-
-    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
-    (void)serviceroi;
-
-    gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
-    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
-    gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);
-
-
-    gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
-    gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
-    gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
-    gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
-
-
-
-
-    double x_src,y_src,x_dst,y_dst;
-
-    x_src = -26;y_src = 10;
-    x_dst = -50;y_dst = -220;
-
-    x_src = 0;y_src = 0;
-    x_dst = -23;y_dst = -18;
-    x_dst = 21;y_dst =-21;
-    x_dst =5;y_dst = 0;
-
-    x_src = -20; y_src = -1000;
-    x_dst = 900; y_dst = -630;
-
-//    x_dst = 450; y_dst = -640;
-//    x_dst = -190; y_dst = -690;
-
-//    x_src = 900; y_src = -610;
-//    x_dst = -100; y_dst = -680;
-    std::vector<PlanPoint> xPlan;
-    double s;
-//    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,3.14,x_dst,y_dst,s,30.0,100.0,1,xPlan);
-
-//    ToGPSTrace(xPlan);
-
-
-//    double lat = 39.1443880;
-//    double lon = 117.0812543;
-
-//    xodrobj xo;
-//    xo.fhgdsrc = 340;
-//    xo.flatsrc = lat; xo.flonsrc = lon;
-//    xo.flat = 39.1490196;
-//    xo.flon = 117.0806979;
-//    xo.lane = 1;
-//    SetPlan(xo);
-
-
-    void * pivexit = iv::ivexit::RegIVExitCall(ExitFunc);
-
-    signal(SIGINT,signal_handler);
-
-    int nrc = a.exec();
-
-    std::cout<<"driver_map_xodr app exit. code :"<<nrc<<std::endl;
-
-    return nrc;
-}
+#include <QCoreApplication>
+#include <math.h>
+#include <string>
+#include <thread>
+
+#include <QFile>
+
+#include "OpenDrive/OpenDrive.h"
+#include "OpenDrive/OpenDriveXmlParser.h"
+
+#include "globalplan.h"
+
+#include "gpsimu.pb.h"
+
+#include "mapdata.pb.h"
+
+#include "mapglobal.pb.h"
+
+#include "v2x.pb.h"
+
+#include "modulecomm.h"
+
+#include "xmlparam.h"
+
+#include "gps_type.h"
+
+#include "xodrdijkstra.h"
+
+#include "gnss_coordinate_convert.h"
+
+#include "ivexit.h"
+
+#include "ivversion.h"
+
+#include "ivbacktrace.h"
+
+#include <signal.h>
+
+#include "service_roi_xodr.h"
+#include "ivservice.h"
+
+
+#ifdef USE_TMERS
+#include "proj.h"
+
+//#include "projects.h"
+
+
+PJ_CONTEXT *C;//用于处理多线程程序
+    PJ *P;//初始化投影目标
+    PJ *norm;//初始化投影目标
+
+#endif
+
+
+OpenDrive mxodr;
+xodrdijkstra * gpxd;
+bool gmapload = false;
+
+double glat0,glon0,ghead0;
+
+double gvehiclewidth = 2.0;
+
+bool gbExtendMap = true;
+
+static bool gbSideEnable = false;
+static bool gbSideLaneEnable = false;
+
+static std::string gstrcdata;
+
+static void * gpa;
+static void * gpasrc;
+static void * gpmap;
+static void * gpagps;
+static void * gpasimple;
+static void * gpav2x;
+static void * gpanewtrace;
+static void * gpamapglobal;
+
+iv::Ivfault *gfault = nullptr;
+iv::Ivlog *givlog = nullptr;
+
+static QCoreApplication * gApp;
+
+service_roi_xodr gsrx;
+
+
+namespace iv {
+struct simpletrace
+{
+    double gps_lat = 0;//纬度
+    double gps_lng = 0;//经度
+
+    double gps_x = 0;
+    double gps_y = 0;
+    double gps_z = 0;
+
+
+    double ins_heading_angle = 0;	//航向角
+};
+
+}
+/**
+ *
+ *
+ *
+ *
+ * */
+bool LoadXODR(std::string strpath)
+{
+    OpenDriveXmlParser xp(&mxodr);
+    xp.ReadFile(strpath);
+    std::cout<<"road cout is "<<mxodr.GetRoadCount()<<std::endl;
+    if(mxodr.GetRoadCount() < 1)
+    {
+        gmapload = true;
+        return false;
+    }
+
+
+    xodrdijkstra * pxd = new xodrdijkstra(&mxodr);
+    gpxd = pxd;
+//    std::vector<int> xpath = pxd->getpath(10001,2,30012,2);
+
+//    pxd->getgpspoint(10001,2,30012,2,xpath);
+
+    int i;
+    double  nlenth = 0;
+    int nroadsize = mxodr.GetRoadCount();
+    for(i=0;i<nroadsize;i++)
+    {
+        Road * px = mxodr.GetRoad(i);
+        nlenth = nlenth + px->GetRoadLength();
+        int bloksize = px->GetGeometryBlockCount();
+        if(px->GetGeometryBlock(0)->GetGeometryAt(0)->GetGeomType() == 4)
+        {
+            GeometryParamPoly3 * p = (GeometryParamPoly3 *) px->GetGeometryBlock(0)->GetGeometryAt(0);
+            double a = p->GetuA();
+            a = p->GetuB();
+            a = p->GetuC();
+            a = p->GetuD();
+            a = p->GetvA();
+        }
+
+
+    }
+
+//    void Header::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)
+
+
+    unsigned short int revMajor,revMinor;
+    std::string name,date;
+    float version;
+    double north,south,east,west,lat0,lon0,hdg0;
+    if(mxodr.GetHeader() != 0)
+    {
+        mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
+        glat0 = lat0;
+        glon0 = lon0;
+    }
+
+
+
+    Road * proad1 = mxodr.GetRoad(0);
+    givlog->info("road name is %s",proad1->GetRoadName().data());
+    std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
+
+    gsrx.SetXODR(&mxodr);
+}
+
+class roadx
+{
+  public:
+    roadx * para;
+    std::vector<roadx> child;
+    int nlen;
+};
+
+
+#define EARTH_RADIUS 6370856.0
+
+//从1到2的距离和方向
+int CalcDisAngle(double lat1,double lon1,double lat2,double lon2,double * pLatDis,double * pLonDis,double * pDis,double * pangle)
+{
+    double a,b;
+    double LonDis,LatDis;
+    double LonRadius;
+    double Dis;
+    double angle;
+    if((lat1 == lat2)&&(lon1 == lon2))return -1;
+
+    LonRadius = EARTH_RADIUS * cos(lat2/(180.0/M_PI));
+    a = (EARTH_RADIUS * M_PI*2.0/360.0)/(100000.0);
+    b = lat2 - lat1; b = b*100000.0;
+    LatDis = a*b;
+    a = (LonRadius * M_PI*2.0/360.0)/100000.0;
+    b = lon2 - lon1; b = b*100000.0;
+    LonDis = a*b;
+    Dis = sqrt(LatDis*LatDis + LonDis *LonDis);
+    angle = acos(fabs(LonDis)/Dis);
+    angle = angle * 180.0/M_PI;
+
+    if(LonDis > 0)
+    {
+        if(LatDis > 0)angle = 90.0 - angle;
+        else angle= 90.0+angle;
+    }
+    else
+    {
+        if(LatDis > 0)angle = 270.0+angle;
+        else angle = 270.0-angle;
+    }
+
+    if(pLatDis != 0)*pLatDis = LatDis;
+    if(pLonDis != 0)*pLonDis = LonDis;
+    if(pDis != 0)*pDis = Dis;
+    if(pangle != 0)*pangle = angle;
+}
+
+
+
+//==========================================================
+
+//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
+//void GaussProjCal(double longitude, double latitude, double *X, double *Y)
+//{
+//    int ProjNo = 0; int ZoneWide; ////带宽
+//    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
+//    double a, f, e2, ee, NN, T, C, A, M, iPI;
+//    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
+//    ZoneWide = 6; ////6度带宽
+//    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
+//                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+//    ProjNo = (int)(longitude / ZoneWide);
+//    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
+//    longitude0 = longitude0 * iPI;
+//    latitude0 = 0;
+//    longitude1 = longitude * iPI; //经度转换为弧度
+//    latitude1 = latitude * iPI; //纬度转换为弧度
+//    e2 = 2 * f - f * f;
+//    ee = e2 * (1.0 - e2);
+//    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
+//    T = tan(latitude1)*tan(latitude1);
+//    C = ee * cos(latitude1)*cos(latitude1);
+//    A = (longitude1 - longitude0)*cos(latitude1);
+//    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
+//        *e2 / 1024)*sin(2 * latitude1)
+//        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
+//    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
+//    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
+//        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
+//    X0 = 1000000L * (ProjNo + 1) + 500000L;
+//    Y0 = 0;
+//    xval = xval + X0; yval = yval + Y0;
+//    *X = xval;
+//    *Y = yval;
+//}
+
+#include <math.h>
+static void OffLngLat(double fLat0,double fLng0,double & fLat,double & fLng,double fHeading,double x,double y)
+{
+    double fxdiff,fydiff;
+
+    double xoff = y*(-1);
+    double yoff = x;
+
+    fxdiff = 0+xoff * cos(fHeading*M_PI/180.0)+ yoff*sin(fHeading*M_PI/180.0);  //East
+    fydiff = 0-xoff * sin(fHeading*M_PI/180.0)+ yoff*cos(fHeading*M_PI/180.0);  //South
+
+    double fEarthRadius = 6378245.0;
+
+    double ns1d = fEarthRadius*2*M_PI/360.0;
+    double fewRadius = fEarthRadius * cos(fLat0*M_PI/180.0);
+    double ew1d = fewRadius * 2*M_PI/360.0;
+
+    fLat = fLat0 + fydiff/ns1d;
+    fLng = fLng0 + fxdiff/ew1d;
+
+
+}
+
+void CalcXY(const double lat0,const double lon0,const double hdg0,
+              const double lat,const double lon,
+              double & x,double & y)
+{
+
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    GaussProjCal(lon,lat,&x,&y);
+    x = x - x0;
+    y = y-  y0;
+
+//    double ang,dis;
+//    CalcDisAngle(lat0,lon0,lat,lon,0,0,&dis,&ang);
+//    double xang = hdg0 - ang;
+//    while(xang<0)xang = xang + 360.0;
+//    x = dis * cos(xang * M_PI/180.0);
+//    y = dis * sin(xang * M_PI/180.0);
+}
+
+//void CalcLatLon(const double lat0,const double lon0,const double hdg0,
+//                const double x,const double y,const double xyhdg,
+//                double &lat,double & lon, double & hdg)
+//{
+//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
+//    hdg = hdg0 - xyhdg * 180.0/M_PI;
+//    while(hdg < 0)hdg = hdg + 360;
+//    while(hdg >= 360)hdg = hdg - 360;
+//}
+
+
+void CalcLatLon(const double lat0,const double lon0,
+                const double x,const double y,
+                double &lat,double & lon)
+{
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    double x_gps,y_gps;
+    x_gps = x0+x;
+    y_gps = y0+y;
+    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
+}
+
+
+void CalcLatLon(const double lat0,const double lon0,const double hdg0,
+                const double x,const double y,const double xyhdg,
+                double &lat,double & lon, double & hdg)
+{
+
+
+    double x0,y0;
+    GaussProjCal(lon0,lat0,&x0,&y0);
+    double x_gps,y_gps;
+    x_gps = x0+x;
+    y_gps = y0+y;
+    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
+ //   hdg = hdg0 -xyhdg * 270/M_PI;
+    hdg = 90 -  xyhdg* 180.0/M_PI;
+
+//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
+//    hdg = hdg0 - xyhdg * 180.0/M_PI;
+    while(hdg < 0)hdg = hdg + 360;
+    while(hdg >= 360)hdg = hdg - 360;
+
+#ifdef USE_TMERS
+    if(gstrcdata.length()>3)
+    {
+        PJ_COORD cp;//初始化投影坐标
+        cp.enu.e = x;
+        cp.enu.n = y;
+        cp = proj_trans(P, PJ_INV, cp);//xy坐标转化为经纬度坐标
+        lon = cp.lp.lam;
+        lat = cp.lp.phi;
+    }
+
+#endif
+
+}
+
+
+
+class xodrobj
+{
+public:
+    double flatsrc;
+    double flonsrc;
+    double fhgdsrc;
+    double flat;
+    double flon;
+    int lane;
+};
+
+static xodrobj gsrc;
+
+void ShareMapGlobal(std::vector<PlanPoint> & xvectorPlan)
+{
+    iv::map::mapglobal xglobal;
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int >(xvectorPlan.size());
+    for(i=0;i<nsize;i++)
+    {
+        iv::map::pointglobal * ppoint = xglobal.add_point();
+        double gps_lon,gps_lat,gps_heading;
+        CalcLatLon(glat0,glon0,ghead0,xvectorPlan[i].x,xvectorPlan[i].y,xvectorPlan[i].hdg,gps_lat,
+                  gps_lon,gps_heading);
+        ppoint->set_gps_lat(gps_lat);
+        ppoint->set_gps_lng(gps_lon);
+        ppoint->set_ins_heading_angle(gps_heading);
+
+        double gps_x,gps_y;
+        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
+        ppoint->set_gps_x(gps_x);
+        ppoint->set_gps_y(gps_y);
+        ppoint->set_gps_z(0);
+
+        ppoint->set_mfcurvature(xvectorPlan[i].mfCurvature);
+        ppoint->set_mfdistolaneleft(xvectorPlan[i].mfDisToLaneLeft);
+        ppoint->set_mfdistoroadleft(xvectorPlan[i].mfDisToRoadLeft);
+        ppoint->set_mflanewidth(xvectorPlan[i].mWidth);
+        ppoint->set_mfroadwidth(xvectorPlan[i].mfRoadWidth);
+        ppoint->set_speed(xvectorPlan[i].speed);
+        unsigned int nlanecount = static_cast<unsigned int>(xvectorPlan[i].mVectorLaneWidth.size());
+        unsigned int j;
+        for(j=0;j<nlanecount;j++)
+        {
+            ppoint->add_mfvectorlanewidth(xvectorPlan[i].mVectorLaneWidth[j]);
+        }
+        ppoint->set_mlanecount(xvectorPlan[i].mLaneCount);
+        ppoint->set_mlanecur(xvectorPlan[i].mLaneCur);
+
+
+    }
+
+    int bytesize = static_cast<int>(xglobal.ByteSize()) ;
+
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[bytesize]);
+    if(xglobal.SerializePartialToArray(pstr_ptr.get(),bytesize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpamapglobal,pstr_ptr.get(),static_cast<unsigned int>(bytesize) );
+    }
+    else
+    {
+        std::cout<<" ShareMapGlobal  Serialzie Fail."<<std::endl;
+    }
+
+}
+
+void ShareMap(std::vector<iv::GPSData> navigation_data)
+{
+    if(navigation_data.size()<1)return;
+    iv::GPS_INS x;
+    x = *(navigation_data.at(0));
+    char * data = new char[sizeof(iv::GPS_INS)*navigation_data.size()];
+    int gpssize = sizeof(iv::GPS_INS);
+    int i;
+    for(i=0;i<navigation_data.size();i++)
+    {
+        x = *(navigation_data.at(i));
+        memcpy(data+i*gpssize,&x,gpssize);
+    }
+
+    iv::modulecomm::ModuleSendMsg(gpmap,data,navigation_data.size()*gpssize);
+
+
+    int nsize = 100;
+    int nstep = 1;
+    if(navigation_data.size() < 100)
+    {
+        nsize = navigation_data.size();
+
+    }
+    else
+    {
+        nstep = navigation_data.size()/100;
+    }
+
+    iv::simpletrace psim[100];
+    for(i=0;i<nsize;i++)
+    {
+        x = *(navigation_data.at(i*nstep));
+        psim[i].gps_lat = x.gps_lat;
+        psim[i].gps_lng = x.gps_lng;
+        psim[i].gps_z = x.gps_z;
+        psim[i].gps_x = x.gps_x;
+        psim[i].gps_y = x.gps_y;
+        psim[i].ins_heading_angle = x.ins_heading_angle;
+    }
+    if(navigation_data.size()>100)
+    {
+        int nlast = 99;
+        x = *(navigation_data.at(navigation_data.size()-1));
+        psim[nlast].gps_lat = x.gps_lat;
+        psim[nlast].gps_lng = x.gps_lng;
+        psim[nlast].gps_z = x.gps_z;
+        psim[nlast].gps_x = x.gps_x;
+        psim[nlast].gps_y = x.gps_y;
+        psim[nlast].ins_heading_angle = x.ins_heading_angle;
+    }
+
+    iv::modulecomm::ModuleSendMsg(gpasimple,(char *)psim,nsize * sizeof(iv::simpletrace));
+
+    delete data;
+}
+
+static void ToGPSTrace(std::vector<PlanPoint> xPlan)
+{
+//    double x_src,y_src,x_dst,y_dst;
+
+
+
+//    x_src = -26;y_src = 10;
+//    x_dst = -50;y_dst = -220;
+
+    int i;
+    int nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+
+
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    bool bFileOpen = xfile.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::GPSData data(new iv::GPS_INS);
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+                   data->gps_lng,data->ins_heading_angle);
+
+        data->index = i;
+        data->speed = xPlan[i].speed;
+        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+
+
+        givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->roadMode = 0;
+        data->roadOri = xPlan[i].mnLaneori;
+
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+        data->mnLaneChangeMark = xPlan[i].lanmp;
+
+        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+
+        mapdata.push_back(data);
+
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                 i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,0,0);
+        if(bFileOpen)  xfile.write(strline);
+    //                             fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<"\t" <<road_width <<std::endl;
+
+    }
+    if(bFileOpen)xfile.close();
+
+    ShareMap(mapdata);
+}
+
+int avoidroadid[] = {10002,10019,10003,10098,10099,10063,10099,10100,10104,10110,10111};
+
+inline bool isboringroad(int nroadid)
+{
+    int i;
+    bool brtn = false;
+    for(i=0;i<11;i++)
+    {
+        if(avoidroadid[i] == nroadid)
+        {
+            brtn = true;
+            break;
+        }
+    }
+    return brtn;
+}
+
+
+void CalcLaneSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 350;
+    const int nStopPoint = 150;
+    if(xPlan.size()<nChangePoint)return;
+
+    int i;
+    int nsize = xPlan.size();
+
+    bool bChangeLane = false;
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+   //     double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
+        if(fMove>2.0)
+        {
+            bChangeLane = true;
+        }
+        if(bChangeLane)
+            xPlan[i].nlrchange = 2;
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+//        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
+        if(bChangeLane)xPlan[i].nlrchange = 2;
+    }
+    return;
+}
+
+void CalcSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+    bool bChange = true;
+    int i;
+    int nsize = xPlan.size();
+    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
+    {
+        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
+        {
+            std::cout<<" Because Lane is narrow, not change."<<std::endl;
+            bChange = false;
+            break;
+        }
+    }
+
+    if(bChange == false)return;
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
+    }
+    return;
+}
+
+#ifdef TESTSPEEDPLAN
+#include <math.h>
+#include <limits>
+
+double valuemin(double a,double b)
+{
+    if(a<b)return a;
+    return  b;
+}
+int getPlanSpeed(std::vector<iv::GPSData> MapTrace)
+{
+                int mode0to5count=0,mode0to18count=0,mode0to5countSum=0,mode0to18countSum=0,mode18count=0,mode18countSum=0,mode0to5flash=0,mode18flash=0;
+                double straightSpeed=40;
+                double straightCurveSpeed=30;
+                double Curve_SmallSpeed=15;
+                double Curve_LargeSpeed=8;
+
+                std::vector<std::vector<double>> doubledata;
+
+                doubledata.clear();
+                for (int i = 0; i < MapTrace.size(); i++) {   //                                       1225/14:25
+                             std::vector<double> temp;
+                             temp.clear();
+                             temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);temp.push_back(0);//先push四个空量，然后就可以给量赋值了
+                             doubledata.push_back(temp);
+                             doubledata[i][0] = MapTrace.at(i)->gps_x;
+                             doubledata[i][1] = MapTrace.at(i)->gps_y;
+                             doubledata[i][2] = (MapTrace.at(i)->ins_heading_angle) / 180 * M_PI;
+                             doubledata[i][3]=0;
+                             doubledata[i][4]=0;
+                }
+
+                for(int i=0;i<doubledata.size();i++)
+                {
+                    if((MapTrace[i]->mfCurvature>-0.02)&&(MapTrace[i]->mfCurvature<0.02)){
+                        MapTrace[i]->roadMode=5;
+                    }else if(((MapTrace[i]->mfCurvature>=0.02)&&(MapTrace[i]->mfCurvature<=0.16))||((MapTrace[i]->mfCurvature>=-0.16)&&(MapTrace[i]->mfCurvature<=-0.02))){
+                        MapTrace[i]->roadMode=18;
+                    }else if(((MapTrace[i]->mfCurvature>0.16))||((MapTrace[i]->mfCurvature<-0.16))){
+                        MapTrace[i]->roadMode=14;
+                    }
+                }
+                for(int i=0;i<MapTrace.size();i++)
+                {
+                    if(MapTrace[i]->roadMode==0){
+                        doubledata[i][4]=straightSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==5){
+                        doubledata[i][4]=straightCurveSpeed;
+                        mode0to5count++;
+                        //mode0to18count++;
+                        mode18count=0;
+                        mode0to5flash=mode0to5count;
+                    }else if(MapTrace[i]->roadMode==18){
+                        mode0to5countSum=mode0to5count;
+                        doubledata[i][4]=Curve_SmallSpeed;
+
+                        double brake_distance_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_SmallSpeed/3.6),2))*10/0.6;
+                        int brake_distance=(int)brake_distance_double;
+                        int step_count=0;
+                        if((i>brake_distance)&&(mode0to5countSum>brake_distance))
+                        {
+                            double step_speed=(straightCurveSpeed-Curve_SmallSpeed)/brake_distance;
+
+                            for(int j=i;j>i-brake_distance;j--){
+                                doubledata[j][4]=valuemin((Curve_SmallSpeed+step_count*step_speed),straightCurveSpeed);
+                                step_count++;
+                            }
+                        }else if(mode0to5countSum>0){
+                            for(int j=i;j>=i-mode0to5countSum;j--){
+                                doubledata[j][4]=Curve_SmallSpeed;
+                                step_count++;
+                            }
+                        }else{
+                            doubledata[i][4]=Curve_SmallSpeed;
+                        }
+                        mode0to5count=0;
+                        //mode0to18count++;
+                        mode18count++;
+                        mode18flash=mode18count;
+                    }else if(MapTrace[i]->roadMode==14){
+                        mode0to18countSum=mode0to5flash+mode18flash;
+                        mode18countSum=mode18count;
+                        double brake_distanceLarge_double=(pow((Curve_SmallSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        double brake_distance0to18_double=(pow((straightCurveSpeed/3.6),2)-pow((Curve_LargeSpeed/3.6),2))*10/0.6;
+                        int brake_distanceLarge=(int)brake_distanceLarge_double;
+                        int brake_distance0to18=(int)brake_distance0to18_double;
+                        int step_count=0;
+                        doubledata[i][4]=Curve_LargeSpeed;
+
+                        if(mode18countSum>brake_distanceLarge)
+                        {
+                                double step_speed=(Curve_SmallSpeed-Curve_LargeSpeed)/brake_distanceLarge;
+
+                                for(int j=i;j>i-brake_distanceLarge;j--){
+                                    doubledata[j][4]=valuemin((Curve_LargeSpeed+step_count*step_speed),Curve_SmallSpeed);
+                                    step_count++;
+                                }
+
+                        }else if(mode0to18countSum>brake_distance0to18){
+
+                                double step_speed=(straightCurveSpeed-Curve_LargeSpeed)/brake_distance0to18;
+
+                                for(int j=i;j>i-brake_distance0to18;j--){
+                                    doubledata[j][4]=valuemin((Curve_LargeSpeed+step_count*step_speed),straightCurveSpeed);
+                                    step_count++;
+                                }
+                        }else{
+                                doubledata[i][4]=Curve_LargeSpeed;
+                        }
+
+                        mode0to5count=0;
+                        mode18count=0;
+                        mode0to5flash=0;
+                        mode18flash=0;
+                    }
+                }
+
+                return 0;
+
+}
+
+#endif
+
+void SetPlan(xodrobj xo)
+{
+
+    double x_src,y_src,x_dst,y_dst;
+
+    CalcXY(glat0,glon0,ghead0,xo.flatsrc,xo.flonsrc,x_src,y_src);
+    CalcXY(glat0,glon0,ghead0,xo.flat,xo.flon,x_dst,y_dst);
+
+
+    std::vector<PlanPoint> xPlan;
+
+    double s;
+
+//    x_src = -5;y_src = 6;
+//    x_dst = -60;y_src = -150;
+    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - xo.fhgdsrc)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,xo.lane,xPlan);
+
+    if(nRtn < 0)
+    {
+        qDebug("plan fail.");
+        return;
+    }
+
+    int i;
+    int nSize = xPlan.size();
+
+    if(gbSideLaneEnable)
+    {
+        CalcLaneSide(xPlan);
+    }
+    else
+    {
+        if(gbSideEnable)
+        {
+            CalcSide(xPlan);
+        }
+    }
+
+    if(nSize<1)
+    {
+        qDebug("plan fail.");
+        return;
+    }
+    PlanPoint xLastPoint = xPlan[nSize -1];
+    for(i=0;i<600;i++)
+    {
+        PlanPoint pp = xLastPoint;
+        double fdis = 0.1*(i+1);
+        pp.mS = pp.mS + i*0.1;
+        pp.x = pp.x + fdis * cos(pp.hdg);
+        pp.y = pp.y + fdis * sin(pp.hdg);
+        pp.nSignal = 23;
+        if(gbExtendMap)
+        {
+            xPlan.push_back(pp);
+        }
+
+    }
+    
+
+   iv::map::tracemap xtrace;
+
+    nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+    //maptrace
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    xfile.open(QIODevice::ReadWrite);
+
+    //maptrace_add
+    QFile xfile_1;
+    QString strpath_1;
+    strpath_1 = getenv("HOME");
+    strpath_1 = strpath_1 + "/map/mapadd.txt";
+    xfile_1.setFileName(strpath_1);
+    xfile_1.open(QIODevice::ReadWrite);
+
+    double fpointdis = 0.1;
+    if(nSize > 2)
+    {
+        fpointdis = sqrt(pow(xPlan[0].x - xPlan[1].x,2)+pow(xPlan[0].y - xPlan[1].y,2));
+    }
+
+    if(fpointdis < 0.1)fpointdis = 0.1;
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::map::mappoint * pmappoint =  xtrace.add_point();
+        double gps_lon,gps_lat,gps_lon_avoidleft,gps_lat_avoidleft,gps_lat_avoidright,gps_lon_avoidright,gps_heading;
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,gps_lat,
+                  gps_lon,gps_heading);
+        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
+                   gps_lat_avoidleft,gps_lon_avoidleft);
+        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
+                   gps_lat_avoidright,gps_lon_avoidright);
+
+        pmappoint->set_gps_lat(gps_lat);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lat_avoidright(gps_lat_avoidright);
+        pmappoint->set_gps_lng(gps_lon);
+        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
+        pmappoint->set_gps_lng_avoidright(gps_lon_avoidright);
+        pmappoint->set_ins_heading_angle(gps_heading);
+
+        double gps_x,gps_y,gps_x_avoidleft,gps_y_avoidleft,gps_x_avoidright,gps_y_avoidright;
+
+        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
+        GaussProjCal(gps_lon_avoidleft,gps_lat_avoidleft,&gps_x_avoidleft,&gps_y_avoidleft);
+        GaussProjCal(gps_lon_avoidright,gps_lat_avoidright,&gps_x_avoidright,&gps_y_avoidright);
+
+        pmappoint->set_gps_x(gps_x);
+        pmappoint->set_gps_x_avoidleft(gps_x_avoidleft);
+        pmappoint->set_gps_x_avoidright(gps_x_avoidright);
+        pmappoint->set_gps_y(gps_y);
+        pmappoint->set_gps_y_avoidleft(gps_y_avoidleft);
+        pmappoint->set_gps_y_avoidright(gps_y_avoidright);
+
+        pmappoint->set_speed(xPlan[i].speed);
+        pmappoint->set_index(i);
+
+        pmappoint->set_roadori(xPlan[i].mnLaneori);
+        pmappoint->set_roadsum(xPlan[i].mnLaneTotal);
+        pmappoint->set_mfdistolaneleft(xPlan[i].mfDisToLaneLeft);
+        pmappoint->set_mfdistoroadleft(xPlan[i].mfDisToRoadLeft);
+        pmappoint->set_mflanewidth(xPlan[i].mWidth);
+        pmappoint->set_mfroadwidth(xPlan[i].mfRoadWidth);
+        pmappoint->set_mbinlaneavoid(xPlan[i].bInlaneAvoid);
+
+
+        iv::GPSData data(new iv::GPS_INS);
+        data->roadMode = 0;
+
+        static int nChangeCount = 0;
+        static int nlrmode = -1;
+
+        if(nChangeCount == 0)
+        {
+
+        }
+        else
+        {
+            data->roadMode = nlrmode;
+            nChangeCount--;
+        }
+ //       std::cout<<"i:"<<i<<" lr:"<<xPlan[i].nlrchange<<std::endl;
+//        if(xPlan[i].nlrchange == 1)
+//        {
+//            data->roadMode = 14;
+//        }
+//        if(xPlan[i].nlrchange == 2)
+//        {
+//            data->roadMode = 15;
+//        }
+
+        if(i>50)
+        {
+            if((xPlan[i-1].nlrchange == 0)&&(xPlan[i].nlrchange != 0))
+            {
+                int j;
+                for(j=(i-1);j>=(i-50);j--)
+                {
+                    if(xPlan[i].nlrchange == 1)
+                    {
+                        mapdata[j]->roadMode = 14;
+                    }
+                    if(xPlan[i].nlrchange == 2)
+                    {
+                        mapdata[j]->roadMode = 15;
+                    }
+                    if(j <=0)break;
+                }
+                int nChangeCount = 0;
+                for(j=i;j<(nSize-1);j++)
+                {
+                    if((xPlan[j].nlrchange != 0)&&(xPlan[j+1].nlrchange != 0))
+                    {
+                        nChangeCount++;
+                    }
+                }
+                if(nChangeCount < static_cast<int>(10.0/fpointdis))   //Fast Change road.
+                {
+                    nChangeCount = nChangeCount + static_cast<int>(3.0/fpointdis);
+                }
+                else
+                {
+                    nChangeCount =  static_cast<int>(5.0/fpointdis);
+                }
+
+                    if(xPlan[i].nlrchange == 1)
+                    {
+                        nlrmode = 14;
+                    }
+                    if(xPlan[i].nlrchange == 2)
+                    {
+                        nlrmode = 15;
+                    }
+
+            }
+        }
+
+//        if(data->roadMode != 0)
+//        {
+//            std::cout<<"i: "<<i<<std::endl;
+//        }
+
+        data->gps_lat = gps_lat;
+        data->gps_lat_avoidleft = gps_lat_avoidleft;
+        data->gps_lat_avoidright = gps_lat_avoidright;
+        data->gps_lng = gps_lon;
+        data->gps_lng_avoidleft = gps_lon_avoidleft;
+        data->gps_lng_avoidright =  gps_lon_avoidright;
+        data->ins_heading_angle = gps_heading;
+        data->gps_x = gps_x;
+        data->gps_x_avoidleft = gps_x_avoidleft;
+        data->gps_x_avoidright = gps_x_avoidright;
+        data->gps_y = gps_y;
+        data->gps_y_avoidleft = gps_y_avoidleft;
+        data->gps_y_avoidright = gps_y_avoidright;
+        pmappoint->set_mbnoavoid(xPlan[i].mbNoavoid);
+        pmappoint->set_mfcurvature(xPlan[i].mfCurvature);
+
+
+//        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+//                   data->gps_lng,data->ins_heading_angle);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
+//                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
+//        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
+//                   data->gps_lat_avoidright,data->gps_lng_avoidright);
+        data->index = i;
+        data->speed = xPlan[i].speed;
+ //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+//        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+//        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
+//        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);
+
+        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+
+
+        data->mbInLaneAvoid = xPlan[i].bInlaneAvoid;
+
+        if(xPlan[i].mbBoringRoad)
+        {
+            data->roadOri = 0;
+            data->roadSum = 2;
+            pmappoint->set_roadori(0);
+            pmappoint->set_roadsum(2);
+        }
+
+
+        data->mbnoavoid = xPlan[i].mbNoavoid;
+        if(data->mbnoavoid)
+        {
+            qDebug("no avoid i = %d",i);
+        }
+        data->mfCurvature = xPlan[i].mfCurvature;
+
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=3000))
+                {
+                 continue;
+                }
+                mapdata.at(k)->mode2 = 3000;
+            }
+         }
+        if(data->mode2 == 1000001)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-10);k--)
+            {
+                if(k<0)break;
+                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=1000001))
+                {
+                 continue;
+                }
+                mapdata.at(k)->mode2 = 1000001;
+            }
+         }
+
+       pmappoint->set_mode2(data->mode2);
+       pmappoint->set_rel_x(xPlan[i].x);
+       pmappoint->set_rel_y(xPlan[i].y);
+       pmappoint->set_rel_z(0);
+       pmappoint->set_rel_yaw(xPlan[i].hdg);
+       pmappoint->set_laneid(-1);
+       pmappoint->set_roadid(xPlan[i].nRoadID);
+
+#ifdef BOAVOID
+    if(isboringroad(xPlan[i].nRoadID))
+    {
+        const int nrangeavoid = 300;
+         if((i+(nrangeavoid + 10))<nSize)
+         {
+             double fhdg1 = xPlan[i].hdg;
+             bool bavoid = true;
+//             int k;
+//             for(k=0;k<=10;k++)
+//             {
+//                 double fhdg5 = xPlan[i+nrangeavoid*k/10].hdg;
+//                 double fhdgdiff1 = fhdg5 - fhdg1;
+//                 while(fhdgdiff1<0)fhdgdiff1 = fhdgdiff1 + 2.0*M_PI;
+//                 if((fhdgdiff1>(M_PI/3.0))&&(fhdgdiff1<(5.0*M_PI/3.0)))
+//                 {
+//                     bavoid = false;
+//                     break;
+//                 }
+
+//             }
+             if(bavoid)
+             {
+                 data->roadSum = 2;
+                 data->roadOri = 0;
+             }
+
+         }
+         else
+         {
+             int a = 1;
+             a++;
+         }
+    }
+#endif
+
+        mapdata.push_back(data);
+
+
+
+
+//        char strline[255];
+//  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
+//  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
+//              snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+//                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri,data->mode2);
+//        xfile.write(strline);
+    }
+
+
+    for(int j=0;j<nSize;j++)
+    {
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri);
+        xfile.write(strline);
+     //mapttrace1
+        char strline_1[255];
+        snprintf(strline_1,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
+                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri,mapdata.at(j)->mode2);
+        xfile_1.write(strline_1);
+    }
+
+    xfile.close();
+    xfile_1.close();
+    ShareMap(mapdata);
+
+#ifdef TESTSPEEDPLAN
+    getPlanSpeed(mapdata);
+#endif
+
+    ShareMapGlobal(xPlan);
+
+    int nnewmapsize = xtrace.ByteSize();
+    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
+    if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
+    {
+        iv::modulecomm::ModuleSendMsg(gpanewtrace ,str_ptr.get(),nnewmapsize);
+    }
+    else
+    {
+        std::cout<<" new trace map serialize fail."<<std::endl;
+    }
+
+
+
+
+    s = 1;
+}
+
+void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane)
+{
+
+    std::vector<PlanPoint> xPlan;
+
+    int i;
+
+    double fLastHdg = 0;
+
+    int ndeflane =nlane;
+
+    for(i=0;i<pxv2x->stgps_size();i++)
+    {
+
+        double x_src,y_src,x_dst,y_dst;
+
+        if(i==0)
+        {
+            CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src);
+        }
+        else
+        {
+            CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src);
+        }
+        CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst);
+
+
+        std::vector<PlanPoint> xPlanPart;
+
+        double s;
+
+        //    x_src = -5;y_src = 6;
+        //    x_dst = -60;y_src = -150;
+        int nRtn = -1;
+        if(i==0)
+        {
+            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
+        }
+        else
+        {
+            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
+        }
+
+
+
+        if(nRtn < 0)
+        {
+            qDebug("plan fail.");
+            return;
+        }
+
+        int j;
+        for(j=0;j<xPlanPart.size();j++)xPlan.push_back(xPlanPart.at(j));
+        fLastHdg = xPlanPart.at(xPlanPart.size()-1).hdg;
+
+    }
+
+
+
+    int nSize = xPlan.size();
+
+    std::vector<iv::GPSData> mapdata;
+
+    QFile xfile;
+    QString strpath;
+    strpath = getenv("HOME");
+    strpath = strpath + "/map/maptrace.txt";
+    xfile.setFileName(strpath);
+    xfile.open(QIODevice::ReadWrite);
+
+    for(i=0;i<nSize;i++)
+    {
+        iv::GPSData data(new iv::GPS_INS);
+        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
+                   data->gps_lng,data->ins_heading_angle);
+        data->index = i;
+        data->speed = xPlan[i].speed;
+ //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
+
+        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
+               data->gps_lng,data->ins_heading_angle);
+
+//        data->roadSum = 1;
+//        data->roadMode = 0;
+//        data->roadOri = 0;
+
+//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
+//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
+        data->roadOri = xPlan[i].mnLaneori;
+        data->roadSum = xPlan[i].mnLaneTotal;
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+
+        data->mode2 = xPlan[i].nSignal;
+        if(data->mode2 == 3000)
+        {
+            int k;
+            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
+            {
+                if(k<0)break;
+                mapdata.at(k)->mode2 = 3000;
+            }
+        }
+
+        mapdata.push_back(data);
+
+        char strline[255];
+        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
+                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
+        xfile.write(strline);
+
+    }
+
+    xfile.close();
+
+    ShareMap(mapdata);
+
+}
+
+void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    if(nSize<sizeof(xodrobj))
+    {
+        std::cout<<"ListenCmd small."<<std::endl;
+        return;
+    }
+
+    xodrobj xo;
+    memcpy(&xo,strdata,sizeof(xodrobj));
+
+    givlog->debug("lat is %f", xo.flat);
+
+    xo.fhgdsrc = gsrc.fhgdsrc;
+    xo.flatsrc = gsrc.flatsrc;
+    xo.flonsrc = gsrc.flonsrc;
+
+    SetPlan(xo);
+
+
+}
+
+void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::v2x::v2x xv2x;
+    if(!xv2x.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ListernV2X Parse Error.");
+        std::cout<<"ListenV2X Parse Error."<<std::endl;
+        return;
+    }
+    if(xv2x.stgps_size()<1)
+    {
+        givlog->debug("ListenV2X no gps station.");
+        std::cout<<"ListenV2X no gps station."<<std::endl;
+        return;
+    }
+
+    MultiStationPlan(&xv2x,gsrc.flatsrc,gsrc.flonsrc,gsrc.fhgdsrc,1);
+}
+
+void ListenSrc(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    if(nSize<sizeof(xodrobj))
+    {
+        givlog->warn("ListenSrc small");
+        std::cout<<"ListenSrc small."<<std::endl;
+        return;
+    }
+
+    memcpy(&gsrc,strdata,sizeof(xodrobj));
+
+    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
+    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
+
+}
+
+void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
+        return;
+    }
+
+    xodrobj xo;
+    xo.fhgdsrc = xgpsimu.heading();
+    xo.flatsrc = xgpsimu.lat();
+    xo.flonsrc = xgpsimu.lon();
+
+    gsrc = xo;
+
+    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
+    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
+}
+
+void ExitFunc()
+{
+//    gApp->quit();
+    iv::modulecomm::Unregister(gpasimple);
+    iv::modulecomm::Unregister(gpav2x);
+    iv::modulecomm::Unregister(gpagps);
+    iv::modulecomm::Unregister(gpasrc);
+    iv::modulecomm::Unregister(gpmap);
+    iv::modulecomm::Unregister(gpa);
+    std::cout<<"driver_map_xodrload exit."<<std::endl;
+    gApp->quit();
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+//    std::this_thread::sleep_for(std::chrono::milliseconds(900));
+}
+
+void signal_handler(int sig)
+{
+    if(sig == SIGINT)
+    {
+        ExitFunc();
+    }
+}
+
+void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
+{
+    (void)pstr_req;
+    (void)nreqsize;
+    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
+    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
+    {
+        std::shared_ptr<iv::roi::resultarray> pstr_roires;
+        gsrx.GetROI(pstr_roireq,pstr_roires);
+        int nbytesize = pstr_roires->ByteSize();
+        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
+        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
+        {
+            nressize = nbytesize;
+            std::cout<<"return res."<<std::endl;
+            return;
+        }
+        else
+        {
+            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
+        }
+    }
+    else
+    {
+        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
+        return;
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    showversion("driver_map_xodrload");
+    QCoreApplication a(argc, argv);
+    gApp = &a;
+
+    RegisterIVBackTrace();
+
+    gfault = new iv::Ivfault("driver_map_xodrload");
+    givlog = new iv::Ivlog("driver_map_xodrload");
+
+    std::string strmapth,strparapath;
+    if(argc<3)
+    {
+//        strmapth = "./map.xodr";
+        strmapth = getenv("HOME");
+        strmapth = strmapth + "/map/map.xodr";
+//        strmapth = "/home/yuchuli/1226.xodr";
+        strparapath = "./driver_map_xodrload.xml";
+    }
+    else
+    {
+        strmapth = argv[1];
+        strparapath = argv[2];
+    }
+
+
+    iv::xmlparam::Xmlparam xp(strparapath);
+    xp.GetParam(std::string("he"),std::string("1"));
+    std::string strlat0 = xp.GetParam("lat0","39");
+    std::string strlon0 = xp.GetParam("lon0","117.0");
+    std::string strhdg0 = xp.GetParam("hdg0","360.0");
+
+    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
+
+    std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");
+
+    std::string strvehiclewidth = xp.GetParam("vehiclewidth","2.0");
+
+    std::string strextendmap = xp.GetParam("extendmap","false");
+
+    std::string strsideenable = xp.GetParam("sideenable","false");
+
+    std::string strsidelaneenable = xp.GetParam("sidelaneenable","false");
+
+    gstrcdata = xp.GetParam("cdata","");
+
+    std::cout<<" cdata: "<<gstrcdata.data()<<std::endl;
+
+
+#ifdef USE_TMERS
+    if(gstrcdata.length()>3)
+    {
+        C = proj_context_create();//创建多线程，由于本示例为单线程，此处为展示作用
+
+
+        P = proj_create_crs_to_crs (C,
+                                    "WGS84",
+                                    "+proj=tmerc +ellps=WGS84 +datum=WGS84 +units=m +lat_0=28.3252659663377 +lon_0=117.810536087614 +no_defs", /* or EPSG:32632 */
+                                    NULL);
+        //      P = proj_create(C,"+proj=tmerc +ellps=WGS84 +datum=WGS84 +units=m +lat_0=28.3252659663377 +lon_0=117.810536087614 +no_defs");
+
+        if (0 == P) {
+            std::cout << "Failed to create transformation object." << stderr << std::endl;
+            return 1;
+        }//如果P中两个投影的字符串不符合proj定义，提示转换失败
+
+        norm = proj_normalize_for_visualization(C, P);
+
+        if (0 == norm) {
+
+            fprintf(stderr, "Failed to normalize transformation object.\n");
+
+            return 1;
+
+        }
+
+        proj_destroy(P);
+
+            P = norm;
+    }
+
+#endif
+
+
+    glat0 = atof(strlat0.data());
+    glon0 = atof(strlon0.data());
+    ghead0 = atof(strhdg0.data());
+
+    gvehiclewidth = atof(strvehiclewidth.data());
+
+    if(strextendmap == "true")
+    {
+        gbExtendMap = true;
+    }
+    else
+    {
+        gbExtendMap = false;
+    }
+
+    if(strsideenable == "true")
+    {
+        gbSideEnable = true;
+    }
+
+    if(strsidelaneenable == "true")
+    {
+        gbSideLaneEnable = true;
+    }
+
+
+    LoadXODR(strmapth);
+
+    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
+    (void)serviceroi;
+
+    gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
+    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
+    gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",20000000,1);
+    gpamapglobal = iv::modulecomm::RegisterSend("mapglobal",20000000,1);
+
+
+    gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
+    gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
+    gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
+    gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);
+
+
+
+
+    double x_src,y_src,x_dst,y_dst;
+
+    x_src = -26;y_src = 10;
+    x_dst = -50;y_dst = -220;
+
+    x_src = 0;y_src = 0;
+    x_dst = -23;y_dst = -18;
+    x_dst = 21;y_dst =-21;
+    x_dst =5;y_dst = 0;
+
+    x_src = -20; y_src = -1000;
+    x_dst = 900; y_dst = -630;
+
+//    x_dst = 450; y_dst = -640;
+//    x_dst = -190; y_dst = -690;
+
+//    x_src = 900; y_src = -610;
+//    x_dst = -100; y_dst = -680;
+    std::vector<PlanPoint> xPlan;
+    double s;
+//    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,3.14,x_dst,y_dst,s,30.0,100.0,1,xPlan);
+
+//    ToGPSTrace(xPlan);
+
+
+//    double lat = 39.1443880;
+//    double lon = 117.0812543;
+
+//    xodrobj xo;
+//    xo.fhgdsrc = 340;
+//    xo.flatsrc = lat; xo.flonsrc = lon;
+//    xo.flat = 39.1490196;
+//    xo.flon = 117.0806979;
+//    xo.lane = 1;
+//    SetPlan(xo);
+
+
+ //   void * pivexit = iv::ivexit::RegIVExitCall(ExitFunc);
+
+    signal(SIGINT,signal_handler);
+
+    int nrc = a.exec();
+
+    std::cout<<"driver_map_xodr app exit. code :"<<nrc<<std::endl;
+
+    return nrc;
+}

src/driver/driver_map_xodrload/mconf.h

@@ -1,199 +1,199 @@
-/*							mconf.h
- *
- *	Common include file for math routines
- *
- *
- *
- * SYNOPSIS:
- *
- * #include "mconf.h"
- *
- *
- *
- * DESCRIPTION:
- *
- * This file contains definitions for error codes that are
- * passed to the common error handling routine mtherr()
- * (which see).
- *
- * The file also includes a conditional assembly definition
- * for the type of computer arithmetic (IEEE, DEC, Motorola
- * IEEE, or UNKnown).
- * 
- * For Digital Equipment PDP-11 and VAX computers, certain
- * IBM systems, and others that use numbers with a 56-bit
- * significand, the symbol DEC should be defined.  In this
- * mode, most floating point constants are given as arrays
- * of octal integers to eliminate decimal to binary conversion
- * errors that might be introduced by the compiler.
- *
- * For little-endian computers, such as IBM PC, that follow the
- * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE
- * Std 754-1985), the symbol IBMPC should be defined.  These
- * numbers have 53-bit significands.  In this mode, constants
- * are provided as arrays of hexadecimal 16 bit integers.
- *
- * Big-endian IEEE format is denoted MIEEE.  On some RISC
- * systems such as Sun SPARC, double precision constants
- * must be stored on 8-byte address boundaries.  Since integer
- * arrays may be aligned differently, the MIEEE configuration
- * may fail on such machines.
- *
- * To accommodate other types of computer arithmetic, all
- * constants are also provided in a normal decimal radix
- * which one can hope are correctly converted to a suitable
- * format by the available C language compiler.  To invoke
- * this mode, define the symbol UNK.
- *
- * An important difference among these modes is a predefined
- * set of machine arithmetic constants for each.  The numbers
- * MACHEP (the machine roundoff error), MAXNUM (largest number
- * represented), and several other parameters are preset by
- * the configuration symbol.  Check the file const.c to
- * ensure that these values are correct for your computer.
- *
- * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL
- * may fail on many systems.  Verify that they are supposed
- * to work on your computer.
- */
-/*
-Cephes Math Library Release 2.3:  June, 1995
-Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier
-*/
-
-
-/* Define if the `long double' type works.  */
-#define HAVE_LONG_DOUBLE 1
-
-/* Define as the return type of signal handlers (int or void).  */
-#define RETSIGTYPE void
-
-/* Define if you have the ANSI C header files.  */
-#define STDC_HEADERS 1
-
-/* Define if your processor stores words with the most significant
-   byte first (like Motorola and SPARC, unlike Intel and VAX).  */
-/* #undef WORDS_BIGENDIAN */
-
-/* Define if floating point words are bigendian.  */
-/* #undef FLOAT_WORDS_BIGENDIAN */
-
-/* The number of bytes in a int.  */
-#define SIZEOF_INT 4
-
-/* Define if you have the <string.h> header file.  */
-#define HAVE_STRING_H 1
-
-/* Name of package */
-#define PACKAGE "cephes"
-
-/* Version number of package */
-#define VERSION "2.7"
-
-/* Constant definitions for math error conditions
- */
-
-//#define DOMAIN		1	/* argument domain error */
-//#define SING		2	/* argument singularity */
-//#define OVERFLOW	3	/* overflow range error */
-//#define UNDERFLOW	4	/* underflow range error */
-//#define TLOSS		5	/* total loss of precision */
-//#define PLOSS		6	/* partial loss of precision */
-
-#define EDOM		33
-#define ERANGE		34
-/* Complex numeral.  */
-typedef struct
-	{
-	double r;
-	double i;
-	} cmplx;
-
-#ifdef HAVE_LONG_DOUBLE
-/* Long double complex numeral.  */
-typedef struct
-	{
-	long double r;
-	long double i;
-	} cmplxl;
-#endif
-
-
-/* Type of computer arithmetic */
-
-/* PDP-11, Pro350, VAX:
- */
-/* #define DEC 1 */
-
-/* Intel IEEE, low order words come first:
- */
-/* #define IBMPC 1 */
-
-/* Motorola IEEE, high order words come first
- * (Sun 680x0 workstation):
- */
-/* #define MIEEE 1 */
-
-/* UNKnown arithmetic, invokes coefficients given in
- * normal decimal format.  Beware of range boundary
- * problems (MACHEP, MAXLOG, etc. in const.c) and
- * roundoff problems in pow.c:
- * (Sun SPARCstation)
- */
-#define UNK 1
-
-/* If you define UNK, then be sure to set BIGENDIAN properly. */
-#ifdef FLOAT_WORDS_BIGENDIAN
-#define BIGENDIAN 1
-#else
-#define BIGENDIAN 0
-#endif
-/* Define this `volatile' if your compiler thinks
- * that floating point arithmetic obeys the associative
- * and distributive laws.  It will defeat some optimizations
- * (but probably not enough of them).
- *
- * #define VOLATILE volatile
- */
-#define VOLATILE
-
-/* For 12-byte long doubles on an i386, pad a 16-bit short 0
- * to the end of real constants initialized by integer arrays.
- *
- * #define XPD 0,
- *
- * Otherwise, the type is 10 bytes long and XPD should be
- * defined blank (e.g., Microsoft C).
- *
- * #define XPD
- */
-#define XPD 0,
-
-/* Define to support tiny denormal numbers, else undefine. */
-#define DENORMAL 1
-
-/* Define to ask for infinity support, else undefine. */
-#define INFINITIES 1
-
-/* Define to ask for support of numbers that are Not-a-Number,
-   else undefine.  This may automatically define INFINITIES in some files. */
-#define NANS 1
-
-/* Define to distinguish between -0.0 and +0.0.  */
-#define MINUSZERO 1
-
-/* Define 1 for ANSI C atan2() function
-   See atan.c and clog.c. */
-#define ANSIC 1
-
-/* Get ANSI function prototypes, if you want them. */
-#if 1
-/* #ifdef __STDC__ */
-#define ANSIPROT 1
-int mtherr ( char *, int );
-#else
-int mtherr();
-#endif
-
-/* Variable for error reporting.  See mtherr.c.  */
-extern int merror;
+/*							mconf.h
+ *
+ *	Common include file for math routines
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * #include "mconf.h"
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * This file contains definitions for error codes that are
+ * passed to the common error handling routine mtherr()
+ * (which see).
+ *
+ * The file also includes a conditional assembly definition
+ * for the type of computer arithmetic (IEEE, DEC, Motorola
+ * IEEE, or UNKnown).
+ * 
+ * For Digital Equipment PDP-11 and VAX computers, certain
+ * IBM systems, and others that use numbers with a 56-bit
+ * significand, the symbol DEC should be defined.  In this
+ * mode, most floating point constants are given as arrays
+ * of octal integers to eliminate decimal to binary conversion
+ * errors that might be introduced by the compiler.
+ *
+ * For little-endian computers, such as IBM PC, that follow the
+ * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE
+ * Std 754-1985), the symbol IBMPC should be defined.  These
+ * numbers have 53-bit significands.  In this mode, constants
+ * are provided as arrays of hexadecimal 16 bit integers.
+ *
+ * Big-endian IEEE format is denoted MIEEE.  On some RISC
+ * systems such as Sun SPARC, double precision constants
+ * must be stored on 8-byte address boundaries.  Since integer
+ * arrays may be aligned differently, the MIEEE configuration
+ * may fail on such machines.
+ *
+ * To accommodate other types of computer arithmetic, all
+ * constants are also provided in a normal decimal radix
+ * which one can hope are correctly converted to a suitable
+ * format by the available C language compiler.  To invoke
+ * this mode, define the symbol UNK.
+ *
+ * An important difference among these modes is a predefined
+ * set of machine arithmetic constants for each.  The numbers
+ * MACHEP (the machine roundoff error), MAXNUM (largest number
+ * represented), and several other parameters are preset by
+ * the configuration symbol.  Check the file const.c to
+ * ensure that these values are correct for your computer.
+ *
+ * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL
+ * may fail on many systems.  Verify that they are supposed
+ * to work on your computer.
+ */
+/*
+Cephes Math Library Release 2.3:  June, 1995
+Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier
+*/
+
+
+/* Define if the `long double' type works.  */
+#define HAVE_LONG_DOUBLE 1
+
+/* Define as the return type of signal handlers (int or void).  */
+#define RETSIGTYPE void
+
+/* Define if you have the ANSI C header files.  */
+#define STDC_HEADERS 1
+
+/* Define if your processor stores words with the most significant
+   byte first (like Motorola and SPARC, unlike Intel and VAX).  */
+/* #undef WORDS_BIGENDIAN */
+
+/* Define if floating point words are bigendian.  */
+/* #undef FLOAT_WORDS_BIGENDIAN */
+
+/* The number of bytes in a int.  */
+#define SIZEOF_INT 4
+
+/* Define if you have the <string.h> header file.  */
+#define HAVE_STRING_H 1
+
+/* Name of package */
+#define PACKAGE "cephes"
+
+/* Version number of package */
+#define VERSION "2.7"
+
+/* Constant definitions for math error conditions
+ */
+
+//#define DOMAIN		1	/* argument domain error */
+//#define SING		2	/* argument singularity */
+//#define OVERFLOW	3	/* overflow range error */
+//#define UNDERFLOW	4	/* underflow range error */
+//#define TLOSS		5	/* total loss of precision */
+//#define PLOSS		6	/* partial loss of precision */
+
+#define EDOM		33
+#define ERANGE		34
+/* Complex numeral.  */
+typedef struct
+	{
+	double r;
+	double i;
+	} cmplx;
+
+#ifdef HAVE_LONG_DOUBLE
+/* Long double complex numeral.  */
+typedef struct
+	{
+	long double r;
+	long double i;
+	} cmplxl;
+#endif
+
+
+/* Type of computer arithmetic */
+
+/* PDP-11, Pro350, VAX:
+ */
+/* #define DEC 1 */
+
+/* Intel IEEE, low order words come first:
+ */
+/* #define IBMPC 1 */
+
+/* Motorola IEEE, high order words come first
+ * (Sun 680x0 workstation):
+ */
+/* #define MIEEE 1 */
+
+/* UNKnown arithmetic, invokes coefficients given in
+ * normal decimal format.  Beware of range boundary
+ * problems (MACHEP, MAXLOG, etc. in const.c) and
+ * roundoff problems in pow.c:
+ * (Sun SPARCstation)
+ */
+#define UNK 1
+
+/* If you define UNK, then be sure to set BIGENDIAN properly. */
+#ifdef FLOAT_WORDS_BIGENDIAN
+#define BIGENDIAN 1
+#else
+#define BIGENDIAN 0
+#endif
+/* Define this `volatile' if your compiler thinks
+ * that floating point arithmetic obeys the associative
+ * and distributive laws.  It will defeat some optimizations
+ * (but probably not enough of them).
+ *
+ * #define VOLATILE volatile
+ */
+#define VOLATILE
+
+/* For 12-byte long doubles on an i386, pad a 16-bit short 0
+ * to the end of real constants initialized by integer arrays.
+ *
+ * #define XPD 0,
+ *
+ * Otherwise, the type is 10 bytes long and XPD should be
+ * defined blank (e.g., Microsoft C).
+ *
+ * #define XPD
+ */
+#define XPD 0,
+
+/* Define to support tiny denormal numbers, else undefine. */
+#define DENORMAL 1
+
+/* Define to ask for infinity support, else undefine. */
+#define INFINITIES 1
+
+/* Define to ask for support of numbers that are Not-a-Number,
+   else undefine.  This may automatically define INFINITIES in some files. */
+#define NANS 1
+
+/* Define to distinguish between -0.0 and +0.0.  */
+#define MINUSZERO 1
+
+/* Define 1 for ANSI C atan2() function
+   See atan.c and clog.c. */
+#define ANSIC 1
+
+/* Get ANSI function prototypes, if you want them. */
+#if 1
+/* #ifdef __STDC__ */
+#define ANSIPROT 1
+int mtherr ( char *, int );
+#else
+int mtherr();
+#endif
+
+/* Variable for error reporting.  See mtherr.c.  */
+extern int merror;

src/driver/driver_map_xodrload/planglobal.cpp

@@ -0,0 +1,15 @@
+#include "planglobal.h"
+
+PlanGlobal::PlanGlobal()
+{
+
+}
+
+
+int PlanGlobal::MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+                    const double x_obj,const double y_obj,const double & obj_dis,
+                    const double srcnearthresh,const double dstnearthresh,
+                    const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth)
+{
+    return 0;
+}

src/driver/driver_map_xodrload/planglobal.h

@@ -0,0 +1,22 @@
+#ifndef PLANGLOBAL_H
+#define PLANGLOBAL_H
+
+#include "xodrfunc.h"
+
+#include "xodrdijkstra.h"
+
+#include "planpoint.h"
+
+class PlanGlobal
+{
+public:
+    PlanGlobal();
+
+public:
+    static int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+                        const double x_obj,const double y_obj,const double & obj_dis,
+                        const double srcnearthresh,const double dstnearthresh,
+                        const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
+};
+
+#endif // PLANGLOBAL_H

src/driver/driver_map_xodrload/planpoint.h

@@ -1,41 +1,66 @@
-#ifndef PLANPOINT_H
-#define PLANPOINT_H
-
-class PlanPoint
-{
-public:
-    bool bInlaneAvoid = false;
-    double mx_left;
-    double my_left;
-    double mx_right;
-    double my_right;
-    double x;
-    double y;
-    double speed;
-    int lanmp; //left 1 right -1
-    double hdg;
-    double dis;
-    double mS;
-    double mWidth;  //Current Lane Width
-    double mLeftWidth[5];
-    double mRightWidth[5];
-
-    double mfRoadWidth;
-    double mfDisToRoadLeft;
-    double mfDisToLaneLeft;
-    int mnLaneori = 0;  //from Right 0
-    int mnLaneTotal = 1; //Lane Total
-
-    int nSignal = -1;   //if 0 no signal point
-
-    int nRoadID =-1;
-
-    double mfSecx;
-    double mfSecy;
-    int nlrchange; //1 left 2 right
-    bool mbBoringRoad = false;
-    bool mbNoavoid = false;
-    double mfCurvature = 0.0;
-};
-
-#endif // PLANPOINT_H
+#ifndef PLANPOINT_H
+#define PLANPOINT_H
+
+
+#include <vector>
+
+class PlanPoint
+{
+public:
+    bool bInlaneAvoid = false;
+    double mx_left;
+    double my_left;
+    double mx_right;
+    double my_right;
+    double x;
+    double y;
+    double speed;
+    int lanmp; //left 1 right -1
+    double hdg;
+    double dis;
+    double mS;
+    double mWidth;  //Current Lane Width
+    double mLeftWidth[5];
+    double mRightWidth[5];
+
+    double mfRoadWidth;
+    double mfDisToRoadLeft;
+    double mfDisToLaneLeft;
+    int mnLaneori = 0;  //from Right 0
+    int mnLaneTotal = 1; //Lane Total
+
+    int nSignal = -1;   //if 0 no signal point
+
+    int nRoadID =-1;
+
+    double mfSecx;
+    double mfSecy;
+    int nlrchange = 0; //1 left 2 right
+    bool mbBoringRoad = false;
+    bool mbNoavoid = false;
+    double mfCurvature = 0.0;
+
+    int mLaneCur;  //Current Lane 1 is left
+    int mLaneCount; //Current Side Lane
+    std::vector<double> mVectorLaneWidth; //Lane Width
+};
+
+
+#include <OpenDrive/OpenDrive.h>
+
+namespace iv {
+struct nearroad
+{
+Road * pObjRoad;
+GeometryBlock * pgeob;
+double nearx;
+double neary;
+double nearhead;
+double frels;
+double fdis;
+int lr = 2; //1 left 2 right;
+int nmode = 0; //0 same direciion dis zero   1 oposite dis = 0 2 same direction dis<5 3 oposite  dis <5   4 same direction > 5 5 oposite direction;
+};
+}
+
+#endif // PLANPOINT_H

src/driver/driver_map_xodrload/polevl.c

@@ -1,97 +1,97 @@
-/*							polevl.c
- *							p1evl.c
- *
- *	Evaluate polynomial
- *
- *
- *
- * SYNOPSIS:
- *
- * int N;
- * double x, y, coef[N+1], polevl[];
- *
- * y = polevl( x, coef, N );
- *
- *
- *
- * DESCRIPTION:
- *
- * Evaluates polynomial of degree N:
- *
- *                     2          N
- * y  =  C  + C x + C x  +...+ C x
- *        0    1     2          N
- *
- * Coefficients are stored in reverse order:
- *
- * coef[0] = C  , ..., coef[N] = C  .
- *            N                   0
- *
- *  The function p1evl() assumes that coef[N] = 1.0 and is
- * omitted from the array.  Its calling arguments are
- * otherwise the same as polevl().
- *
- *
- * SPEED:
- *
- * In the interest of speed, there are no checks for out
- * of bounds arithmetic.  This routine is used by most of
- * the functions in the library.  Depending on available
- * equipment features, the user may wish to rewrite the
- * program in microcode or assembly language.
- *
- */
-
-
-/*
-Cephes Math Library Release 2.1:  December, 1988
-Copyright 1984, 1987, 1988 by Stephen L. Moshier
-Direct inquiries to 30 Frost Street, Cambridge, MA 02140
-*/
-
-
-double polevl( double x,  double *coef, int N )
-//double x;
-//double coef[];
-//int N;
-{
-double ans;
-int i;
-double *p;
-
-p = coef;
-ans = *p++;
-i = N;
-
-do
-	ans = ans * x  +  *p++;
-while( --i );
-
-return( ans );
-}
-
-/*							p1evl()	*/
-/*                                          N
- * Evaluate polynomial when coefficient of x  is 1.0.
- * Otherwise same as polevl.
- */
-
-double p1evl( double x, double *coef, int N )
-//double x;
-//double coef[];
-//int N;
-{
-double ans;
-double *p;
-int i;
-
-p = coef;
-ans = x + *p++;
-i = N-1;
-
-do
-	ans = ans * x  + *p++;
-while( --i );
-
-return( ans );
-}
+/*							polevl.c
+ *							p1evl.c
+ *
+ *	Evaluate polynomial
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * int N;
+ * double x, y, coef[N+1], polevl[];
+ *
+ * y = polevl( x, coef, N );
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates polynomial of degree N:
+ *
+ *                     2          N
+ * y  =  C  + C x + C x  +...+ C x
+ *        0    1     2          N
+ *
+ * Coefficients are stored in reverse order:
+ *
+ * coef[0] = C  , ..., coef[N] = C  .
+ *            N                   0
+ *
+ *  The function p1evl() assumes that coef[N] = 1.0 and is
+ * omitted from the array.  Its calling arguments are
+ * otherwise the same as polevl().
+ *
+ *
+ * SPEED:
+ *
+ * In the interest of speed, there are no checks for out
+ * of bounds arithmetic.  This routine is used by most of
+ * the functions in the library.  Depending on available
+ * equipment features, the user may wish to rewrite the
+ * program in microcode or assembly language.
+ *
+ */
+
+
+/*
+Cephes Math Library Release 2.1:  December, 1988
+Copyright 1984, 1987, 1988 by Stephen L. Moshier
+Direct inquiries to 30 Frost Street, Cambridge, MA 02140
+*/
+
+
+double polevl( double x,  double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+int i;
+double *p;
+
+p = coef;
+ans = *p++;
+i = N;
+
+do
+	ans = ans * x  +  *p++;
+while( --i );
+
+return( ans );
+}
+
+/*							p1evl()	*/
+/*                                          N
+ * Evaluate polynomial when coefficient of x  is 1.0.
+ * Otherwise same as polevl.
+ */
+
+double p1evl( double x, double *coef, int N )
+//double x;
+//double coef[];
+//int N;
+{
+double ans;
+double *p;
+int i;
+
+p = coef;
+ans = x + *p++;
+i = N-1;
+
+do
+	ans = ans * x  + *p++;
+while( --i );
+
+return( ans );
+}

src/driver/driver_map_xodrload/readme.md

@@ -0,0 +1,3 @@
+主线上的工程直接复制到分支上来了
+
+余工修改了driver_map_xodrload, 增加了起点终点模糊路段智能选择的算法， 增加了终点靠边停车的算法（需要设置sidelaneenable为true）。

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -1,342 +1,342 @@
-#include "service_roi_xodr.h"
-
-#include <iostream>
-#include "gnss_coordinate_convert.h"
-
-service_roi_xodr::service_roi_xodr()
-{
-
-}
-
-void service_roi_xodr::SetXODR(OpenDrive *pxodr)
-{
-    mpxodr = pxodr;
-    mpxodr->GetHeader()->GetLat0Lon0(mlat0,mlon0);
-    updateroadarea();
-}
-
-
-#include <QTime>
-int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_ptr)
-{
-
-    xres_ptr = std::shared_ptr<iv::roi::resultarray>(new iv::roi::resultarray);
-    xres_ptr->set_lat(xreq_ptr->lat());
-    xres_ptr->set_lon(xreq_ptr->lon());
-    xres_ptr->set_heading(xreq_ptr->heading());
-    xres_ptr->set_range(xreq_ptr->range());
-    xres_ptr->set_gridsize(xreq_ptr->gridsize());
-
-    double x0,y0;
-    double x,y;
-    GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
-    GaussProjCal(mlon0,mlat0,&x0,&y0);
-    x = x - x0;
-    y = y - y0;
-    unsigned int nroadsize = mvectorarea.size();
-    unsigned int i;
-    unsigned int j;
-    std::vector<iv::roi::area> xvectorarea;
-
-    QTime xTime;
-    xTime.start();
-
-    //Find Near ROI area
-    for(i=0;i<nroadsize;i++)
-    {
-        double fdis1;
-        iv::roi::roadarea * proadarea = &mvectorarea[i];
-        fdis1 = sqrt(pow(proadarea->startPoint.x - x,2)+pow(proadarea->startPoint.y - y,2));
-        if(fdis1 > (xreq_ptr->range() + proadarea->fRoadLen + 30.0))  //+30.0 road width.
-        {
-            continue;
-        }
-        unsigned int nareasize = proadarea->mvectorarea.size();
-
-        for(j=0;j<nareasize;j++)
-        {
-            iv::roi::area * parea = &proadarea->mvectorarea[j];
-            double fd1,fd2,fd3,fd4;
-            fd1 = sqrt(pow(x - parea->P1.x,2)+pow(y - parea->P1.y,2));
-            if(fd1<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-                continue;
-            }
-            fd2 = sqrt(pow(x - parea->P2.x,2)+pow(y - parea->P2.y,2));
-            if(fd2<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-                continue;
-            }
-            fd3 = sqrt(pow(x - parea->P3.x,2)+pow(y - parea->P3.y,2));
-            if(fd3<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-                continue;
-            }
-            fd4 = sqrt(pow(x - parea->P4.x,2)+pow(y - parea->P4.y,2));
-            if(fd4<xreq_ptr->range())
-            {
-                xvectorarea.push_back(*parea);
-            }
-        }
-    }
-
-    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
-
-    if(xreq_ptr->gridsize() <= 0.0)
-    {
-        return -1;  //grid size is error
-    }
-
-    double fgridsize = xreq_ptr->gridsize()/2.0;
-
-    std::vector<iv::roi::Point> xvectorpoint;
-    unsigned int nareasize = xvectorarea.size();
-    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0 -3.0*M_PI/2.0;
-    for(i=0;i<nareasize;i++)
-    {
-        int ix,iy;
-        int ny = xvectorarea[i].fsteplen/fgridsize;
-        int nxl = 0;
-        int nxr = 0;
-        if(xvectorarea[i].fleftwidth>0)nxl = xvectorarea[i].fleftwidth/fgridsize;
-        if(xvectorarea[i].frightwidth>0)nxr = xvectorarea[i].frightwidth/fgridsize;
-        double fyaw = xvectorarea[i].fhdg - fgpsyaw;
- //       double fyaw = xvectorarea[i].fhdg - fgpsyaw;
-        double flaneoff = xvectorarea[i].flaneoff;
-        if(fyaw < 0)fyaw = fyaw + 2.0*M_PI;
-        double frelxraw = xvectorarea[i].refPoint.x - x;
-        double frelyraw = xvectorarea[i].refPoint.y - y;
-        double fxrel,fyrel;
-//        fgpsyaw = 0;
-        fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
-        fyrel = frelxraw * sin(-fgpsyaw) + frelyraw * cos(-fgpsyaw);
-        double fxbase = fxrel + flaneoff * cos(fyaw + M_PI/2.0);
-        double fybase = fyrel + flaneoff * sin(fyaw + M_PI/2.0);
-//        iv::roi::Point xpoint;
-//        xpoint.x = fxbase;
-//        xpoint.y = fybase;
-//        xvectorpoint.push_back(xpoint);
-//        continue;
-
-        fyaw = fyaw + 3.0*M_PI/2.0;
-        if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
-        {
-            iv::roi::Point xpoint;
-            xpoint.x = fxbase;
-            xpoint.y = fybase;
-            xvectorpoint.push_back(xpoint);
-        }
-        else
-        {
-            if((nxl == 0)&&(nxr== 0))
-            {
-                for(iy=(-ny);iy<=0;iy++)
-                {
-                    iv::roi::Point xpoint;
-                    xpoint.x = fxbase +(iy*fgridsize)*cos(fyaw);
-                    xpoint.y = fybase +(iy*fgridsize)*sin(fyaw);
-                    xvectorpoint.push_back(xpoint);
-                }
-            }
-            else
-            {
-                for(ix=(-nxl);ix<=nxr;ix++)
-                {
-                    for(iy=(-ny);iy<=0;iy++)
-                    {
-                        double fxlocal,fylocal;
-                        fxlocal = ix*fgridsize;
-                        fylocal = iy*fgridsize;
-                        iv::roi::Point xpoint;
-
-                        xpoint.x = fxbase +fxlocal*cos(fyaw) - fylocal * sin(fyaw);
-                        xpoint.y = fybase +fxlocal*sin(fyaw) + fylocal * cos(fyaw);
-                        xvectorpoint.push_back(xpoint);
-                    }
-                }
-            }
-        }
-    }
-
-
-    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
-
-    for(i=0;i<xvectorpoint.size();i++)
-    {
-        iv::roi::resultunit * padd = xres_ptr->add_res();
-//        std::cout<<" x: "<<xvectorpoint[i].x<<" y: "<<xvectorpoint[i].y<<std::endl;
-        padd->set_x(xvectorpoint[i].x);
-        padd->set_y(xvectorpoint[i].y);
-    }
-
-    return 0;
-}
-
-void service_roi_xodr::updateroadarea()
-{
-    OpenDrive * pxodr = mpxodr;
-    mvectorarea.clear();
-    unsigned int nroadcount = pxodr->GetRoadCount();
-    unsigned int i;
-    double fstep = GEOSAMPLESTEP;
-    for(i=0;i<nroadcount;i++)
-    {
-        Road * pRoad = pxodr->GetRoad(i);
-        if(pRoad == NULL)
-        {
-            std::cout<<" Warning: OpenDrive Road "<<i<<" is NULL"<<std::endl;
-            continue;
-        }
-        iv::roi::roadarea xroadarea = GetRoadArea(pRoad,fstep);
-        mvectorarea.push_back(xroadarea);
-    }
-}
-
-iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
-{
-    iv::roi::roadarea xroadarea;
-    xroadarea.fRoadLen = pRoad->GetRoadLength();
-    double x1,y1,hdg1;
-    pRoad->GetGeometryCoords(0.0,x1,y1,hdg1);
-    xroadarea.startPoint.x = x1;
-    xroadarea.startPoint.y = y1;
-    unsigned int nSectionCount = pRoad->GetLaneSectionCount();
-    unsigned int i;
-    if(nSectionCount == 0)
-    {
-        std::cout<<"Warning. Road"<<pRoad->GetRoadId()<<" not have lanesection."<<std::endl;
-    }
-    double snow= 0;
-    for(i=0;i<nSectionCount;i++)
-    {
-        LaneSection * pLS = pRoad->GetLaneSection(i);
-        if(pLS == NULL)
-        {
-            std::cout<<"Warning. LaneSection is NULL."<<std::endl;
-            continue;
-        }
-        snow = pLS->GetS();
-        double endS;
-        if(i == (nSectionCount -1))
-        {
-            endS = pRoad->GetRoadLength();
-        }
-        else
-        {
-            endS = pRoad->GetLaneSection(i+1)->GetS();
-        }
-
-        double fleftwidth = pRoad->GetRoadLeftWidth(snow);
-        double frigthwidth = pRoad->GetRoadRightWidth(snow);
-        double x,y,yaw;
-        pRoad->GetGeometryCoords(snow,x,y,yaw);
-        iv::roi::Point pointLeft,pointRight,pointLeft_old,pointRight_old;
-        double flaneoff = pRoad->GetLaneOffsetValue(snow);
-        pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
-        pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
-        pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
-        pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
-        pointLeft_old = pointLeft;
-        pointRight_old = pointRight;
-        snow = snow + fstep;
-        while(snow < endS)
-        {
-            fleftwidth = pRoad->GetRoadLeftWidth(snow);
-            frigthwidth = pRoad->GetRoadRightWidth(snow);
-            pRoad->GetGeometryCoords(snow,x,y,yaw);
-            flaneoff = pRoad->GetLaneOffsetValue(snow);
-            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
-            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
-            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
-            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
-            iv::roi::area xarea;
-            xarea.P1 = pointLeft_old;
-            xarea.P2 = pointLeft;
-            xarea.P3 = pointRight;
-            xarea.P4 = pointRight_old;
-            xarea.refPoint.x = x;
-            xarea.refPoint.y = y;
-            xarea.s = snow;
-            xarea.fmaxlen = fleftwidth + frigthwidth;
-            xarea.fhdg = yaw;
-            xarea.fleftwidth = fleftwidth;
-            xarea.frightwidth = frigthwidth;
-            xarea.flaneoff = flaneoff;
-            xarea.fsteplen = fstep;
-            xroadarea.mvectorarea.push_back(xarea);
-//            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
-//            double frigthwidth = pRoad->GetRoadRightWidth(snow);
-//            std::cout<<" road : "<<pRoad->GetRoadId()<<" s: "<<snow<<" left: "<<fleftwidth<<" right:"<<frigthwidth<<std::endl;
-
-            pointLeft_old = pointLeft;
-            pointRight_old = pointRight;
-            snow = snow + fstep;
-
-        }
-        if((snow-fstep) < (endS - 0.000001))
-        {
-            double foldsnow = snow - fstep;
-            snow = endS - 0.000001;
-            fleftwidth = pRoad->GetRoadLeftWidth(snow);
-            frigthwidth = pRoad->GetRoadRightWidth(snow);
-            pRoad->GetGeometryCoords(snow,x,y,yaw);
-            flaneoff = pRoad->GetLaneOffsetValue(snow);
-            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
-            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
-            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
-            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
-            iv::roi::area xarea;
-            xarea.P1 = pointLeft_old;
-            xarea.P2 = pointLeft;
-            xarea.P3 = pointRight;
-            xarea.P4 = pointRight_old;
-            xarea.refPoint.x = x;
-            xarea.refPoint.y = y;
-            xarea.s = snow;
-            xarea.fhdg = yaw;
-            xarea.fleftwidth = fleftwidth;
-            xarea.frightwidth = frigthwidth;
-            xarea.flaneoff = flaneoff;
-            xarea.fsteplen = endS - foldsnow;
-            xroadarea.mvectorarea.push_back(xarea);
-        }
-    }
-    return xroadarea;
-}
-
-bool service_roi_xodr::CheckPointInArea(double x, double y, iv::roi::area &xarea)
-{
-    double a = (xarea.P2.x - xarea.P1.x)*(y - xarea.P1.y) - (xarea.P2.y-xarea.P1.y)*(x-xarea.P1.x);
-    double b = (xarea.P3.x - xarea.P2.x)*(y - xarea.P2.y) - (xarea.P3.y-xarea.P2.y)*(x-xarea.P2.x);
-    double c = (xarea.P4.x - xarea.P3.x)*(y - xarea.P3.y) - (xarea.P4.y-xarea.P3.y)*(x-xarea.P3.x);
-    double d = (xarea.P1.x - xarea.P4.x)*(y - xarea.P4.y) - (xarea.P1.y-xarea.P4.y)*(x-xarea.P4.x);
-
-    if((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0)) {
-        return true;
-    }
-    return false;
-}
-
-bool service_roi_xodr::CheckPointInROI(double x, double y, std::vector<iv::roi::area> &xvectorroi)
-{
-    unsigned int i;
-    unsigned nsize = xvectorroi.size();
-    for(i=0;i<nsize;i++)
-    {
-        iv::roi::area xarea = xvectorroi[i];
-        double fdis = sqrt(pow(xarea.refPoint.x-x,2)+pow(xarea.refPoint.y-y,2));
-        if(fdis>(xarea.fmaxlen+0.1))
-        {
-            continue;
-        }
-        if(CheckPointInArea(x,y,xarea))
-        {
-            return true;
-        }
-    }
-    return false;
-}
-
+#include "service_roi_xodr.h"
+
+#include <iostream>
+#include "gnss_coordinate_convert.h"
+
+service_roi_xodr::service_roi_xodr()
+{
+
+}
+
+void service_roi_xodr::SetXODR(OpenDrive *pxodr)
+{
+    mpxodr = pxodr;
+    mpxodr->GetHeader()->GetLat0Lon0(mlat0,mlon0);
+    updateroadarea();
+}
+
+
+#include <QTime>
+int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::shared_ptr<iv::roi::resultarray> &xres_ptr)
+{
+
+    xres_ptr = std::shared_ptr<iv::roi::resultarray>(new iv::roi::resultarray);
+    xres_ptr->set_lat(xreq_ptr->lat());
+    xres_ptr->set_lon(xreq_ptr->lon());
+    xres_ptr->set_heading(xreq_ptr->heading());
+    xres_ptr->set_range(xreq_ptr->range());
+    xres_ptr->set_gridsize(xreq_ptr->gridsize());
+
+    double x0,y0;
+    double x,y;
+    GaussProjCal(xreq_ptr->lon(),xreq_ptr->lat(),&x,&y);
+    GaussProjCal(mlon0,mlat0,&x0,&y0);
+    x = x - x0;
+    y = y - y0;
+    unsigned int nroadsize = mvectorarea.size();
+    unsigned int i;
+    unsigned int j;
+    std::vector<iv::roi::area> xvectorarea;
+
+    QTime xTime;
+    xTime.start();
+
+    //Find Near ROI area
+    for(i=0;i<nroadsize;i++)
+    {
+        double fdis1;
+        iv::roi::roadarea * proadarea = &mvectorarea[i];
+        fdis1 = sqrt(pow(proadarea->startPoint.x - x,2)+pow(proadarea->startPoint.y - y,2));
+        if(fdis1 > (xreq_ptr->range() + proadarea->fRoadLen + 30.0))  //+30.0 road width.
+        {
+            continue;
+        }
+        unsigned int nareasize = proadarea->mvectorarea.size();
+
+        for(j=0;j<nareasize;j++)
+        {
+            iv::roi::area * parea = &proadarea->mvectorarea[j];
+            double fd1,fd2,fd3,fd4;
+            fd1 = sqrt(pow(x - parea->P1.x,2)+pow(y - parea->P1.y,2));
+            if(fd1<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd2 = sqrt(pow(x - parea->P2.x,2)+pow(y - parea->P2.y,2));
+            if(fd2<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd3 = sqrt(pow(x - parea->P3.x,2)+pow(y - parea->P3.y,2));
+            if(fd3<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+                continue;
+            }
+            fd4 = sqrt(pow(x - parea->P4.x,2)+pow(y - parea->P4.y,2));
+            if(fd4<xreq_ptr->range())
+            {
+                xvectorarea.push_back(*parea);
+            }
+        }
+    }
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+
+    if(xreq_ptr->gridsize() <= 0.0)
+    {
+        return -1;  //grid size is error
+    }
+
+    double fgridsize = xreq_ptr->gridsize()/2.0;
+
+    std::vector<iv::roi::Point> xvectorpoint;
+    unsigned int nareasize = xvectorarea.size();
+    double fgpsyaw = (90-xreq_ptr->heading())*M_PI/180.0 -3.0*M_PI/2.0;
+    for(i=0;i<nareasize;i++)
+    {
+        int ix,iy;
+        int ny = xvectorarea[i].fsteplen/fgridsize;
+        int nxl = 0;
+        int nxr = 0;
+        if(xvectorarea[i].fleftwidth>0)nxl = xvectorarea[i].fleftwidth/fgridsize;
+        if(xvectorarea[i].frightwidth>0)nxr = xvectorarea[i].frightwidth/fgridsize;
+        double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+ //       double fyaw = xvectorarea[i].fhdg - fgpsyaw;
+        double flaneoff = xvectorarea[i].flaneoff;
+        if(fyaw < 0)fyaw = fyaw + 2.0*M_PI;
+        double frelxraw = xvectorarea[i].refPoint.x - x;
+        double frelyraw = xvectorarea[i].refPoint.y - y;
+        double fxrel,fyrel;
+//        fgpsyaw = 0;
+        fxrel = frelxraw * cos(-fgpsyaw) - frelyraw * sin(-fgpsyaw);
+        fyrel = frelxraw * sin(-fgpsyaw) + frelyraw * cos(-fgpsyaw);
+        double fxbase = fxrel + flaneoff * cos(fyaw + M_PI/2.0);
+        double fybase = fyrel + flaneoff * sin(fyaw + M_PI/2.0);
+//        iv::roi::Point xpoint;
+//        xpoint.x = fxbase;
+//        xpoint.y = fybase;
+//        xvectorpoint.push_back(xpoint);
+//        continue;
+
+        fyaw = fyaw + 3.0*M_PI/2.0;
+        if((nxl == 0)&&(nxr== 0)&&(ny <= 1))
+        {
+            iv::roi::Point xpoint;
+            xpoint.x = fxbase;
+            xpoint.y = fybase;
+            xvectorpoint.push_back(xpoint);
+        }
+        else
+        {
+            if((nxl == 0)&&(nxr== 0))
+            {
+                for(iy=(-ny);iy<=0;iy++)
+                {
+                    iv::roi::Point xpoint;
+                    xpoint.x = fxbase +(iy*fgridsize)*cos(fyaw);
+                    xpoint.y = fybase +(iy*fgridsize)*sin(fyaw);
+                    xvectorpoint.push_back(xpoint);
+                }
+            }
+            else
+            {
+                for(ix=(-nxl);ix<=nxr;ix++)
+                {
+                    for(iy=(-ny);iy<=0;iy++)
+                    {
+                        double fxlocal,fylocal;
+                        fxlocal = ix*fgridsize;
+                        fylocal = iy*fgridsize;
+                        iv::roi::Point xpoint;
+
+                        xpoint.x = fxbase +fxlocal*cos(fyaw) - fylocal * sin(fyaw);
+                        xpoint.y = fybase +fxlocal*sin(fyaw) + fylocal * cos(fyaw);
+                        xvectorpoint.push_back(xpoint);
+                    }
+                }
+            }
+        }
+    }
+
+
+    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
+
+    for(i=0;i<xvectorpoint.size();i++)
+    {
+        iv::roi::resultunit * padd = xres_ptr->add_res();
+//        std::cout<<" x: "<<xvectorpoint[i].x<<" y: "<<xvectorpoint[i].y<<std::endl;
+        padd->set_x(xvectorpoint[i].x);
+        padd->set_y(xvectorpoint[i].y);
+    }
+
+    return 0;
+}
+
+void service_roi_xodr::updateroadarea()
+{
+    OpenDrive * pxodr = mpxodr;
+    mvectorarea.clear();
+    unsigned int nroadcount = pxodr->GetRoadCount();
+    unsigned int i;
+    double fstep = GEOSAMPLESTEP;
+    for(i=0;i<nroadcount;i++)
+    {
+        Road * pRoad = pxodr->GetRoad(i);
+        if(pRoad == NULL)
+        {
+            std::cout<<" Warning: OpenDrive Road "<<i<<" is NULL"<<std::endl;
+            continue;
+        }
+        iv::roi::roadarea xroadarea = GetRoadArea(pRoad,fstep);
+        mvectorarea.push_back(xroadarea);
+    }
+}
+
+iv::roi::roadarea service_roi_xodr::GetRoadArea(Road *pRoad, const double fstep)
+{
+    iv::roi::roadarea xroadarea;
+    xroadarea.fRoadLen = pRoad->GetRoadLength();
+    double x1,y1,hdg1;
+    pRoad->GetGeometryCoords(0.0,x1,y1,hdg1);
+    xroadarea.startPoint.x = x1;
+    xroadarea.startPoint.y = y1;
+    unsigned int nSectionCount = pRoad->GetLaneSectionCount();
+    unsigned int i;
+    if(nSectionCount == 0)
+    {
+        std::cout<<"Warning. Road"<<pRoad->GetRoadId()<<" not have lanesection."<<std::endl;
+    }
+    double snow= 0;
+    for(i=0;i<nSectionCount;i++)
+    {
+        LaneSection * pLS = pRoad->GetLaneSection(i);
+        if(pLS == NULL)
+        {
+            std::cout<<"Warning. LaneSection is NULL."<<std::endl;
+            continue;
+        }
+        snow = pLS->GetS();
+        double endS;
+        if(i == (nSectionCount -1))
+        {
+            endS = pRoad->GetRoadLength();
+        }
+        else
+        {
+            endS = pRoad->GetLaneSection(i+1)->GetS();
+        }
+
+        double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+        double frigthwidth = pRoad->GetRoadRightWidth(snow);
+        double x,y,yaw;
+        pRoad->GetGeometryCoords(snow,x,y,yaw);
+        iv::roi::Point pointLeft,pointRight,pointLeft_old,pointRight_old;
+        double flaneoff = pRoad->GetLaneOffsetValue(snow);
+        pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+        pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+        pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+        pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+        pointLeft_old = pointLeft;
+        pointRight_old = pointRight;
+        snow = snow + fstep;
+        while(snow < endS)
+        {
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xarea.fmaxlen = fleftwidth + frigthwidth;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = fstep;
+            xroadarea.mvectorarea.push_back(xarea);
+//            double fleftwidth = pRoad->GetRoadLeftWidth(snow);
+//            double frigthwidth = pRoad->GetRoadRightWidth(snow);
+//            std::cout<<" road : "<<pRoad->GetRoadId()<<" s: "<<snow<<" left: "<<fleftwidth<<" right:"<<frigthwidth<<std::endl;
+
+            pointLeft_old = pointLeft;
+            pointRight_old = pointRight;
+            snow = snow + fstep;
+
+        }
+        if((snow-fstep) < (endS - 0.000001))
+        {
+            double foldsnow = snow - fstep;
+            snow = endS - 0.000001;
+            fleftwidth = pRoad->GetRoadLeftWidth(snow);
+            frigthwidth = pRoad->GetRoadRightWidth(snow);
+            pRoad->GetGeometryCoords(snow,x,y,yaw);
+            flaneoff = pRoad->GetLaneOffsetValue(snow);
+            pointLeft.x = x + (fleftwidth + flaneoff) * cos(yaw + M_PI/2.0);
+            pointLeft.y = y + (fleftwidth + flaneoff) * sin(yaw + M_PI/2.0);
+            pointRight.x = x + (frigthwidth - flaneoff ) * cos(yaw - M_PI/2.0);
+            pointRight.y = y + (frigthwidth - flaneoff) * sin(yaw -M_PI/2.0);
+            iv::roi::area xarea;
+            xarea.P1 = pointLeft_old;
+            xarea.P2 = pointLeft;
+            xarea.P3 = pointRight;
+            xarea.P4 = pointRight_old;
+            xarea.refPoint.x = x;
+            xarea.refPoint.y = y;
+            xarea.s = snow;
+            xarea.fhdg = yaw;
+            xarea.fleftwidth = fleftwidth;
+            xarea.frightwidth = frigthwidth;
+            xarea.flaneoff = flaneoff;
+            xarea.fsteplen = endS - foldsnow;
+            xroadarea.mvectorarea.push_back(xarea);
+        }
+    }
+    return xroadarea;
+}
+
+bool service_roi_xodr::CheckPointInArea(double x, double y, iv::roi::area &xarea)
+{
+    double a = (xarea.P2.x - xarea.P1.x)*(y - xarea.P1.y) - (xarea.P2.y-xarea.P1.y)*(x-xarea.P1.x);
+    double b = (xarea.P3.x - xarea.P2.x)*(y - xarea.P2.y) - (xarea.P3.y-xarea.P2.y)*(x-xarea.P2.x);
+    double c = (xarea.P4.x - xarea.P3.x)*(y - xarea.P3.y) - (xarea.P4.y-xarea.P3.y)*(x-xarea.P3.x);
+    double d = (xarea.P1.x - xarea.P4.x)*(y - xarea.P4.y) - (xarea.P1.y-xarea.P4.y)*(x-xarea.P4.x);
+
+    if((a > 0 && b > 0 && c > 0 && d > 0) || (a < 0 && b < 0 && c < 0 && d < 0)) {
+        return true;
+    }
+    return false;
+}
+
+bool service_roi_xodr::CheckPointInROI(double x, double y, std::vector<iv::roi::area> &xvectorroi)
+{
+    unsigned int i;
+    unsigned nsize = xvectorroi.size();
+    for(i=0;i<nsize;i++)
+    {
+        iv::roi::area xarea = xvectorroi[i];
+        double fdis = sqrt(pow(xarea.refPoint.x-x,2)+pow(xarea.refPoint.y-y,2));
+        if(fdis>(xarea.fmaxlen+0.1))
+        {
+            continue;
+        }
+        if(CheckPointInArea(x,y,xarea))
+        {
+            return true;
+        }
+    }
+    return false;
+}
+

src/driver/driver_map_xodrload/service_roi_xodr.h

@@ -1,86 +1,86 @@
-#ifndef SERVICE_ROI_XODR_H
-#define SERVICE_ROI_XODR_H
-
-#include <vector>
-#include <OpenDrive/OpenDrive.h>
-#include "gpsimu.pb.h"
-#include "roireqest.pb.h"
-#include "roiresult.pb.h"
-
-namespace iv {
-
-namespace roi {
-
-struct Point
-{
-    double x;
-    double y;
-};
-
-struct area
-{
-
-    double s;
-    Point refPoint;
-    Point P1;
-    Point P2;
-    Point P3;
-    Point P4;
-    double fhdg;
-    double fleftwidth;
-    double frightwidth;
-    double fsteplen;
-    double flaneoff;
-    double fmaxlen;
-
-};
-
-struct roadarea
-{
-    int nroadid;
-    Point startPoint;
-    double fRoadLen;
-    std::vector<area> mvectorarea;
-};
-}
-}
-
-class service_roi_xodr
-{
-
-
-public:
-    service_roi_xodr();
-
-    /**
-     * @brief SetXODR Set OpenDrive Pointer to Service
-     * @param pxodr
-     */
-    void SetXODR(OpenDrive * pxodr);
-
-    /**
-     * @brief GetROI Get ROI From XODR Map
-     * @param xreq_ptr  now position and roi param
-     * @param xres_prt  result
-     * @return 0 Succeed -1 No Map  -2 Not in Map
-     */
-    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_ptr );
-
-private:
-    void updateroadarea();
-    iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
-
-    bool CheckPointInROI(double x,double y, std::vector<iv::roi::area> & xvectorroi);
-
-    bool CheckPointInArea(double x, double y,iv::roi::area & xarea);
-
-private:
-    OpenDrive * mpxodr = NULL;
-    std::vector<iv::roi::roadarea> mvectorarea;
-    double mlon0;
-    double mlat0;
-    const double GEOSAMPLESTEP = 0.5;  // Get Road Unit at 0.5m
-
-};
-
-#endif // SERVICE_ROI_XODR_H
+#ifndef SERVICE_ROI_XODR_H
+#define SERVICE_ROI_XODR_H
+
+#include <vector>
+#include <OpenDrive/OpenDrive.h>
+#include "gpsimu.pb.h"
+#include "roireqest.pb.h"
+#include "roiresult.pb.h"
+
+namespace iv {
+
+namespace roi {
+
+struct Point
+{
+    double x;
+    double y;
+};
+
+struct area
+{
+
+    double s;
+    Point refPoint;
+    Point P1;
+    Point P2;
+    Point P3;
+    Point P4;
+    double fhdg;
+    double fleftwidth;
+    double frightwidth;
+    double fsteplen;
+    double flaneoff;
+    double fmaxlen;
+
+};
+
+struct roadarea
+{
+    int nroadid;
+    Point startPoint;
+    double fRoadLen;
+    std::vector<area> mvectorarea;
+};
+}
+}
+
+class service_roi_xodr
+{
+
+
+public:
+    service_roi_xodr();
+
+    /**
+     * @brief SetXODR Set OpenDrive Pointer to Service
+     * @param pxodr
+     */
+    void SetXODR(OpenDrive * pxodr);
+
+    /**
+     * @brief GetROI Get ROI From XODR Map
+     * @param xreq_ptr  now position and roi param
+     * @param xres_prt  result
+     * @return 0 Succeed -1 No Map  -2 Not in Map
+     */
+    int GetROI(std::shared_ptr<iv::roi::request> xreq_ptr,std::shared_ptr<iv::roi::resultarray> & xres_ptr );
+
+private:
+    void updateroadarea();
+    iv::roi::roadarea GetRoadArea(Road * pRoad,const double fstep);
+
+    bool CheckPointInROI(double x,double y, std::vector<iv::roi::area> & xvectorroi);
+
+    bool CheckPointInArea(double x, double y,iv::roi::area & xarea);
+
+private:
+    OpenDrive * mpxodr = NULL;
+    std::vector<iv::roi::roadarea> mvectorarea;
+    double mlon0;
+    double mlat0;
+    const double GEOSAMPLESTEP = 0.5;  // Get Road Unit at 0.5m
+
+};
+
+#endif // SERVICE_ROI_XODR_H

src/driver/driver_map_xodrload/xodrplan.cpp

@@ -1,15 +1,15 @@
-#include "xodrplan.h"
-
-xodrplan::xodrplan()
-{
-
-}
-
-int xodrplan::MakePlan(xodrdijkstra *pxd, OpenDrive *pxodr, const double x_now,
-                       const double y_now, const double head, const double x_obj,
-                       const double y_obj, const double &obj_dis, const double srcnearthresh,
-                       const double dstnearthresh, const int nlanesel, std::vector<PlanPoint> &xPlan,
-                       const double fvehiclewidth)
-{
-
-}
+#include "xodrplan.h"
+
+xodrplan::xodrplan()
+{
+
+}
+
+int xodrplan::MakePlan(xodrdijkstra *pxd, OpenDrive *pxodr, const double x_now,
+                       const double y_now, const double head, const double x_obj,
+                       const double y_obj, const double &obj_dis, const double srcnearthresh,
+                       const double dstnearthresh, const int nlanesel, std::vector<PlanPoint> &xPlan,
+                       const double fvehiclewidth)
+{
+
+}

src/driver/driver_map_xodrload/xodrplan.h

@@ -1,26 +1,26 @@
-#ifndef XODRPLAN_H
-#define XODRPLAN_H
-
-#include "OpenDrive/OpenDrive.h"
-
-#include <vector>
-
-#include "xodrfunc.h"
-
-#include "xodrdijkstra.h"
-
-#include "planpoint.h"
-
-class xodrplan
-{
-public:
-    xodrplan();
-
-public:
-    static int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
-                 const double x_obj,const double y_obj,const double & obj_dis,
-                 const double srcnearthresh,const double dstnearthresh,
-                 const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
-};
-
-#endif // XODRPLAN_H
+#ifndef XODRPLAN_H
+#define XODRPLAN_H
+
+#include "OpenDrive/OpenDrive.h"
+
+#include <vector>
+
+#include "xodrfunc.h"
+
+#include "xodrdijkstra.h"
+
+#include "planpoint.h"
+
+class xodrplan
+{
+public:
+    xodrplan();
+
+public:
+    static int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const double y_now,const double head,
+                 const double x_obj,const double y_obj,const double & obj_dis,
+                 const double srcnearthresh,const double dstnearthresh,
+                 const int nlanesel,std::vector<PlanPoint> & xPlan,const double fvehiclewidth = 2.0);
+};
+
+#endif // XODRPLAN_H

src/driver/driver_rs_m1/ChannelNum.csv

@@ -0,0 +1,44 @@
+syntax = "proto2";
+
+package iv.map;
+
+message pointglobal
+{
+        required double gps_lat = 1 [default=0.0];//纬度
+        required double gps_lng  = 2 [default=0.0];//经度
+        required double ins_heading_angle  = 3 [default=0];	//航向角
+
+        optional double mfCurvature  = 4 [default=0];
+
+        optional double gps_x  = 5 [default=0];   
+        optional double gps_y  = 6 [default=0];
+        optional double gps_z  = 7 [default=0];
+
+        
+        optional double speed  = 8 [default=0];			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+
+        required double mfLaneWidth  = 9 [default=3.5]; // Current Lane Width
+
+        optional double mfDisToLaneLeft  = 10 [default=1.8]; //Distance to Lane Left
+        optional int32 mnLaneChangeMark  = 11 [default=0]; //1 to Left 0 not change   -1 to right
+        optional double mfDisToRoadLeft  = 12 [default=1.8]; //Distance to Road Left
+        optional double mfRoadWidth  = 13 [default=3.5]; // Road Width
+        
+        optional int32  mlanecur = 15; //当前车道编号 1 靠近中心线的第一车道
+        optional int32  mlanecount = 16; //当前道路测的可行驶车道数
+        repeated double mfVectorLaneWidth = 17; //从左到右车道宽       
+        
+        required int32 mnRoadID = 18;        
+        optional string mstrRoadName = 19;
+	
+
+
+};
+
+
+message mapglobal
+{
+	repeated pointglobal point = 1;
+	optional int32 nState  = 2;  
+};

src/include/proto/vbox.proto

@@ -5,13 +5,13 @@ package iv.vbox;
 message vbox_light
 {
 	required uint32 st_straight	= 1;
-	required uint32 st_left 	= 2;
-	required uint32 st_right	= 3;
-	required uint32 st_turn		= 4;
+	optional uint32 st_left 	= 2;
+	optional uint32 st_right	= 3;
+	optional uint32 st_turn		= 4;
 	required uint32 time_straight	= 5;
-	required uint32 time_left	= 6;
-	required uint32 time_right 	= 7;
-	required uint32 time_turn 	= 8;
+	optional uint32 time_left	= 6;
+	optional uint32 time_right 	= 7;
+	optional uint32 time_turn 	= 8;
 };
 
 message vbox_obj_arr

src/include/proto3/protomake.sh

@@ -400,7 +400,7 @@ ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
     shareMapReqMsg();
 
     ModuleFun funvbox = std::bind(&ADCIntelligentVehicle::UpdateVbox,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpaVbox = iv::modulecomm::RegisterRecvPlus("vbox",funvbox);
+    mpaVbox = iv::modulecomm::RegisterRecvPlus("vboxLight",funvbox);
 
 #if 1
     ui->lcdNumber->setDigitCount(3); //设置显示几个数字
@@ -1738,6 +1738,14 @@ void ADCIntelligentVehicle::UpdateMap(const char * strdata,const unsigned int nS
 }
 
 
+
+
+
+#ifdef USE_UTM
+#include <GeographicLib/UTMUPS.hpp>
+#include <iostream>
+#endif
+
 //=======================zhaobo0904
 #define PI  3.14159265358979
 
@@ -1802,6 +1810,19 @@ void GaussProjCal(double lon, double lat, double *X, double *Y)
 //高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
 void GaussProjCal(double longitude, double latitude, double *X, double *Y)
 {
+
+#ifdef USE_UTM
+
+    int zone{};
+    bool northp{};
+    try {
+      GeographicLib::UTMUPS::Forward(latitude, longitude, zone, northp, *X,*Y);
+      zone = zone;
+    } catch (GeographicLib::GeographicErr& e) {
+  //    throw ForwardProjectionError(e.what());
+        std::cout<<"GeographicLib::UTMUPS::Forward Fail. what: "<<e.what()<<std::endl;
+    }
+#else
     int ProjNo = 0; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double a, f, e2, ee, NN, T, C, A, M, iPI;
@@ -1832,13 +1853,29 @@ void GaussProjCal(double longitude, double latitude, double *X, double *Y)
     xval = xval + X0; yval = yval + Y0;
     *X = xval;
     *Y = yval;
+#endif
 }
 
 
 
+
+
 //高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
 void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
 {
+
+#ifdef USE_UTM
+    int zone = 50;
+    bool isInNorthernHemisphere_ = true;
+    try {
+      GeographicLib::UTMUPS::Reverse(zone, isInNorthernHemisphere_, X,
+                                     Y, *latitude, *longitude);
+    } catch (GeographicLib::GeographicErr& e) {
+      std::cout<<"GeographicLib::UTMUPS::Reverse what:  "<<e.what()<<std::endl;
+    }
+
+#else
+
     int ProjNo; int ZoneWide; ////带宽
     double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
     double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
@@ -1874,8 +1911,9 @@ void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
     //转换为度 DD
     *longitude = longitude1 / iPI;
     *latitude = latitude1 / iPI;
-}
 
+#endif
+}
 
 //==========================================================
 
@@ -2264,7 +2302,7 @@ void ADCIntelligentVehicle::shareMapReqMsg()
 
 void ADCIntelligentVehicle::UpdateVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-    iv::vbox::vbox xvbox;
+    iv::vbox::vbox_light xvbox;
     if(!xvbox.ParseFromArray(strdata,nSize))
     {
         gIvlog->warn("ADCIntelligentVehicle::UpdateVbox parse errror. nSize is %d",nSize);
@@ -2306,12 +2344,12 @@ void ADCIntelligentVehicle::onStateTimer1()
             img = img.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_38->setPixmap(QPixmap::fromImage(img));
             break;
-        case 2:
+        case 3:
             img.load(":/light_image/diaotou-red.png");
             img = img.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_38->setPixmap(QPixmap::fromImage(img));
             break;
-        case 3:
+        case 2:
             img.load(":/light_image/diaotou-yellow.png");
             img = img.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_38->setPixmap(QPixmap::fromImage(img));
@@ -2337,12 +2375,12 @@ void ADCIntelligentVehicle::onStateTimer1()
             img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_33->setPixmap(QPixmap::fromImage(img2));
             break;
-        case 2:
+        case 3:
             img2.load(":/light_image/zuoguai-red.png");
             img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_33->setPixmap(QPixmap::fromImage(img2));
             break;
-        case 3:
+        case 2:
             img2.load(":/light_image/zuoguai-yellow.png");
             img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_33->setPixmap(QPixmap::fromImage(img2));
@@ -2368,12 +2406,12 @@ void ADCIntelligentVehicle::onStateTimer1()
             img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_35->setPixmap(QPixmap::fromImage(img3));
             break;
-        case 2:
+        case 3:
             img3.load(":/light_image/zhixing-red.png");
             img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_35->setPixmap(QPixmap::fromImage(img3));
             break;
-        case 3:
+        case 2:
             img3.load(":/light_image/zhixing-yellow.png");
             img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_35->setPixmap(QPixmap::fromImage(img3));
@@ -2400,12 +2438,12 @@ void ADCIntelligentVehicle::onStateTimer1()
             img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_32->setPixmap(QPixmap::fromImage(img4));
             break;
-        case 2:
+        case 3:
             img4.load(":/light_image/youguai-red.png");
             img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_32->setPixmap(QPixmap::fromImage(img4));
             break;
-        case 3:
+        case 2:
             img4.load(":/light_image/youguai-yellow.png");
             img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
             ui->label_32->setPixmap(QPixmap::fromImage(img4));

src/ui/ui_ads_hmi/ui_ads_hmi.pro

@@ -141,3 +141,9 @@ DISTFILES += \
 #    -lrobosense_detection -lrobosense_module_manager -lrobosense_preprocessing -lrobosense_tracking
 
 
+
+#DEFINES += USE_UTM
+
+if(contains(DEFINES,USE_UTM)){
+LIBS += -lGeographic
+}

src/v2x/obuUdpClient/main.cpp

@@ -8,11 +8,11 @@
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
-    UdpReciver mUdpReciver(NULL,"192.168.1.70",5901);
+    UdpReciver mUdpReciver(NULL,"192.168.1.108",1086);
     UdpSender mUdpSender;
     QByteArray byteArray;
     QString strings="hello";
     byteArray = strings.toUtf8();
-    mUdpSender.recMsg(byteArray);
+ //   mUdpSender.recMsg(byteArray);
     return a.exec();
 }

src/v2x/obuUdpClient/obu.json

@@ -1,12 +1,23 @@
+
 #include "udpreciver.h"
 #include "vbox.pb.h"
 
+
+#include "modulecomm.h"
+#include "xmlparam.h"
+
+#include <QJsonArray>
+#include <QJsonObject>
+#include <QJsonDocument>
+
+#include <iostream>
+
 iv::vbox::vbox_light gobjl;
 iv::vbox::vbox_obj_arr gobja;
 
 void * gpLight , * gpObj;
 
-void UdpReciver::ShareResultLights()
+void UdpReciver::ShareResultObs()
 {
     lightLock.lock();
     char * str = new char[gobja.ByteSize()];
@@ -15,13 +26,13 @@ void UdpReciver::ShareResultLights()
     {
         iv::modulecomm::ModuleSendMsg(gpObj,str,nsize);
     }
-    gobjl.Clear();
+    gobja.Clear();
     lightLock.unlock();
     delete[] str;
     str = nullptr;
 }
 
-void UdpReciver::ShareResultObs()
+void UdpReciver::ShareResultLights()
 {
     obsLock.lock();
     char * str = new char[gobjl.ByteSize()];
@@ -30,7 +41,7 @@ void UdpReciver::ShareResultObs()
     {
         iv::modulecomm::ModuleSendMsg(gpLight,str,nsize);
     }
-    gobja.Clear();
+ //   gobja.Clear();
     obsLock.unlock();
 
     delete[] str;
@@ -53,13 +64,12 @@ UdpReciver::UdpReciver(QObject *parent, QString localhost, int port) : QObject(p
     x.set_time_right(0);
     x.set_time_turn(0);
 
-
     gobjl.CopyFrom(x);
 
     iv::vbox::vbox_obj_arr y;
     iv::vbox::vbox_obj *obj;
     obj = y.add_obj();
-    obj->set_allocated_id("");
+    obj->set_id("");
     obj->set_objtype(4);
     obj->set_vehh(0);
     obj->set_vehw(0);
@@ -71,12 +81,12 @@ UdpReciver::UdpReciver(QObject *parent, QString localhost, int port) : QObject(p
     obj->set_heading(0);
     gobja.CopyFrom(y);
 
-    QString strpath = QCoreApplication::applicationDirPath();
-    if(argc < 2)
+    QString strpath =".";// QCoreApplication::applicationDirPath();
+//    if(argc < 2)
         strpath = strpath + "/obuUdpClient.xml";
-    else
-        strpath = argv[1];
-    std::cout<<strpath.toStdString()<<std::endl;
+//    else
+ //       strpath = argv[1];
+  //  std::cout<<strpath.toStdString()<<std::endl;
 
     iv::xmlparam::Xmlparam xp(strpath.toStdString());
 
@@ -89,9 +99,9 @@ UdpReciver::UdpReciver(QObject *parent, QString localhost, int port) : QObject(p
 
     init_port(localhost, port);
     connect(m_udpSocket, SIGNAL(readyRead()), this, SLOT(readDatagrams()), Qt::DirectConnection);
-    this->moveToThread(m_thread);
-    m_udpSocket->moveToThread(m_thread);
-    m_thread->start();
+//    this->moveToThread(m_thread);
+//    m_udpSocket->moveToThread(m_thread);
+//    m_thread->start();
 }
 UdpReciver::~UdpReciver()
 {
@@ -111,6 +121,7 @@ void UdpReciver::readDatagrams()
 {
     QHostAddress client_address; //client ip addr
     m_data.clear();
+    std::cout<<" have data ."<<std::endl;
     while(m_udpSocket->hasPendingDatagrams())
     {
         m_data.resize(m_udpSocket->pendingDatagramSize());
@@ -151,7 +162,7 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
                 return;
         }
 
-        int jsonSize = data[3]<<8 | data[4];
+        int jsonSize = data[4]<<8 | data[3];
 
         if(data[1] == 0x02) //LightsInfo
         {
@@ -163,9 +174,9 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
             memcpy(jsonData.data(), data,jsonSize);
             QJsonObject object = QJsonDocument::fromJson(jsonData).object();
 
-            if(object.contains("LightsStates"))
+            if(object.contains("LightStates"))
             {
-                int lightState = object.value("LightsStates");
+                int lightState = object.value("LightStates").toInt();
             }
 
             if (object.contains("LightsInfo"))
@@ -175,17 +186,17 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
                 qDebug("lightsInfo size %d", size);
                 for(int i = 0; i < size; i++){
                     qDebug() << array.at(0).toString();
-                    QJsonObject lightInfo = array.at(0).toObject();
+                    QJsonObject lightInfo = array.at(i).toObject();
                     int lightFuncValue = 0;
                     if(lightInfo.contains("function"))
                     {
-                        lightFuncValue = lightInfo.value("function");
+                        lightFuncValue = lightInfo.value("function").toInt();
                         qDebug("light fuction value: %d", lightFuncValue);
                     }
 
                     if(lightInfo.contains("LightsColor"))
                     {
-                        int lightColorValue = lightInfo.value("LightsColor");
+                        int lightColorValue = lightInfo.value("LightsColor").toInt();
                         switch (lightFuncValue)
                         {
                         case 1:
@@ -208,7 +219,7 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
 
                     if(lightInfo.contains("RemainingTime"))
                     {
-                        int lightRemTimeValue = lightInfo.value("RemainingTime");
+                        int lightRemTimeValue = lightInfo.value("RemainingTime").toInt();
                         switch (lightFuncValue)
                         {
                         case 1:
@@ -230,16 +241,18 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
                     }
 
                 }
-                lightLock.lock();
-                gobjl.CopyFrom(objl);
-                lightLock.unlock();
-                ShareResultLights();
+
             }
+            lightLock.lock();
+            gobjl.CopyFrom(objl);
+            lightLock.unlock();
+            if(objl.has_st_straight())
+                ShareResultLights();
 
         }
         else if(data[1] == 0x04)
         {
-            QByteArray jsonData();
+            QByteArray jsonData;//();
             data.remove(0, 5);
             jsonData.resize(jsonSize);
             memcpy(jsonData.data(), data,jsonSize);
@@ -256,67 +269,67 @@ void UdpReciver::ReceiveDecode(QByteArray &data)
                     obj = obja.add_obj();
                     if(obsObj.contains("ID"))
                     {
-                        string objID = obsObj.value("ID");
+                        std::string objID = obsObj.value("ID").toString().toStdString();
                         qDebug("obs ID : %s", objID);
                     }
 
                     if(obsObj.contains("PtcType"))
                     {
-                        int ptcType = obsObj.value("PtcType");
+                        int ptcType = obsObj.value("PtcType").toInt();
                         obj->set_objtype(4);
                         qDebug("obs type value: %d", ptcType);
                     }
 
                     if(obsObj.contains("VehL"))
                     {
-                        double dVehL = obsObj.value("VehL");
+                        double dVehL = obsObj.value("VehL").toDouble();
                         obj->set_vehl(0);
                         qDebug("obs VehL :%f", dVehL);
                     }
                     if(obsObj.contains("VehW"))
                     {
-                        double dVehW = obsObj.value("VehW");
+                        double dVehW = obsObj.value("VehW").toDouble();
                         obj->set_vehw(0);
                         qDebug("obs VehW :%f", dVehW);
                     }
                     if(obsObj.contains("VehH"))
                     {
-                        double dVehH = obsObj.value("VehH");
+                        double dVehH = obsObj.value("VehH").toDouble();
                         obj->set_vehh(0);
                         qDebug("obs VehH :%f", dVehH);
                     }
 
                     if(obsObj.contains("PtcLon"))
                     {
-                        double dPtcLon = obsObj.value("PtcLon");
+                        double dPtcLon = obsObj.value("PtcLon").toDouble();
                         obj->set_lon(0);
                         qDebug("obs PtcLon :%f", dPtcLon);
                     }
 
                     if(obsObj.contains("PtcLat"))
                     {
-                        double dPtcLat = obsObj.value("PtcLat");
+                        double dPtcLat = obsObj.value("PtcLat").toDouble();
                         obj->set_lat(0);
                         qDebug("obs Ptc :%f", dPtcLat);
                     }
 
                     if(obsObj.contains("PtcEle"))
                     {
-                        double dPtcEle = obsObj.value("PtcEle");
+                        double dPtcEle = obsObj.value("PtcEle").toDouble();
                         obj->set_ele(0);
                         qDebug("obs PtcEle :%f", dPtcEle);
                     }
 
                     if(obsObj.contains("PtcSpeed"))
                     {
-                        double dPtcSpeed = obsObj.value("PtcSpeed");
+                        double dPtcSpeed = obsObj.value("PtcSpeed").toDouble();
                         obj->set_speed(0);
                         qDebug("obs PtcSpeed :%f", dPtcSpeed);
                     }
 
                     if(obsObj.contains("PtcHeading"))
                     {
-                        double dPtcHeading = obsObj.value("PtcHeading");
+                        double dPtcHeading = obsObj.value("PtcHeading").toDouble();
                         obj->set_heading(0);
                         qDebug("obs PtcHeading :%f", dPtcHeading);
                     }

src/v2x/obuUdpClient/udpsender.cpp

@@ -3,7 +3,7 @@
 UdpSender::UdpSender()
 {
     m_Socket = new QUdpSocket();
-    initSender("192.168.1.70", 5901);
+    initSender("192.168.1.70", 10086);
 }
 UdpSender::~UdpSender()
 {