change map_lanetoxodr. add laneoffset support.

src/tool/map_lanetoxodr/OpenDrive/Lane.cpp

@@ -1506,4 +1506,63 @@ void LaneHeight::SetS(double value)
 void LaneHeight::SetInner(double value)
 {	mInner=value;	}
 void LaneHeight::SetOuter(double value)
-{	mOuter=value;	}
+{	mOuter=value;	}
+
+/**
+* Lane offset class. Contains all the data that describes lane offset record
+*
+*
+*
+*
+*
+*/
+
+/*
+* Constructors
+*/
+LaneOffset::LaneOffset()
+{    ms = 0; ma=0; mb=0; mc=0; md=0;}
+
+LaneOffset::LaneOffset(double sOffset, double a, double b, double c, double d)
+{   ms = sOffset; ma = a; mb = b; mc = c; md = d;}
+
+/*
+* Methods that return the parameters of the lane height
+*/
+double LaneOffset::GetS()
+{   return ms;}
+double LaneOffset::Geta()
+{   return ma;}
+double LaneOffset::Getb()
+{   return mb;}
+double LaneOffset::Getc()
+{   return mc;}
+double LaneOffset::Getd()
+{   return md;}
+
+/**
+* Check if the tested s-offset is inside the lane offset interval
+* @param A double s-offset value that has to be checked
+* @return Return true if the s-offset value belongs to current lane section, false otherwise
+*/
+bool LaneOffset::CheckInterval(double s_check)
+{
+    if (s_check>=ms)
+        return true;
+    else
+        return false;
+}
+
+/*
+* Methods that set the parameters of the lane offset
+*/
+void LaneOffset::SetS(double value)
+{   ms = value;}
+void LaneOffset::Seta(double value)
+{   ma = value;}
+void LaneOffset::Setb(double value)
+{   mb = value;}
+void LaneOffset::Setc(double value)
+{   mc = value;}
+void LaneOffset::Setd(double value)
+{   md = value;}

src/tool/map_lanetoxodr/OpenDrive/Lane.h

@@ -770,4 +770,44 @@ public:
 //----------------------------------------------------------------------------------
 
 
+class LaneOffset
+{
+private:
+    /*
+    * Parameters that describe the lane offeset
+    */
+    double ms;
+    double ma;
+    double mb;
+    double mc;
+    double md;
+public:
+    /*
+    * Constructors
+    */
+    LaneOffset();
+    LaneOffset (double sOffset, double a, double b,double c,double d);
+
+
+    /*
+    * Methods that return the parameters of the lane offset
+    */
+    double GetS();
+    double Geta();
+    double Getb();
+    double Getc();
+    double Getd();
+
+    bool CheckInterval(double s_check);
+
+    /*
+    * Methods that set the parameters of the lane offset
+    */
+    void SetS(double value);
+    void Seta(double value);
+    void Setb(double value);
+    void Setc(double value);
+    void Setd(double value);
+};
+
 #endif

src/tool/map_lanetoxodr/OpenDrive/OpenDriveXmlParser.cpp

@@ -533,10 +533,40 @@ bool OpenDriveXmlParser::ReadLanes (Road* road, TiXmlElement *node)
 		subNode=subNode->NextSiblingElement("laneSection");
 	}
 
+    subNode = node->FirstChildElement("laneOffset");
+    while (subNode)
+    {
+        ReadLaneOffsets(road, subNode);
+        subNode=subNode->NextSiblingElement("laneOffset");
+    }
+
 	return true;
 }
 //--------------
 
+bool OpenDriveXmlParser::ReadLaneOffsets(Road *road, TiXmlElement *node)
+{
+    int checker=TIXML_SUCCESS;
+    double s,a,b,c,d;
+    checker+=node->QueryDoubleAttribute("s",&s);
+    checker+=node->QueryDoubleAttribute("a",&a);
+    checker+=node->QueryDoubleAttribute("b",&b);
+    checker+=node->QueryDoubleAttribute("c",&c);
+    checker+=node->QueryDoubleAttribute("d",&d);
+
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Lane Offset attributes"<<endl;
+        return false;
+    }
+
+    road->AddLaneOffset(s,a,b,c,d);
+
+   return true;
+
+
+}
+
 bool OpenDriveXmlParser::ReadLaneSections (Road* road, TiXmlElement *node)
 {
 
@@ -766,15 +796,25 @@ bool OpenDriveXmlParser::ReadLaneRoadMark(Lane* lane, TiXmlElement *node)
 	double sOffset;
 	string type;
 	string weight;
+    string weightdef = "standard";
 	string color; 
+    string colordef = "white";
 	double width;
 	string laneChange;
 
 	int checker=TIXML_SUCCESS;
 	checker+=node->QueryDoubleAttribute("sOffset",&sOffset);
 	checker+=node->QueryStringAttribute("type",&type);
-	checker+=node->QueryStringAttribute("weight",&weight);
-	checker+=node->QueryStringAttribute("color",&color);
+    if(node->QueryStringAttribute("weight",&weight) != TIXML_SUCCESS)
+    {
+        weight = weightdef;
+    }
+//	checker+=node->QueryStringAttribute("weight",&weight);
+    if(node->QueryStringAttribute("color",&color) != TIXML_SUCCESS)
+    {
+        color = colordef;
+    }
+
 	
 	if (checker!=TIXML_SUCCESS)
 	{

src/tool/map_lanetoxodr/OpenDrive/OpenDriveXmlParser.h

@@ -60,6 +60,7 @@ public:
 	bool ReadLaneSpeed(Lane* lane, TiXmlElement *node);
 	bool ReadLaneAccess(Lane* lane, TiXmlElement *node);
 	bool ReadLaneHeight(Lane* lane, TiXmlElement *node);
+    bool ReadLaneOffsets(Road * road,TiXmlElement * node);
 	//--------------
 
 	bool ReadObjects (Road* road, TiXmlElement *node);

src/tool/map_lanetoxodr/OpenDrive/Road.cpp

@@ -61,6 +61,7 @@ Road::Road (const Road& road)
 	mLaneSectionsVector=road.mLaneSectionsVector;
 	mObjectsVector=road.mObjectsVector;
 	mSignalsVector=road.mSignalsVector;
+    mLaneOffsetVector=road.mLaneOffsetVector;
 }
 
 /**
@@ -102,6 +103,7 @@ const Road& Road::operator=(const Road& otherRoad)
 		mLaneSectionsVector=otherRoad.mLaneSectionsVector;
 		mObjectsVector=otherRoad.mObjectsVector;
 		mSignalsVector=otherRoad.mSignalsVector;
+        mLaneOffsetVector=otherRoad.mLaneOffsetVector;
 	}
 	return *this;
 }
@@ -278,6 +280,23 @@ unsigned int Road::GetLaneSectionCount()
 {
 	return mLaneSectionsVector.size();
 }
+
+// Road lane offset records
+vector<LaneOffset> * Road::GetLaneOffsetVector()
+{
+    return &mLaneOffsetVector;
+}
+LaneOffset * Road::GetLaneOffset(unsigned int i)
+{
+    if ((mLaneOffsetVector.size()>0)&&(i<mLaneOffsetVector.size()))
+        return &mLaneOffsetVector.at(i);
+    else
+        return NULL;
+}
+unsigned int Road::GetLaneOffsetCount()
+{
+    return mLaneOffsetVector.size();
+}
 // Road object records
 vector<Object> *Road::GetObjectVector()
 {
@@ -599,6 +618,16 @@ unsigned int Road::AddLaneSection(double s)
 	mLastAddedLaneSection=index;
 	return index;
 }
+
+unsigned int Road::AddLaneOffset(double s, double a, double b, double c, double d)
+{
+    unsigned int index = CheckLaneOffsetInterval(s)+1;
+    if(index>=GetLaneOffsetCount()) mLaneOffsetVector.push_back(LaneOffset(s,a,b,c,d));
+    else mLaneOffsetVector.insert(mLaneOffsetVector.begin()+index, LaneOffset(s,a,b,c,d));
+    mLastAddedLaneOffset=index;
+    return index;
+}
+
 //-------------
 unsigned int Road::AddObject()
 {	
@@ -827,6 +856,10 @@ void Road::DeleteLaneSection(unsigned int index)
 {
 	mLaneSectionsVector.erase(mLaneSectionsVector.begin()+index);
 }
+void Road::DeleteLaneOffset(unsigned int index)
+{
+    mLaneOffsetVector.erase(mLaneOffsetVector.begin()+index);
+}
 void Road::DeleteObject(unsigned int index)
 {
 	mObjectsVector.erase(mObjectsVector.begin()+index);
@@ -1027,6 +1060,22 @@ int  Road::CheckLaneSectionInterval(double s_check)
 	return res;		//return the result: 0 to MaxInt as the index to the record containing s_check or -1 if nothing found
 }
 //-----------
+int Road::CheckLaneOffsetInterval(double s_check)
+{
+    int res=-1;
+    //Go through all the lane section records
+    for (unsigned int i=0;i<mLaneOffsetVector.size();i++)
+    {
+        //check if the s_check belongs to the current record
+        if (mLaneOffsetVector.at(i).CheckInterval(s_check))
+            res=i;	//assign it to the result id
+        else
+            break;	//if not, break;
+    }
+    return res;		//return the result: 0 to MaxInt as the index to the record containing s_check or -1 if nothing found
+
+}
+//-----------
 void  Road::FillLaneSectionSample(double s_check, LaneSectionSample& laneSectionSample)
 {
 	int index=CheckLaneSectionInterval(s_check);

src/tool/map_lanetoxodr/OpenDrive/Road.h

@@ -23,6 +23,7 @@ class Crossfall;
 //lanes
 class LaneSection;
 class LaneSectionSample;
+class LaneOffset;
 //objects, signals
 class Object;
 class Signal;
@@ -69,6 +70,8 @@ private:
 	vector<Crossfall> mCrossfallVector;
 	// Lane Section vector
 	vector<LaneSection> mLaneSectionsVector;
+    // Lane offset vector
+    vector<LaneOffset> mLaneOffsetVector;
 	// Objects vectors
 	vector<Object> mObjectsVector;
 	// Signal vector
@@ -83,6 +86,7 @@ private:
 	unsigned int mLastAddedSuperElevation;
 	unsigned int mLastAddedCrossfall;
 	unsigned int mLastAddedLaneSection;
+    unsigned int mLastAddedLaneOffset;
 	unsigned int mLastAddedObject;
 	unsigned int mLastAddedSignal;
 
@@ -156,6 +160,10 @@ public:
 	vector<LaneSection> *GetLaneSectionVector();
 	LaneSection*	GetLaneSection(unsigned int i);
 	unsigned int GetLaneSectionCount();
+    //Road lane offset records
+    vector<LaneOffset> * GetLaneOffsetVector();
+    LaneOffset* GetLaneOffset(unsigned int i);
+    unsigned int GetLaneOffsetCount();
 	// Road object records
 	vector<Object> *GetObjectVector();
 	Object*	GetObject(unsigned int i);
@@ -228,6 +236,7 @@ public:
 	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 AddObject();
     unsigned int AddSignal(double s,double t,string id,string name,bool dynamic,string orientation,double zOffset,string type,string country,string countryRevision,
                            string subtype,double hOffset,double pitch,double roll ,double height,double width);
@@ -253,6 +262,7 @@ public:
 	void DeleteSuperElevation(unsigned int index);
 	void DeleteCrossfall(unsigned int index);
 	void DeleteLaneSection(unsigned int index);
+    void DeleteLaneOffset(unsigned int index);
 	void DeleteObject(unsigned int index);
 	void DeleteSignal(unsigned int index);
 	
@@ -292,6 +302,8 @@ public:
 
 	int CheckLaneSectionInterval(double s_check);
 	void FillLaneSectionSample(double s_check, LaneSectionSample &laneSectionSample);
+
+    int CheckLaneOffsetInterval(double s_check);
 	
 	//-------------------------------------------------
 

src/tool/map_lanetoxodr/xodrfunc.cpp

@@ -769,6 +769,7 @@ std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad,  const double
 //        {
 //            int a= 1;
 //        }
+
         LaneSection * pLS = pRoad->GetLaneSection(i);
         if(i<(nLSCount -1))
         {
@@ -777,6 +778,24 @@ std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad,  const double
                 continue;
             }
         }
+        LaneOffset * pLO = pRoad->GetLaneOffset(0);
+        int nLOCount = pRoad->GetLaneOffsetCount();
+        if(nLOCount > 0)
+        {
+            int nlonow = 0;
+            while(nlonow < (nLOCount-1))
+            {
+                if(pRoad->GetLaneOffset(nlonow+1)->GetS()<s)
+                {
+                    nlonow++;
+                    pLO=pRoad->GetLaneOffset(nlonow);
+                }
+                else
+                {
+                    break;
+                }
+            }
+        }
         s_section = pLS->GetS();
         int nlanecount = pLS->GetLaneCount();
         int j;
@@ -963,6 +982,15 @@ std::vector<iv::LanePoint> xodrfunc::GetAllLanePoint(Road *pRoad,  const double
                 xvectorlanepoint[j].mfX = x + xvectorlanepoint[j].mflanetocenter * cos(fhdg+M_PI/2.0);
                 xvectorlanepoint[j].mfY = y + xvectorlanepoint[j].mflanetocenter * sin(fhdg+M_PI/2.0);
             }
+
+            if(pLO != NULL)
+            {
+                double s_tem = s - pLO->GetS();
+                double foff = pLO->Geta() + pLO->Getb()*(s_tem)
+                        +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
+                xvectorlanepoint[j].mfX = xvectorlanepoint[j].mfX + foff * cos(fhdg+M_PI/2.0);
+                xvectorlanepoint[j].mfY = xvectorlanepoint[j].mfY + foff * sin(fhdg+M_PI/2.0);
+            }
         }
 
         break;

src/tool/map_lanetoxodr/xvmainwindow.cpp

@@ -86,6 +86,7 @@ void XVMainWindow::on_actionLoad_triggered()
     QString strpath = QFileDialog::getOpenFileName(this,"Load XODR",".","*.xodr");
     if(strpath.isEmpty())return;
     LoadXODR(strpath);
+    OpenDrive * pxodr = &mxodr;
     UpdateScene();
 #else
 //    QMessageBox::warning(this,"warning","no file dialog.",QMessageBox::YesAll);
@@ -180,7 +181,7 @@ void XVMainWindow::UpdateScene()
     std::vector<RoadDigit> xvectorrd;
     for(i=0;i<nsize;i++)
     {
-        RoadDigit xrd(mxodr.GetRoad(i),10.0);
+        RoadDigit xrd(mxodr.GetRoad(i),5.0);  //Space Must <= Broken dot dis
         xvectorrd.push_back(xrd);
     }
     for(i=0;i<nsize;i++)
@@ -364,6 +365,7 @@ void XVMainWindow::onFileOpen()
     QString strpath = mFileDialog.currentFile();
     if(strpath.isEmpty())return;
     LoadXODR(strpath);
+    OpenDrive * pxodr = &mxodr;
     UpdateScene();
 }