change map_lanetoxodr. completing 1.6 standard.

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

@@ -2577,8 +2577,72 @@ bool OpenDriveXmlParser::ReadSignal_positionInertial(Signal *pSignal, TiXmlEleme
 
 bool OpenDriveXmlParser::ReadSurface (Road* road, TiXmlElement *node)
 {
+    TiXmlElement *subNode = node->FirstChildElement("CRG");
+    while (subNode)
+    {
+        ReadSurfaceCRG(road, subNode);
+        subNode=subNode->NextSiblingElement("CRG");
+    }
 	return true;
 }
+
+//--------------
+
+bool OpenDriveXmlParser::ReadSurfaceCRG(Road* road,TiXmlElement *node)
+{
+    std::string file,orientation,mode;
+    double sStart,sEnd;
+
+    int checker=TIXML_SUCCESS;
+
+    checker+=node->QueryStringAttribute("file",&file);
+    checker+=node->QueryDoubleAttribute("file",&sStart);
+    checker+=node->QueryDoubleAttribute("file",&sEnd);
+    checker+=node->QueryStringAttribute("orientation",&orientation);
+    checker+=node->QueryStringAttribute("mode",&mode);
+
+    if (checker!=TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Surface CRG attributes"<<endl;
+        return false;
+    }
+
+    road->AddSurfaceCRG(file,sStart,sEnd,orientation,mode);
+
+    surface_CRG * pCRG = road->GetLastAddedSurfaceCRG();
+
+    std::string purpose;
+    if(node->QueryStringAttribute("purpose",&purpose) == TIXML_SUCCESS)
+    {
+        pCRG->Setpurpose(purpose);
+    }
+
+    double sOffset,tOffset,zOffset,zScale,hOffset;
+    if(node->QueryDoubleAttribute("sOffset",&sOffset))
+    {
+        pCRG->SetsOffset(sOffset);
+    }
+    if(node->QueryDoubleAttribute("tOffset",&tOffset))
+    {
+        pCRG->SetsOffset(tOffset);
+    }
+    if(node->QueryDoubleAttribute("zOffset",&zOffset))
+    {
+        pCRG->SetsOffset(zOffset);
+    }
+    if(node->QueryDoubleAttribute("zScale",&zScale))
+    {
+        pCRG->SetsOffset(zScale);
+    }
+    if(node->QueryDoubleAttribute("hOffset",&hOffset))
+    {
+        pCRG->SetsOffset(hOffset);
+    }
+
+    return true;
+
+}
+
 //--------------
 
 bool OpenDriveXmlParser::ReadController (TiXmlElement *node)

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

@@ -112,6 +112,7 @@ public:
 	//--------------
 
 	bool ReadSurface (Road* road, TiXmlElement *node);
+    bool ReadSurfaceCRG(Road* road,TiXmlElement *node);
 	//--------------
 
 	bool ReadController (TiXmlElement *node);

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

@@ -189,14 +189,18 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
         WriteRoadNoavoids(nodeRoad,road);
     }
 
-	/*
+    if(road->GetSurfaceCRGCount()>0)
+    {
+        WriteSurface(nodeRoad,road);
+    }
+
 	//Proceed to Surface
-	subNode=node->FirstChildElement("surface");
-	if (subNode)
-	{
-	WriteSurface(road, subNode);
-	}
-	*/
+//	subNode=node->FirstChildElement("surface");
+//	if (subNode)
+//	{
+//	WriteSurface(road, subNode);
+//	}
+
 
 	return true;
 }
@@ -2130,10 +2134,64 @@ bool OpenDriveXmlWriter::WriteSignal_referency(TiXmlElement * node, signal_refer
 
 //--------------
 
-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::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::WriteController (TiXmlElement *node,Controller* controller)

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

@@ -105,7 +105,8 @@ public:
 
     //--------------
 
-	bool WriteSurface (TiXmlElement *node, Road* road);
+    bool WriteSurface (TiXmlElement *node, Road * road);
+    bool WriteSurfaceCRG(TiXmlElement * node, surface_CRG * psurfaceCRG);
 	//--------------
 
     bool WriteController (TiXmlElement *node,Controller* controller);

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

@@ -64,6 +64,7 @@ Road::Road (const Road& road)
     mLaneOffsetVector=road.mLaneOffsetVector;
     mRoadBorrowVector = road.mRoadBorrowVector;
     mRoadNoavoidVector = road.mRoadNoavoidVector;
+    mSurfaceCRGVector = road.mSurfaceCRGVector;
 
 }
 
@@ -109,6 +110,7 @@ const Road& Road::operator=(const Road& otherRoad)
         mLaneOffsetVector=otherRoad.mLaneOffsetVector;
         mRoadBorrowVector = otherRoad.mRoadBorrowVector;
         mRoadNoavoidVector = otherRoad.mRoadNoavoidVector;
+        mSurfaceCRGVector = otherRoad.mSurfaceCRGVector;
 	}
 	return *this;
 }
@@ -455,22 +457,22 @@ unsigned int Road::GetRoadNoavoidCount()
     return mRoadNoavoidVector.size();
 }
 //-------------------------------------------------
-vector<surface> *Road::GetSurfaceVector()
+vector<surface_CRG> *Road::GetSurfaceCRGVector()
 {
-    return &mSurfaceVector;
+    return &mSurfaceCRGVector;
 }
 
-surface*	Road::GetSurface(unsigned int i)
+surface_CRG*	Road::GetSurfaceCRG(unsigned int i)
 {
-    if((mSurfaceVector.size()>0)&&(i<mSurfaceVector.size()))
-        return &mSurfaceVector.at(i);
+    if((mSurfaceCRGVector.size()>0)&&(i<mSurfaceCRGVector.size()))
+        return &mSurfaceCRGVector.at(i);
     else
         return NULL;
 }
 
-unsigned int Road::GetSurfaceCount()
+unsigned int Road::GetSurfaceCRGCount()
 {
-    return static_cast<unsigned int>(mSurfaceVector.size());
+    return static_cast<unsigned int>(mSurfaceCRGVector.size());
 }
 //-------------------------------------------------
 
@@ -581,10 +583,10 @@ RoadNoavoid* Road::GetLastRoadNoavoid()
         return NULL;
 }
 
-surface *       Road::GetLastSurface()
+surface_CRG *       Road::GetLastSurfaceCRG()
 {
-    if(mSurfaceVector.size()>0)
-        return &mSurfaceVector.at(mSurfaceVector.size()-1);
+    if(mSurfaceCRGVector.size()>0)
+        return &mSurfaceCRGVector.at(mSurfaceCRGVector.size()-1);
     else
         return NULL;
 }
@@ -695,10 +697,10 @@ RoadNoavoid* Road::GetLastAddedRoadNoavoid()
     else
         return NULL;
 }
-surface*        Road::GetLastAddedSurface()
+surface_CRG*        Road::GetLastAddedSurfaceCRG()
 {
-    if(mLastAddedSurface<mSurfaceVector.size())
-        return &mSurfaceVector.at(mLastAddedSurface);
+    if(mLastAddedSurfaceCRG<mSurfaceCRGVector.size())
+        return &mSurfaceCRGVector.at(mLastAddedSurfaceCRG);
     else
         return NULL;
 }
@@ -901,11 +903,14 @@ unsigned int Road::AddRoadNoavoid(double s, double length)
     return index;
 }
 
-unsigned int Road::AddSurface()
+unsigned int Road::AddSurfaceCRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode)
 {
-    mSurfaceVector.push_back(surface());
-    mLastAddedSurface = static_cast<unsigned int >( mSurfaceVector.size()-1);
-    return mLastAddedSurface;
+    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;
+
 }
 
 //-------------
@@ -1201,14 +1206,14 @@ unsigned int Road::CloneRoadNoavoid(unsigned int index)
     return mLastAddedRoadNoavoid;
 }
 
-unsigned int Road::CloneSurface(unsigned int index)
+unsigned int Road::CloneSurfaceCRG(unsigned int index)
 {
-    if(index<mSurfaceVector.size()-1)
-        mSurfaceVector.insert(mSurfaceVector.begin()+index+1, mSurfaceVector[index]);
-    else if(index==mSurfaceVector.size()-1)
-        mSurfaceVector.push_back(mSurfaceVector[index]);
-    mLastAddedSurface=index+1;
-    return mLastAddedSurface;
+    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;
 }
 
 
@@ -1280,9 +1285,9 @@ void Road::DeleteRoadNoavoid(unsigned int index)
     mRoadNoavoidVector.erase(mRoadNoavoidVector.begin()+index);
 }
 
-void Road::DeleteSurface(unsigned int index)
+void Road::DeleteSurfaceCRG(unsigned int index)
 {
-    mSurfaceVector.erase(mSurfaceVector.begin()+index);
+    mSurfaceCRGVector.erase(mSurfaceCRGVector.begin()+index);
 }
 
 //-------------------------------------------------
@@ -1734,6 +1739,21 @@ int Road::CheckLaneOffsetInterval(double s_check)
 
 }
 //-----------
+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);

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

@@ -23,6 +23,7 @@ class Elevation;
 class SuperElevation;
 class Crossfall;
 class surface;
+class surface_CRG;
 //lanes
 class LaneSection;
 class LaneSectionSample;
@@ -97,7 +98,7 @@ private:
 
     vector<RoadNoavoid> mRoadNoavoidVector;
 
-    vector<surface> mSurfaceVector;
+    vector<surface_CRG> mSurfaceCRGVector;
 
 	/**
 	 * Indices of the last added child records
@@ -117,7 +118,7 @@ private:
     unsigned int mLastAddedSignalReference;
     unsigned int mLastAddedRoadBorrow;
     unsigned int mLastAddedRoadNoavoid;
-    unsigned int mLastAddedSurface;
+    unsigned int mLastAddedSurfaceCRG;
 
 public:
 	/**
@@ -228,9 +229,9 @@ public:
     RoadNoavoid*	GetRoadNoavoid(unsigned int i);
     unsigned int GetRoadNoavoidCount();
 
-    vector<surface> *GetSurfaceVector();
-    surface*	GetSurface(unsigned int i);
-    unsigned int GetSurfaceCount();
+    vector<surface_CRG> *GetSurfaceCRGVector();
+    surface_CRG*	GetSurfaceCRG(unsigned int i);
+    unsigned int GetSurfaceCRGCount();
 
 
     vector<string> * GetUserData();
@@ -253,7 +254,7 @@ public:
     signals_signalReference* GetLastSignalReference();
     RoadBorrow *    GetLastRoadBorrow();
     RoadNoavoid *   GetLastRoadNoavoid();
-    surface *       GetLastSurface();
+    surface_CRG *       GetLastSurfaceCRG();
 
 	/**
 	 * Getters for the last added child records in their respective vectors
@@ -272,7 +273,7 @@ public:
     signals_signalReference* GetLastAddedSignalReference();
     RoadBorrow*     GetLastAddedRoadBorrow();
     RoadNoavoid*    GetLastAddedRoadNoavoid();
-    surface*        GetLastAddedSurface();
+    surface_CRG*        GetLastAddedSurfaceCRG();
 
 	//-------------------------------------------------
 
@@ -324,7 +325,7 @@ public:
 
     unsigned int AddRoadBorrow(double s,double length,string mode);
     unsigned int AddRoadNoavoid(double s,double length);
-    unsigned int AddSurface();
+    unsigned int AddSurfaceCRG(std::string file,double sStart,double sEnd,std::string orientation,std::string mode);
 	/**
 	 * Methods used to clone child records in the respective vectors
 	 */
@@ -342,7 +343,7 @@ public:
     unsigned int CloneSignalReference(unsigned int index);
     unsigned int CloneRoadBorrow(unsigned int index);
     unsigned int CloneRoadNoavoid(unsigned int index);
-    unsigned int CloneSurface(unsigned int index);
+    unsigned int CloneSurfaceCRG(unsigned int index);
 
 	/**
 	 * Methods used to delete child records from the respective vectors
@@ -362,7 +363,7 @@ public:
     void DeleteSignalReference(unsigned int index);
     void DeleteRoadBorrow(unsigned int index);
     void DeleteRoadNoavoid(unsigned int index);
-    void DeleteSurface(unsigned int index);
+    void DeleteSurfaceCRG(unsigned int index);
 	
 	//-------------------------------------------------
 
@@ -424,6 +425,8 @@ public:
     int CheckSignalReferencInterval(double s_check);
 
     int CheckSignalInterval(double s_check);
+
+    int CheckSurfaceCRGInterval(double s_check);
 	
 	//-------------------------------------------------