change map_lanetoxodr. completing opendrive 1.6

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

@@ -199,6 +199,14 @@ bool OpenDriveXmlParser::ReadRoad(TiXmlElement *node)
 		ReadSurface(road, subNode);
 	}
 
+
+    //Proceed to Railroad
+    subNode=node->FirstChildElement("railroad");
+    if (subNode)
+    {
+        ReadRailroad(road, subNode);
+    }
+
     //Proceed to userData
     subNode=node->FirstChildElement("userData");
     while (subNode)
@@ -2645,6 +2653,126 @@ bool OpenDriveXmlParser::ReadSurfaceCRG(Road* road,TiXmlElement *node)
 
 //--------------
 
+bool OpenDriveXmlParser::ReadRailroad(Road *road, TiXmlElement *node)
+{
+    TiXmlElement *subNode = node->FirstChildElement("switch");
+    while (subNode)
+    {
+        ReadRailroadSwitch(road,subNode);
+        subNode=subNode->NextSiblingElement("switch");
+    }
+    return true;
+}
+
+//--------------
+
+bool OpenDriveXmlParser::ReadRailroadSwitch(Road* road, TiXmlElement *node)
+{
+
+    string name;
+    string id;
+    string position;
+
+    int checker=TIXML_SUCCESS;
+
+    checker+=node->QueryStringAttribute("name",&name);
+    checker+=node->QueryStringAttribute("id",&id);
+    checker+=node->QueryStringAttribute("position",&position);
+
+    if(checker != TIXML_SUCCESS)
+    {
+        cout<<"Error parsing Railroad Switch attributes"<<endl;
+        return false;
+    }
+
+    Railroad_Switch_MainTrack mainTrack;
+    Railroad_Switch_SideTrack sideTrack;
+    Railroad_Switch_Partner partner;
+
+    TiXmlElement *subNode = node->FirstChildElement("mainTrack");
+    if (subNode)
+    {
+        std::string id,dir;
+        double s;
+
+        checker=TIXML_SUCCESS;
+        checker+=subNode->QueryStringAttribute("id",&id);
+        checker+=subNode->QueryDoubleAttribute("s",&s);
+        checker+=subNode->QueryStringAttribute("dir",&dir);
+
+        if(checker != TIXML_SUCCESS)
+        {
+            cout<<"Error parsing Railroad Switch  mainTrack attributes"<<endl;
+            return false;
+        }
+
+        mainTrack.Setid(id);
+        mainTrack.Sets(s);
+        mainTrack.Setdir(dir);
+
+    }
+    else
+    {
+        cout<<"Error parsing Railroad Switch attributes. no mainTrack."<<endl;
+        return false;
+    }
+
+    subNode = node->FirstChildElement("sideTrack");
+    if (subNode)
+    {
+        std::string id,dir;
+        double s;
+
+        checker=TIXML_SUCCESS;
+        checker+=subNode->QueryStringAttribute("id",&id);
+        checker+=subNode->QueryDoubleAttribute("s",&s);
+        checker+=subNode->QueryStringAttribute("dir",&dir);
+
+        if(checker != TIXML_SUCCESS)
+        {
+            cout<<"Error parsing Railroad Switch  sideTrack attributes"<<endl;
+            return false;
+        }
+
+        sideTrack.Setid(id);
+        sideTrack.Sets(s);
+        sideTrack.Setdir(dir);
+
+    }
+    else
+    {
+        cout<<"Error parsing Railroad Switch attributes. no sideTrack."<<endl;
+        return false;
+    }
+
+    road->AddRailroadSwitch(name,id,position,mainTrack,sideTrack);
+
+    Railroad_Switch * pRailroad_Switch = road->GetLastAddedRailroadSwitch();
+
+    subNode = node->FirstChildElement("partner");
+    if (subNode)
+    {
+        std::string name,id;
+
+        checker=TIXML_SUCCESS;
+        checker+=subNode->QueryStringAttribute("id",&id);
+
+        if(checker == TIXML_SUCCESS)
+        {
+            partner.Setid(id);
+            if(subNode->QueryStringAttribute("name",&name) == TIXML_SUCCESS)
+            {
+                partner.Setname(name);
+            }
+            pRailroad_Switch->SetPartner(partner);
+        }
+    }
+
+    return true;
+}
+
+//--------------
+
 bool OpenDriveXmlParser::ReadController (TiXmlElement *node)
 {
     string name;

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

@@ -115,6 +115,11 @@ public:
     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);
 	//--------------

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

@@ -194,6 +194,11 @@ bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
         WriteSurface(nodeRoad,road);
     }
 
+    if(road->GetRailroadSwitchCount())
+    {
+        WriteRailroad(node,road);
+    }
+
 	//Proceed to Surface
 //	subNode=node->FirstChildElement("surface");
 //	if (subNode)
@@ -2194,6 +2199,91 @@ bool OpenDriveXmlWriter::WriteSurfaceCRG(TiXmlElement * node, surface_CRG * psur
 }
 //--------------
 
+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

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

@@ -109,6 +109,14 @@ public:
     bool WriteSurfaceCRG(TiXmlElement * node, surface_CRG * psurfaceCRG);
 	//--------------
 
+    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);
 	//--------------

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

@@ -474,6 +474,27 @@ 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());
+}
+
 //-------------------------------------------------
 
 /**
@@ -591,6 +612,14 @@ surface_CRG *       Road::GetLastSurfaceCRG()
         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
@@ -704,6 +733,14 @@ surface_CRG*        Road::GetLastAddedSurfaceCRG()
     else
         return NULL;
 }
+
+Railroad_Switch * Road::GetLastAddedRailroadSwitch()
+{
+    if(mLastAddedRailroadSwitch<mRailroadSwitchVector.size())
+        return &mRailroadSwitchVector.at(mLastAddedRailroadSwitch);
+    else
+        return NULL;
+}
 //-------------------------------------------------
 
 /**
@@ -913,6 +950,12 @@ unsigned int Road::AddSurfaceCRG(std::string file,double sStart,double sEnd,std:
 
 }
 
+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)
 {
@@ -1216,6 +1259,16 @@ unsigned int Road::CloneSurfaceCRG(unsigned int index)
     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
@@ -1290,6 +1343,11 @@ void Road::DeleteSurfaceCRG(unsigned int index)
     mSurfaceCRGVector.erase(mSurfaceCRGVector.begin()+index);
 }
 
+void Road::DeleteRailroadSwitch(unsigned int index)
+{
+    mRailroadSwitchVector.erase(mRailroadSwitchVector.begin()+index);
+}
+
 //-------------------------------------------------
 // EVALUATION METHODS
 
@@ -2345,4 +2403,198 @@ int surface::CheckCRGInterval(double s_check)
 }
 
 
+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()
+{
+
+}
+
+std::string  Railroad_Switch_MainTrack::Getid()
+{
+    return mid;
+}
+
+double Railroad_Switch_MainTrack::Gets()
+{
+    return ms;
+}
+
+std::string Railroad_Switch_MainTrack::Getdir()
+{
+    return  mdir;
+}
+
+void Railroad_Switch_MainTrack::Setid(std::string id)
+{
+    mid = id;
+}
+
+void Railroad_Switch_MainTrack::Sets(double s)
+{
+    ms = s;
+}
+
+void Railroad_Switch_MainTrack::Setdir(std::string dir)
+{
+    mdir = dir;
+}
+
+
+
+Railroad_Switch_SideTrack::Railroad_Switch_SideTrack(std::string id, double s, std::string dir)
+{
+    mid = id;
+    ms = s;
+    mdir = dir;
+}
+
+Railroad_Switch_SideTrack::Railroad_Switch_SideTrack()
+{
+
+}
+
+std::string  Railroad_Switch_SideTrack::Getid()
+{
+    return mid;
+}
+
+double Railroad_Switch_SideTrack::Gets()
+{
+    return ms;
+}
+
+std::string Railroad_Switch_SideTrack::Getdir()
+{
+    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;
+}
+
+
+Railroad_Switch_Partner::Railroad_Switch_Partner(std::string id)
+{
+    mid = id;
+}
+
+Railroad_Switch_Partner::Railroad_Switch_Partner()
+{
+
+}
+
+int Railroad_Switch_Partner::Getname(std::string & name)
+{
+    if(mname.size() == 0)return  0;
+    name = mname[0];
+    return 1;
+}
+
+std::string Railroad_Switch_Partner::Getid()
+{
+    return  mid;
+}
+
+void Railroad_Switch_Partner::Setname(std::string name)
+{
+    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();
+}
+
+
+
+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;
+}
+
+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);
+}
+
+int Railroad_Switch::GetPartner(Railroad_Switch_Partner & partner)
+{
+    if(mPartner.size() == 0)return  0;
+    partner = mPartner[0];
+    return 1;
+}
+
+void Railroad_Switch::ResetPartner()
+{
+    if(mPartner.size()>0)mPartner.clear();
+}
+
+Railroad_Switch_MainTrack * Railroad_Switch::GetMainTrack()
+{
+    return &mMainTrack;
+}
+
+Railroad_Switch_SideTrack * Railroad_Switch::GetSideTrack()
+{
+    return &mSideTrack;
+}
+
 

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

@@ -24,6 +24,9 @@ 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;
@@ -100,6 +103,8 @@ private:
 
     vector<surface_CRG> mSurfaceCRGVector;
 
+    vector<Railroad_Switch> mRailroadSwitchVector;
+
 	/**
 	 * Indices of the last added child records
 	 */
@@ -119,6 +124,7 @@ private:
     unsigned int mLastAddedRoadBorrow;
     unsigned int mLastAddedRoadNoavoid;
     unsigned int mLastAddedSurfaceCRG;
+    unsigned int mLastAddedRailroadSwitch;
 
 public:
 	/**
@@ -233,6 +239,10 @@ public:
     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();
 	//-------------------------------------------------
@@ -255,6 +265,7 @@ public:
     RoadBorrow *    GetLastRoadBorrow();
     RoadNoavoid *   GetLastRoadNoavoid();
     surface_CRG *       GetLastSurfaceCRG();
+    Railroad_Switch * GetLastRailroadSwitch();
 
 	/**
 	 * Getters for the last added child records in their respective vectors
@@ -274,6 +285,7 @@ public:
     RoadBorrow*     GetLastAddedRoadBorrow();
     RoadNoavoid*    GetLastAddedRoadNoavoid();
     surface_CRG*        GetLastAddedSurfaceCRG();
+    Railroad_Switch * GetLastAddedRailroadSwitch();
 
 	//-------------------------------------------------
 
@@ -326,6 +338,7 @@ public:
     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
 	 */
@@ -344,6 +357,7 @@ public:
     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
@@ -364,6 +378,7 @@ public:
     void DeleteRoadBorrow(unsigned int index);
     void DeleteRoadNoavoid(unsigned int index);
     void DeleteSurfaceCRG(unsigned int index);
+    void DeleteRailroadSwitch(unsigned int index);
 	
 	//-------------------------------------------------
 
@@ -761,6 +776,100 @@ public:
 
 
 
+};
+
+//----------------------------------------------------------------------------------
+
+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();
+
 };