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

#include "OpenDriveXmlWriter.h"
#include <iostream>
#include <algorithm>
//#include "windows.h"

using std::cout;
using std::endl;

/**
 * Constructor which saves a reference to OpenDrive structure
 */
OpenDriveXmlWriter::OpenDriveXmlWriter (OpenDrive* openDriveObj)
{	
	mOpenDrive=openDriveObj;	
}

/**
 * The following methods are used to create the XML representation of the OpenDrive structure
 * Methods follow the same hierarchical structure and are called automatically when WriteFile
 * is executed
 */
bool OpenDriveXmlWriter::WriteHeader(TiXmlElement *node)
{
	//Write the Header
	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;
    string strvendor;

	Header *lHeader = mOpenDrive->GetHeader();
    lHeader->GetAllParams(revMajor, revMinor, name, version, date, north, south, east, west,lat0,lon0,hdg0);
    lHeader->GetVendor(strvendor);

	TiXmlElement *nodeHeader = new TiXmlElement("header");
	node->LinkEndChild(nodeHeader);

	nodeHeader->SetAttribute("revMajor",revMajor);
	nodeHeader->SetAttribute("revMinor",revMinor);
	nodeHeader->SetAttribute("name",name);
	nodeHeader->SetDoubleAttribute("version",version);
	nodeHeader->SetAttribute("date",date);

	std::stringstream snorth;
	snorth << setprecision(16) << setiosflags (ios_base::scientific) << north;
	nodeHeader->SetAttribute("north",snorth.str());

	std::stringstream ssouth;
	ssouth << setprecision(16) << setiosflags (ios_base::scientific) << south;
	nodeHeader->SetAttribute("south",ssouth.str());

	std::stringstream seast;
	seast << setprecision(16) << setiosflags (ios_base::scientific) << east;
	nodeHeader->SetAttribute("east",seast.str());

	std::stringstream swest;
	swest << setprecision(16) << setiosflags (ios_base::scientific) << west;
	nodeHeader->SetAttribute("west",swest.str());

    std::stringstream slat0;
    slat0 << setprecision(16) << setiosflags (ios_base::scientific) << lat0;
    nodeHeader->SetAttribute("lat0",slat0.str());

    std::stringstream slon0;
    slon0 << setprecision(16) << setiosflags (ios_base::scientific) << lon0;
    nodeHeader->SetAttribute("lon0",slon0.str());

    std::stringstream shdg0;
    shdg0 << setprecision(16) << setiosflags (ios_base::scientific) << hdg0;
    nodeHeader->SetAttribute("hdg0",shdg0.str());

    if(strvendor.size()>0)
        nodeHeader->SetAttribute("vendor",strvendor);
    else
    {
        nodeHeader->SetAttribute("vendor","adc");
    }

    std::string strgeoReference;
    std::string struserData;
    lHeader->GetgeoReference(strgeoReference);
    lHeader->GetuserData(struserData);

    if(strgeoReference != "")
    {
        TiXmlElement * nodegeoReference = new TiXmlElement("geoReference");
        nodegeoReference->Parse(strgeoReference.data(),0,TIXML_ENCODING_UTF8);
        nodeHeader->LinkEndChild(nodegeoReference);

    }

    if(struserData != "")
    {
        TiXmlElement * nodeuserData = new TiXmlElement("userData");
        nodeuserData->Parse(struserData.data(),0,TIXML_ENCODING_UTF8);
        nodeHeader->LinkEndChild(nodeuserData);

    }




	return true;
}
//--------------
bool OpenDriveXmlWriter::WriteRoad(TiXmlElement *node, Road *road)
{
	//Write road attributes
	string name;
	double length;
	string id;
	string junction;
    string rule;

	name = road->GetRoadName();
	length = road->GetRoadLength();
	id = road->GetRoadId();
	junction = road->GetRoadJunction();
    rule = road->GetRoadRule();

	TiXmlElement *nodeRoad = new TiXmlElement("road");
	node->LinkEndChild(nodeRoad);

	nodeRoad->SetAttribute("name",name);
	std::stringstream slength;
	slength << setprecision(16) << setiosflags (ios_base::scientific) << length;
	nodeRoad->SetAttribute("length",slength.str());
	nodeRoad->SetAttribute("id",id);
	nodeRoad->SetAttribute("junction",junction);
    if(rule != "")
    {
        nodeRoad->SetAttribute("rule",rule);
    }

	//Fill in

	//Links
	WriteRoadLinks (nodeRoad,road);

	//Types
	WriteRoadType(nodeRoad, road);

	//PlanView
	WritePlanView(nodeRoad, road);

	//ElevationProfile
	WriteElevationProfile(nodeRoad, road);

	//LateralProfile
	WriteLateralProfile(nodeRoad, road);

	//Proceed to Lanes
	WriteLanes(nodeRoad, road);




	//Proceed to Objects
	WriteObjects(nodeRoad, road);

	//Proceed to Signals
	WriteSignals(nodeRoad, road);


    vector<string> * puserDataVector = road->GetUserData();
    unsigned int luserDataCount = puserDataVector->size();
    for(unsigned int i=0;i<luserDataCount;i++)
    {
        TiXmlElement * nodeuserData = new TiXmlElement("userData");
        nodeuserData->Parse(puserDataVector->at(i).data(),0,TIXML_ENCODING_UTF8);
        nodeRoad->LinkEndChild(nodeuserData);
    }

    if(road->GetRoadBorrowCount()>0)
    {
        WriteRoadBorrows(nodeRoad,road);
    }

    if(road->GetRoadNoavoidCount()>0)
    {
        WriteRoadNoavoids(nodeRoad,road);
    }

	/*
	//Proceed to Surface
	subNode=node->FirstChildElement("surface");
	if (subNode)
	{
	WriteSurface(road, subNode);
	}
	*/

	return true;
}
//--------------

bool  OpenDriveXmlWriter::WriteRoadLinks (TiXmlElement *node, Road* road)
{
	TiXmlElement* nodeLink = new TiXmlElement("link");
	node->LinkEndChild(nodeLink);

	RoadLink *lPredecessor = road->GetPredecessor();
	if(lPredecessor)
	{
		TiXmlElement* nodeLinkPredecessor = new TiXmlElement("predecessor");
		nodeLink->LinkEndChild(nodeLinkPredecessor);
		nodeLinkPredecessor->SetAttribute("elementType", lPredecessor->GetElementType());
		nodeLinkPredecessor->SetAttribute("elementId", lPredecessor->GetElementId());
		nodeLinkPredecessor->SetAttribute("contactPoint", lPredecessor->GetContactPoint());
        if(lPredecessor->GetElementS()>=0)
        {
            std::stringstream ss;
            ss << setprecision(16) << setiosflags (ios_base::scientific) << lPredecessor->GetElementS();
            nodeLinkPredecessor->SetAttribute("elementS", ss.str());
        }
        if(lPredecessor->GetElementDir() != "")
        {
            nodeLinkPredecessor->SetAttribute("elementDir",lPredecessor->GetElementDir());
        }
	}
	RoadLink *lSuccessor = road->GetSuccessor();
	if(lSuccessor)
	{
		TiXmlElement* nodeLinkSuccessor = new TiXmlElement("successor");
		nodeLink->LinkEndChild(nodeLinkSuccessor);
		nodeLinkSuccessor->SetAttribute("elementType", lSuccessor->GetElementType());
		nodeLinkSuccessor->SetAttribute("elementId", lSuccessor->GetElementId());
		nodeLinkSuccessor->SetAttribute("contactPoint", lSuccessor->GetContactPoint());
        if(lSuccessor->GetElementS()>=0)
        {
            std::stringstream ss;
            ss << setprecision(16) << setiosflags (ios_base::scientific) << lSuccessor->GetElementS();
            nodeLinkSuccessor->SetAttribute("elementS", ss.str());
        }
        if(lSuccessor->GetElementDir() != "")
        {
            nodeLinkSuccessor->SetAttribute("elementDir",lSuccessor->GetElementDir());
        }
	}
	RoadNeighbor *lNeighbor1 = road->GetNeighbor1();
	if(lNeighbor1)
	{
		TiXmlElement* nodeLinkNeighbor1 = new TiXmlElement("neighbor");
		nodeLink->LinkEndChild(nodeLinkNeighbor1);
		nodeLinkNeighbor1->SetAttribute("side", lNeighbor1->GetSide());
		nodeLinkNeighbor1->SetAttribute("elementId", lNeighbor1->GetElementId());
		nodeLinkNeighbor1->SetAttribute("direction", lNeighbor1->GetDirection());
	}
	RoadNeighbor *lNeighbor2 = road->GetNeighbor2();
	if(lNeighbor2)
	{
		TiXmlElement* nodeLinkNeighbor2 = new TiXmlElement("neighbor");
		nodeLink->LinkEndChild(nodeLinkNeighbor2);
		nodeLinkNeighbor2->SetAttribute("side", lNeighbor2->GetSide());
		nodeLinkNeighbor2->SetAttribute("elementId", lNeighbor2->GetElementId());
		nodeLinkNeighbor2->SetAttribute("direction", lNeighbor2->GetDirection());
	}

	return true;
}

//--------------

bool OpenDriveXmlWriter::WriteRoadTypeSpeed(TiXmlElement *node, RoadType *roadtype)
{
    double maxSpeed;
    string unit;

    unsigned int roadTypeSpeedCount = roadtype->GetRoadTypeSpeedCount();
    for(unsigned int i=0;i<roadTypeSpeedCount;i++)
    {
        RoadTypeSpeed * lRoadTypeSpeed = roadtype->GetRoadTypeSpeed(i);

        maxSpeed = lRoadTypeSpeed->GetmaxSpeed();
        unit = lRoadTypeSpeed->Getunit();

        TiXmlElement * nodeRoadTypeSpeed = new TiXmlElement("speed");
        node->LinkEndChild(nodeRoadTypeSpeed);

        std::stringstream ss;
        ss << setprecision(16) << setiosflags (ios_base::scientific) << maxSpeed;
        nodeRoadTypeSpeed->SetAttribute("max",ss.str());
        nodeRoadTypeSpeed->SetAttribute("unit",unit);
    }

    return true;
}

//--------------

bool OpenDriveXmlWriter::WriteRoadType (TiXmlElement *node, Road* road)
{
	double s;
	string type;

	unsigned int roadTypeCount = road->GetRoadTypeCount();
	for(unsigned int i=0; i<roadTypeCount; i++)
	{
		RoadType *lRoadType = road->GetRoadType(i);

		s=lRoadType->GetS();
		type=lRoadType->GetType();

		TiXmlElement *nodeRoadType = new TiXmlElement("type");
		node->LinkEndChild(nodeRoadType);

		std::stringstream ss;
		ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
		nodeRoadType->SetAttribute("s",ss.str());
		nodeRoadType->SetAttribute("type",type);

        WriteRoadTypeSpeed(nodeRoadType,lRoadType);
	}

	return true;
}
//--------------

bool OpenDriveXmlWriter::WritePlanView(TiXmlElement *node, Road *road)
{
	TiXmlElement* nodePlanView = new TiXmlElement("planView");
	node->LinkEndChild(nodePlanView);

	unsigned int geometryCount = road->GetGeometryBlockCount();
	for(unsigned int i=0; i<geometryCount; i++)
	{
		WriteGeometryBlock(nodePlanView, road->GetGeometryBlock(i));
	}

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteGeometryBlock (TiXmlElement *node, GeometryBlock* geometryBlock)
{

	if(geometryBlock->CheckIfLine())
	{

        WriteGeometry(node, geometryBlock->GetGeometryAt(0), geometryBlock->GetGeometryAt(0)->GetGeomType());
	}
	else
	{
		WriteGeometry(node, geometryBlock->GetGeometryAt(0), 1);
		WriteGeometry(node, geometryBlock->GetGeometryAt(1), 2);
		WriteGeometry(node, geometryBlock->GetGeometryAt(2), 1);
	}

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteGeometry(TiXmlElement *node, RoadGeometry* roadGeometry,  short int geometryType)
{
	double s, x, y, hdg, length;

	s=roadGeometry->GetS();
	x=roadGeometry->GetX();
	y=roadGeometry->GetY();
	hdg=roadGeometry->GetHdg();
	length=roadGeometry->GetLength();

	//Write the geometry node
	TiXmlElement *nodeGeometry = new TiXmlElement("geometry");
	node->LinkEndChild(nodeGeometry);

	std::stringstream ss;
	ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
	nodeGeometry->SetAttribute("s",ss.str());

	std::stringstream sx;
	sx << setprecision(16) << setiosflags (ios_base::scientific) << x;
	nodeGeometry->SetAttribute("x",sx.str());

	std::stringstream sy;
	sy << setprecision(16) << setiosflags (ios_base::scientific) << y;
	nodeGeometry->SetAttribute("y",sy.str());

	std::stringstream shdg;
	shdg << setprecision(16) << setiosflags (ios_base::scientific) << hdg;
	nodeGeometry->SetAttribute("hdg",shdg.str());

	std::stringstream slength;
	slength << setprecision(16) << setiosflags (ios_base::scientific) << length;
	nodeGeometry->SetAttribute("length",slength.str());


	//Write the type nodes
	switch ( geometryType )
	{
	case 0:	
		{
			//line
			TiXmlElement *nodeLine = new TiXmlElement("line");
			nodeGeometry->LinkEndChild(nodeLine);
			break;
		}
	case 1:
		{		
			//spiral
			double curvatureStart, curvatureEnd; 
			GeometrySpiral *lSpiral=static_cast<GeometrySpiral*>(roadGeometry);
			curvatureStart=lSpiral->GetCurvatureStart();
			curvatureEnd=lSpiral->GetCurvatureEnd();

			TiXmlElement *nodeSpiral = new TiXmlElement("spiral");
			nodeGeometry->LinkEndChild(nodeSpiral);


			std::stringstream scurvatureStart;
			scurvatureStart << setprecision(16) << setiosflags (ios_base::scientific) << curvatureStart;
			nodeSpiral->SetAttribute("curvStart",scurvatureStart.str());

			std::stringstream scurvatureEnd;
			scurvatureEnd << setprecision(16) << setiosflags (ios_base::scientific) << curvatureEnd;
			nodeSpiral->SetAttribute("curvEnd",scurvatureEnd.str());
			break;
		}
	case 2:
		{		
			//arc
			double curvature;
			GeometryArc *lArc=static_cast<GeometryArc*>(roadGeometry);
			curvature=lArc->GetCurvature();

			TiXmlElement *nodeArc = new TiXmlElement("arc");
			nodeGeometry->LinkEndChild(nodeArc);

			std::stringstream scurvature;
			scurvature << setprecision(16) << setiosflags (ios_base::scientific) << curvature;
			nodeArc->SetAttribute("curvature",scurvature.str());
			break;
		}
	case 3:
        {
            double ua,ub,uc,ud;
            GeometryPoly3 * lp3 = static_cast<GeometryPoly3 *>(roadGeometry);
            ua = lp3->GetA();
            ub = lp3->GetB();
            uc = lp3->GetC();
            ud = lp3->GetD();
            TiXmlElement *nodePoly3 = new TiXmlElement("poly3");
            nodeGeometry->LinkEndChild(nodePoly3);
            nodePoly3->SetDoubleAttribute("a",ua);
            nodePoly3->SetDoubleAttribute("b",ub);
            nodePoly3->SetDoubleAttribute("c",uc);
            nodePoly3->SetDoubleAttribute("d",ud);
			//poly3
			break;
		}

    case 4:
        {   //paramPoly3 add by Yu Chuli. 2019.11.1
            double ua,ub,uc,ud,va,vb,vc,vd;
            bool bNormal;
            GeometryParamPoly3 *lpp3=static_cast<GeometryParamPoly3 *>(roadGeometry);

            ua = lpp3->GetuA();
            ub = lpp3->GetuB();
            uc = lpp3->GetuC();
            ud = lpp3->GetuD();
            va = lpp3->GetvA();
            vb = lpp3->GetvB();
            vc = lpp3->GetvC();
            vd = lpp3->GetvD();
            bNormal = lpp3->GetNormal();

            TiXmlElement *nodeParamPoly3 = new TiXmlElement("paramPoly3");
            nodeGeometry->LinkEndChild(nodeParamPoly3);

            std::stringstream sua;
            sua << setprecision(16) << setiosflags (ios_base::scientific) << ua;
            nodeParamPoly3->SetAttribute("aU",sua.str());

            std::stringstream sub;
            sub << setprecision(16) << setiosflags (ios_base::scientific) << ub;
            nodeParamPoly3->SetAttribute("bU",sub.str());

            std::stringstream suc;
            suc << setprecision(16) << setiosflags (ios_base::scientific) << uc;
            nodeParamPoly3->SetAttribute("cU",suc.str());

            std::stringstream sud;
            sud << setprecision(16) << setiosflags (ios_base::scientific) << ud;
            nodeParamPoly3->SetAttribute("dU",sud.str());

            std::stringstream sva;
            sva << setprecision(16) << setiosflags (ios_base::scientific) << va;
            nodeParamPoly3->SetAttribute("aV",sva.str());

            std::stringstream svb;
            svb << setprecision(16) << setiosflags (ios_base::scientific) << vb;
            nodeParamPoly3->SetAttribute("bV",svb.str());

            std::stringstream svc;
            svc << setprecision(16) << setiosflags (ios_base::scientific) << vc;
            nodeParamPoly3->SetAttribute("cV",svc.str());

            std::stringstream svd;
            svd << setprecision(16) << setiosflags (ios_base::scientific) << vd;
            nodeParamPoly3->SetAttribute("dV",svd.str());

            if(bNormal == false)
            {
                nodeParamPoly3->SetAttribute("pRange","arcLength");
            }
            break;

        }
	}

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteElevationProfile (TiXmlElement *node, Road* road)
{
	double s, a, b, c, d;

	TiXmlElement* nodeElevationProfile = new TiXmlElement("elevationProfile");
	node->LinkEndChild(nodeElevationProfile);

	unsigned int lElevationCount = road->GetElevationCount();
	for(unsigned int i=0; i<lElevationCount; i++)
	{
		Elevation *lElevation = road->GetElevation(i);
		s=lElevation->GetS();
		a=lElevation->GetA();
		b=lElevation->GetB();
		c=lElevation->GetC();
		d=lElevation->GetD();

		TiXmlElement *nodeElevation = new TiXmlElement("elevation");
		nodeElevationProfile->LinkEndChild(nodeElevation);

		std::stringstream ss;
		ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
		nodeElevation->SetAttribute("s",ss.str());

		std::stringstream sa;
		sa << setprecision(16) << setiosflags (ios_base::scientific) << a;
		nodeElevation->SetAttribute("a",sa.str());

		std::stringstream sb;
		sb << setprecision(16) << setiosflags (ios_base::scientific) << b;
		nodeElevation->SetAttribute("b",sb.str());

		std::stringstream sc;
		sc << setprecision(16) << setiosflags (ios_base::scientific) << c;
		nodeElevation->SetAttribute("c",sc.str());

		std::stringstream sd;
		sd << setprecision(16) << setiosflags (ios_base::scientific) << d;
		nodeElevation->SetAttribute("d",sd.str());
	}
	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLateralProfile (TiXmlElement *node, Road* road)
{
	double s, a, b, c, d;
	string side;

	TiXmlElement* nodeLateralProfile = new TiXmlElement("lateralProfile");
	node->LinkEndChild(nodeLateralProfile);

	unsigned int lSuperElevationCount = road->GetSuperElevationCount();
	for(unsigned int i=0; i<lSuperElevationCount; i++)
	{
		SuperElevation *lSuperElevation = road->GetSuperElevation(i);
		s=lSuperElevation->GetS();
		a=lSuperElevation->GetA();
		b=lSuperElevation->GetB();
		c=lSuperElevation->GetC();
		d=lSuperElevation->GetD();

		TiXmlElement *nodeSuperElevation = new TiXmlElement("superelevation");
		nodeLateralProfile->LinkEndChild(nodeSuperElevation);

		std::stringstream ss;
		ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
		nodeSuperElevation->SetAttribute("s",ss.str());

		std::stringstream sa;
		sa << setprecision(16) << setiosflags (ios_base::scientific) << a;
		nodeSuperElevation->SetAttribute("a",sa.str());

		std::stringstream sb;
		sb << setprecision(16) << setiosflags (ios_base::scientific) << b;
		nodeSuperElevation->SetAttribute("b",sb.str());

		std::stringstream sc;
		sc << setprecision(16) << setiosflags (ios_base::scientific) << c;
		nodeSuperElevation->SetAttribute("c",sc.str());

		std::stringstream sd;
		sd << setprecision(16) << setiosflags (ios_base::scientific) << d;
		nodeSuperElevation->SetAttribute("d",sd.str());
	}


	unsigned int lCrossfallCount = road->GetCrossfallCount();
	for(unsigned int i=0; i<lCrossfallCount; i++)
	{
		Crossfall *lCrossfall = road->GetCrossfall(i);
		s=lCrossfall->GetS();
		a=lCrossfall->GetA();
		b=lCrossfall->GetB();
		c=lCrossfall->GetC();
		d=lCrossfall->GetD();
		side=lCrossfall->GetSide();

		TiXmlElement *nodeCrossfall = new TiXmlElement("crossfall");
		nodeLateralProfile->LinkEndChild(nodeCrossfall);

		nodeCrossfall->SetAttribute("side",side);

		std::stringstream ss;
		ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
		nodeCrossfall->SetAttribute("s",ss.str());

		std::stringstream sa;
		sa << setprecision(16) << setiosflags (ios_base::scientific) << a;
		nodeCrossfall->SetAttribute("a",sa.str());

		std::stringstream sb;
		sb << setprecision(16) << setiosflags (ios_base::scientific) << b;
		nodeCrossfall->SetAttribute("b",sb.str());

		std::stringstream sc;
		sc << setprecision(16) << setiosflags (ios_base::scientific) << c;
		nodeCrossfall->SetAttribute("c",sc.str());

		std::stringstream sd;
		sd << setprecision(16) << setiosflags (ios_base::scientific) << d;
		nodeCrossfall->SetAttribute("d",sd.str());
	}

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLanes (TiXmlElement *node, Road* road)
{
	TiXmlElement* nodeLanes = new TiXmlElement("lanes");
	node->LinkEndChild(nodeLanes);

	unsigned int lLaneSectionCount = road->GetLaneSectionCount();
	for(unsigned int i=0; i<lLaneSectionCount; i++)
	{
		WriteLaneSections(nodeLanes, road->GetLaneSection(i));
	}

    unsigned int lLaneOffsetCount = road->GetLaneOffsetCount();
    for(unsigned int i=0; i<lLaneOffsetCount; i++)
    {
        WriteLaneOffset(nodeLanes, road->GetLaneOffset(i));
    }

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneSections (TiXmlElement *node, LaneSection *laneSection)
{
	double s;
	s=laneSection->GetS();

	TiXmlElement* nodeLaneSection = new TiXmlElement("laneSection");
	node->LinkEndChild(nodeLaneSection);

	std::stringstream ss;
	ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
	nodeLaneSection->SetAttribute("s",ss.str());

    if(laneSection->GetSingleSide() != "")
    {
        nodeLaneSection->SetAttribute("singleSide",laneSection->GetSingleSide());
    }

	//Fill in lane section
	short int curType=1;
	TiXmlElement* nodeLanesLeft=NULL;
	if(laneSection->GetLeftLaneCount()>0)
	{
		nodeLanesLeft = new TiXmlElement("left");
		nodeLaneSection->LinkEndChild(nodeLanesLeft);
	}
	
	TiXmlElement* nodeLanesCenter = new TiXmlElement("center");
	nodeLaneSection->LinkEndChild(nodeLanesCenter);

	TiXmlElement* nodeLanesRight=NULL;
	if(laneSection->GetRightLaneCount()>0)
	{
		nodeLanesRight = new TiXmlElement("right");
		nodeLaneSection->LinkEndChild(nodeLanesRight);
	}


	unsigned int lLaneCount = laneSection->GetLaneCount();
	for(unsigned int i=0; i<lLaneCount; i++)
	{
		Lane *lLane = laneSection->GetLane(i);
		short int lType=lLane->GetSide();
		if(lType>0 && nodeLanesLeft!=NULL)
		{
			WriteLane(nodeLanesLeft, lLane);
		}
		else if(lType==0)
		{
			WriteLane(nodeLanesCenter, lLane);
		}
		else if(lType<0 && nodeLanesRight!=NULL)
		{
			WriteLane(nodeLanesRight, lLane);
		}
	}

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneOffset(TiXmlElement *node, LaneOffset *laneOffset)
{
    double s;
    s=laneOffset->GetS();

    TiXmlElement* nodeLaneSection = new TiXmlElement("laneOffset");
    node->LinkEndChild(nodeLaneSection);

    nodeLaneSection->SetDoubleAttribute("s",laneOffset->GetS());
    nodeLaneSection->SetDoubleAttribute("a",laneOffset->Geta());
    nodeLaneSection->SetDoubleAttribute("b",laneOffset->Getb());
    nodeLaneSection->SetDoubleAttribute("c",laneOffset->Getc());
    nodeLaneSection->SetDoubleAttribute("d",laneOffset->Getd());

//    std::stringstream ss;
//    ss << setprecision(16) << setiosflags (ios_base::scientific) << s;
//    nodeLaneSection->SetAttribute("s",ss.str());


//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Geta();
//    nodeLaneSection->SetAttribute("a",ss.str());


//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getb();
//    nodeLaneSection->SetAttribute("b",ss.str());

//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getc();
//    nodeLaneSection->SetAttribute("c",ss.str());

//    ss << setprecision(16) << setiosflags (ios_base::scientific) << laneOffset->Getd();
//    nodeLaneSection->SetAttribute("d",ss.str());

    return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLane (TiXmlElement *node, Lane* lane)
{
	//Write Lane attributes
	int id;
	string type; 
	string level; 
	bool boolLevel;
	int predecessor;
	int successor;

	id=lane->GetId();
	type=lane->GetType();
	boolLevel=lane->GetLevel();
	if(boolLevel) level="true";
	else level="false";
	predecessor=lane->GetPredecessor();
	successor=lane->GetSuccessor();
	
	TiXmlElement* nodeLane = new TiXmlElement("lane");
	node->LinkEndChild(nodeLane);

	//Attributes
	nodeLane->SetAttribute("id",id);
	nodeLane->SetAttribute("type",type);
	nodeLane->SetAttribute("level",level);
	
	//Links
	TiXmlElement* nodeLaneLink = new TiXmlElement("link");
	nodeLane->LinkEndChild(nodeLaneLink);
	if(lane->IsPredecessorSet())
	{
		TiXmlElement* nodeLaneLinkPredecessor = new TiXmlElement("predecessor");
		nodeLaneLink->LinkEndChild(nodeLaneLinkPredecessor);
		nodeLaneLinkPredecessor->SetAttribute("id",predecessor);
	}
	if(lane->IsSuccessorSet())
	{
		TiXmlElement* nodeLaneLinkSuccessor = new TiXmlElement("successor");
		nodeLaneLink->LinkEndChild(nodeLaneLinkSuccessor);
		nodeLaneLinkSuccessor->SetAttribute("id",successor);
	}

	//Lane Width
	unsigned int lLaneWidthCount = lane->GetLaneWidthCount();
	for(unsigned int i=0; i<lLaneWidthCount; i++)
	{
		WriteLaneWidth(nodeLane, lane->GetLaneWidth(i));
	}

    //Lane Border
    unsigned int lLaneBorderCount = lane->GetLaneBorderCount();
    for(unsigned int i=0; i<lLaneBorderCount; i++)
    {
        WriteLaneBorder(nodeLane, lane->GetLaneBorder(i));
    }

	//Lane Road Mark
	unsigned int lLaneRoadMark = lane->GetLaneRoadMarkCount();
	for(unsigned int i=0; i<lLaneRoadMark; i++)
	{
		WriteLaneRoadMark(nodeLane, lane->GetLaneRoadMark(i));
	}

	//Lane Material
	unsigned int lLaneMaterial = lane->GetLaneMaterialCount();
	for(unsigned int i=0; i<lLaneMaterial; i++)
	{
		WriteLaneMaterial(nodeLane, lane->GetLaneMaterial(i));
	}

	//Lane Visibility
	unsigned int lLaneVisibility = lane->GetLaneVisibilityCount();
	for(unsigned int i=0; i<lLaneVisibility; i++)
	{
		WriteLaneVisibility(nodeLane, lane->GetLaneVisibility(i));
	}

	//Lane speed
	unsigned int lLaneSpeed = lane->GetLaneSpeedCount();
	for(unsigned int i=0; i<lLaneSpeed; i++)
	{
		WriteLaneSpeed(nodeLane, lane->GetLaneSpeed(i));
	}

	//Lane access
	unsigned int lLaneAccess = lane->GetLaneAccessCount();
	for(unsigned int i=0; i<lLaneAccess; i++)
	{
		WriteLaneAccess(nodeLane, lane->GetLaneAccess(i));
	}

	//Lane height
	unsigned int lLaneHeight = lane->GetLaneHeightCount();
	for(unsigned int i=0; i<lLaneHeight; i++)
	{
		WriteLaneHeight(nodeLane, lane->GetLaneHeight(i));
	}

 //   string struserData;
    vector<string> * puserDataVector = lane->GetUserDataVector();
    unsigned int luserDataCount = puserDataVector->size();
    for(unsigned int i=0;i<luserDataCount;i++)
    {
        TiXmlElement * nodeuserData = new TiXmlElement("userData");
        nodeuserData->Parse(puserDataVector->at(i).data(),0,TIXML_ENCODING_UTF8);
        nodeLane->LinkEndChild(nodeuserData);
    }
//    lane->GetuserData(struserData);
//    if(struserData != "")
//    {
//        TiXmlElement * nodeuserData = new TiXmlElement("userData");
//        nodeuserData->Parse(struserData.data(),0,TIXML_ENCODING_UTF8);
//        nodeLane->LinkEndChild(nodeuserData);

//    }

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneBorder(TiXmlElement *node, LaneBorder* laneBorder)
{
    double sOffset, a, b, c, d;

    sOffset=laneBorder->GetS();
    a=laneBorder->GetA();
    b=laneBorder->GetB();
    c=laneBorder->GetC();
    d=laneBorder->GetD();

    TiXmlElement* nodeLaneBorder = new TiXmlElement("border");
    node->LinkEndChild(nodeLaneBorder);

    std::stringstream ssOffset;
    ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
    nodeLaneBorder->SetAttribute("sOffset",ssOffset.str());

    std::stringstream sa;
    sa << setprecision(16) << setiosflags (ios_base::scientific) << a;
    nodeLaneBorder->SetAttribute("a",sa.str());

    std::stringstream sb;
    sb << setprecision(16) << setiosflags (ios_base::scientific) << b;
    nodeLaneBorder->SetAttribute("b",sb.str());

    std::stringstream sc;
    sc << setprecision(16) << setiosflags (ios_base::scientific) << c;
    nodeLaneBorder->SetAttribute("c",sc.str());

    std::stringstream sd;
    sd << setprecision(16) << setiosflags (ios_base::scientific) << d;
    nodeLaneBorder->SetAttribute("d",sd.str());

    return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneWidth(TiXmlElement *node, LaneWidth* laneWidth)
{
	double sOffset, a, b, c, d;

	sOffset=laneWidth->GetS();
	a=laneWidth->GetA();
	b=laneWidth->GetB();
	c=laneWidth->GetC();
	d=laneWidth->GetD();

	TiXmlElement* nodeLaneWidth = new TiXmlElement("width");
	node->LinkEndChild(nodeLaneWidth);

	std::stringstream ssOffset;
	ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
	nodeLaneWidth->SetAttribute("sOffset",ssOffset.str());

	std::stringstream sa;
	sa << setprecision(16) << setiosflags (ios_base::scientific) << a;
	nodeLaneWidth->SetAttribute("a",sa.str());

	std::stringstream sb;
	sb << setprecision(16) << setiosflags (ios_base::scientific) << b;
	nodeLaneWidth->SetAttribute("b",sb.str());

	std::stringstream sc;
	sc << setprecision(16) << setiosflags (ios_base::scientific) << c;
	nodeLaneWidth->SetAttribute("c",sc.str());

	std::stringstream sd;
	sd << setprecision(16) << setiosflags (ios_base::scientific) << d;
	nodeLaneWidth->SetAttribute("d",sd.str());

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneRoadMark(TiXmlElement *node, LaneRoadMark* laneRoadMark)
{

	double sOffset;
	string type;
	string weight;
	string color; 
	double width;
	string laneChange;

	sOffset=laneRoadMark->GetS();
	type=laneRoadMark->GetType();
	weight=laneRoadMark->GetWeight();
	color=laneRoadMark->GetColor();
	width=laneRoadMark->GetWidth();
	laneChange=laneRoadMark->GetLaneChange();

	TiXmlElement* nodeLaneRoadMark = new TiXmlElement("roadMark");
	node->LinkEndChild(nodeLaneRoadMark);

	std::stringstream ssOffset;
	ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
	nodeLaneRoadMark->SetAttribute("sOffset",ssOffset.str());
	nodeLaneRoadMark->SetAttribute("type",type);
	nodeLaneRoadMark->SetAttribute("weight",weight);
	nodeLaneRoadMark->SetAttribute("color",color);

	std::stringstream swidth;
	swidth << setprecision(16) << setiosflags (ios_base::scientific) << width;
	nodeLaneRoadMark->SetAttribute("width",swidth.str());
	nodeLaneRoadMark->SetAttribute("laneChange",laneChange);

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneMaterial(TiXmlElement *node, LaneMaterial* laneMaterial)
{
	double sOffset;
	string surface;
	double friction;
	double roughness;

	sOffset=laneMaterial->GetS();
	surface=laneMaterial->GetSurface();
	friction=laneMaterial->GetFriction();
	roughness=laneMaterial->GetRoughness();

	TiXmlElement* nodeLaneMaterial = new TiXmlElement("material");
	node->LinkEndChild(nodeLaneMaterial);

	std::stringstream ssOffset;
	ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
	nodeLaneMaterial->SetAttribute("sOffset",ssOffset.str());
	nodeLaneMaterial->SetAttribute("surface",surface);

	std::stringstream sfriction;
	sfriction << setprecision(16) << setiosflags (ios_base::scientific) << friction;
	nodeLaneMaterial->SetAttribute("friction",sfriction.str());

	std::stringstream sroughness;
	sroughness << setprecision(16) << setiosflags (ios_base::scientific) << roughness;
	nodeLaneMaterial->SetAttribute("roughness",sroughness.str());

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneVisibility(TiXmlElement *node, LaneVisibility* laneVisibility)
{
	double sOffset;
	double forward;
	double back;
	double left;
	double right;

	sOffset=laneVisibility->GetS();
	forward=laneVisibility->GetForward();
	back=laneVisibility->GetBack();
	left=laneVisibility->GetLeft();
	right=laneVisibility->GetRight();

	TiXmlElement* nodeLaneVisibility = new TiXmlElement("visibility");
	node->LinkEndChild(nodeLaneVisibility);

	std::stringstream ssOffset;
	ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
	nodeLaneVisibility->SetAttribute("sOffset",ssOffset.str());

	std::stringstream sforward;
	sforward << setprecision(16) << setiosflags (ios_base::scientific) << forward;
	nodeLaneVisibility->SetAttribute("forward",sforward.str());

	std::stringstream sback;
	sback << setprecision(16) << setiosflags (ios_base::scientific) << back;
	nodeLaneVisibility->SetAttribute("back",sback.str());

	std::stringstream sleft;
	sleft << setprecision(16) << setiosflags (ios_base::scientific) << left;
	nodeLaneVisibility->SetAttribute("left",sleft.str());

	std::stringstream sright;
	sright << setprecision(16) << setiosflags (ios_base::scientific) << right;
	nodeLaneVisibility->SetAttribute("right",sright.str());

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneSpeed(TiXmlElement *node, LaneSpeed* laneSpeed)
{	
	double sOffset;
	double max;

	sOffset=laneSpeed->GetS();
	max=laneSpeed->GetMax();

	TiXmlElement* nodeLaneSpeed = new TiXmlElement("speed");
	node->LinkEndChild(nodeLaneSpeed);

	std::stringstream ssOffset;
	ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
	nodeLaneSpeed->SetAttribute("sOffset",ssOffset.str());

	std::stringstream smax;
	smax << setprecision(16) << setiosflags (ios_base::scientific) << max;
	nodeLaneSpeed->SetAttribute("max",smax.str());

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneAccess(TiXmlElement *node, LaneAccess* laneAccess)
{
	double sOffset;
	string restriction;

	sOffset=laneAccess->GetS();
	restriction=laneAccess->GetRestriction();

	TiXmlElement* nodeLaneAccess = new TiXmlElement("access");
	node->LinkEndChild(nodeLaneAccess);

	std::stringstream ssOffset;
	ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
	nodeLaneAccess->SetAttribute("sOffset",ssOffset.str());
	nodeLaneAccess->SetAttribute("restriction",restriction);

	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteLaneHeight(TiXmlElement *node, LaneHeight* laneHeight)
{
	double sOffset;
	double inner;
	double outer;

	sOffset=laneHeight->GetS();
	inner=laneHeight->GetInner();
	outer=laneHeight->GetOuter();

	TiXmlElement* nodeLaneHeight = new TiXmlElement("height");
	node->LinkEndChild(nodeLaneHeight);

	std::stringstream ssOffset;
	ssOffset << setprecision(16) << setiosflags (ios_base::scientific) << sOffset;
	nodeLaneHeight->SetAttribute("sOffset",ssOffset.str());

	std::stringstream sinner;
	sinner << setprecision(16) << setiosflags (ios_base::scientific) << inner;
	nodeLaneHeight->SetAttribute("inner",sinner.str());

	std::stringstream souter;
	souter << setprecision(16) << setiosflags (ios_base::scientific) << outer;
	nodeLaneHeight->SetAttribute("outer",souter.str());
	
	return true;
}
//--------------

//--------------

bool OpenDriveXmlWriter::WriteObjects (TiXmlElement *node, Road* road)
{
	TiXmlElement* nodeObjects = new TiXmlElement("objects");
	node->LinkEndChild(nodeObjects);

    unsigned int lObjectCount = road->GetObjectCount();
    for(unsigned int i=0; i<lObjectCount; i++)
    {
        WriteObject(nodeObjects, road->GetObject(i));
    }

	return true;
}
//--------------
bool OpenDriveXmlWriter::WriteObject(TiXmlElement *node, Object *pObject)
{
    TiXmlElement* nodeObject = new TiXmlElement("object");
    node->LinkEndChild(nodeObject);

    nodeObject->SetDoubleAttribute("t",pObject->Gett());
    nodeObject->SetDoubleAttribute("zOffset",pObject->GetzOffset());
    if(pObject->Gettype() != "")
    {
        nodeObject->SetAttribute("type",pObject->Gettype());
    }
    double validLength;
    if(pObject->GetvalidLength(validLength) == 1)
    {
        nodeObject->SetDoubleAttribute("validLength",validLength);
    }
    if(pObject->Getorientation() != "")
    {
        nodeObject->SetAttribute("orientation",pObject->Getorientation());
    }
    if(pObject->Getsubtype() != "")
    {
        nodeObject->SetAttribute("subtype",pObject->Getsubtype());
    }
    if(pObject->Getdynamic() != "")
    {
        nodeObject->SetAttribute("dynamic",pObject->Getdynamic());
    }
    double hdg;
    if(pObject->Gethdg(hdg) == 1)
    {
        nodeObject->SetDoubleAttribute("hdg",hdg);
    }
    if(pObject->Getname() != "")
    {
        nodeObject->SetAttribute("name",pObject->Getname());
    }
    double pitch;
    if(pObject->Getpitch(pitch) == 1)
    {
        nodeObject->SetDoubleAttribute("pitch",pitch);
    }
    nodeObject->SetAttribute("id",pObject->Getid());
    double roll;
    if(pObject->Getroll(roll) == 1)
    {
        nodeObject->SetDoubleAttribute("roll",roll);
    }
    double height;
    if(pObject->Getheight(height) == 1)
    {
        nodeObject->SetDoubleAttribute("height",height);
    }
    nodeObject->SetDoubleAttribute("s",pObject->Gets());
    double length;
    if(pObject->Getlength(length) == 1)
    {
        nodeObject->SetDoubleAttribute("length",length);
    }
    double width;
    if(pObject->Getwidth(width) == 1)
    {
        nodeObject->SetDoubleAttribute("width",width);
    }
    double radius;
    if(pObject->Getradius(radius) == 1)
    {
        nodeObject->SetDoubleAttribute("radius",radius);
    }

    Object_parkingSpace parkingSpace;
    if(pObject->GetparkingSpace(parkingSpace) == 1)
    {
        WriteObjectParkingSpace(nodeObject,&parkingSpace);
    }

    unsigned int i;

    unsigned nrepeatcount = pObject->GetObjectrepeatCount();
    for(i=0;i<nrepeatcount;i++)
    {
        Object_repeat * pObject_repeat = pObject->GetObjectrepeat(i);
        WriteObjectrepeat(nodeObject,pObject_repeat);
    }

    unsigned nmaterialcount = pObject->GetObjectmaterialCount();
    for(i=0;i<nmaterialcount;i++)
    {
        Object_material * pObject_material = pObject->GetObjectmaterial(i);
        WriteObjectMaterial(nodeObject,pObject_material);
    }

    Object_outlines * pObject_outlines =  pObject->Getoutlines();

    if(pObject_outlines != NULL)
    {
        WriteObjectOutlines(nodeObject,pObject_outlines);
    }

    unsigned noutlinecount = pObject->GetObjectoutlineCount();
    for(i=0;i<noutlinecount;i++)
    {
        Object_outlines_outline * pObject_outlines_outline = pObject->GetObjectoutline(i);
        WriteObjectOutlinesOutline(nodeObject,pObject_outlines_outline);
    }

    return true;
}
//--------------

bool OpenDriveXmlWriter::WriteObjectrepeat(TiXmlElement * node,Object_repeat * pObject_repeat)
{
    double s;
    double length;
    double distance;
    double tStart;
    double tEnd;
    double heightStart;
    double heightEnd;
    double zOffsetStart;
    double zOffsetEnd;

    double widthStart;
    double widthEnd;
    double lengthStart;
    double lengthEnd;
    double radiusStart;
    double radiusEnd;

    s = pObject_repeat->Gets();
    length = pObject_repeat->Getlength();
    distance = pObject_repeat->Getdistance();
    tStart = pObject_repeat->GettStart();
    tEnd = pObject_repeat->GettEnd();
    heightStart = pObject_repeat->GetheightStart();
    heightEnd = pObject_repeat->GetheightEnd();
    zOffsetStart = pObject_repeat->GetzOffsetStart();
    zOffsetEnd = pObject_repeat->GetzOffsetEnd();

    TiXmlElement* nodeObjectRepeat = new TiXmlElement("repeat");
    node->LinkEndChild(nodeObjectRepeat);
    nodeObjectRepeat->SetDoubleAttribute("s",s);
    nodeObjectRepeat->SetDoubleAttribute("length",length);
    nodeObjectRepeat->SetDoubleAttribute("distance",distance);
    nodeObjectRepeat->SetDoubleAttribute("tStart",tStart);
    nodeObjectRepeat->SetDoubleAttribute("tEnd",tEnd);
    nodeObjectRepeat->SetDoubleAttribute("heightStart",heightStart);
    nodeObjectRepeat->SetDoubleAttribute("heightEnd",heightEnd);
    nodeObjectRepeat->SetDoubleAttribute("zOffsetStart",zOffsetStart);
    nodeObjectRepeat->SetDoubleAttribute("zOffsetEnd",zOffsetEnd);

    if(pObject_repeat->GetwidthStart(widthStart) == 1)
    {
        nodeObjectRepeat->SetDoubleAttribute("widthStart",widthStart);
    }
    if(pObject_repeat->GetwidthEnd(widthEnd) == 1)
    {
        nodeObjectRepeat->SetDoubleAttribute("widthEnd",widthEnd);
    }
    if(pObject_repeat->GetlengthStart(lengthStart) == 1)
    {
        nodeObjectRepeat->SetDoubleAttribute("lengthStart",lengthStart);
    }
    if(pObject_repeat->GetlengthEnd(lengthEnd) == 1)
    {
        nodeObjectRepeat->SetDoubleAttribute("lengthEnd",lengthEnd);
    }
    if(pObject_repeat->GetradiusStart(radiusStart) == 1)
    {
        nodeObjectRepeat->SetDoubleAttribute("radiusStart",radiusStart);
    }
    if(pObject_repeat->GetradiusEnd(radiusEnd) == 1)
    {
        nodeObjectRepeat->SetDoubleAttribute("radiusEnd",radiusEnd);
    }
    return true;
}
//--------------
bool OpenDriveXmlWriter::WriteObjectMaterial(TiXmlElement * node,Object_material * pObject_material)
{
    TiXmlElement* nodeObjectMaterial = new TiXmlElement("material");
    node->LinkEndChild(nodeObjectMaterial);
    string surface;
    double friction;
    double roughness;
    if(pObject_material->Getsurface(surface) == 1)
    {
       nodeObjectMaterial->SetAttribute("surface",surface);
    }
    if(pObject_material->Getfriction(friction) == 1)
    {
       nodeObjectMaterial->SetDoubleAttribute("friction",friction);
    }
    if(pObject_material->Getroughness(roughness) == 1)
    {
       nodeObjectMaterial->SetDoubleAttribute("roughness",roughness);
    }
    return true;
}

//--------------
bool OpenDriveXmlWriter::WriteObjectParkingSpace(TiXmlElement *node, Object_parkingSpace *pObject_parkingSpace)
{
    TiXmlElement* nodeObjectParkingSpace = new TiXmlElement("parkingSpace");
    node->LinkEndChild(nodeObjectParkingSpace);
    nodeObjectParkingSpace->SetAttribute("access",pObject_parkingSpace->Getaccess());
    if(pObject_parkingSpace->Getrestrictions() != "")
    {
       nodeObjectParkingSpace->SetAttribute("restrictions",pObject_parkingSpace->Getrestrictions());
    }
    return true;
}
//--------------
bool OpenDriveXmlWriter::WriteObjectOutlines(TiXmlElement *node, Object_outlines *pObject_outlines)
{
    Object_outlines_outline xoutlines_outline;
    if(pObject_outlines->GetObject_outlines_outline(xoutlines_outline) == 1)
    {
        TiXmlElement * nodeObjectOutlines = new TiXmlElement("outlines");
        node->LinkEndChild(nodeObjectOutlines);
        WriteObjectOutlinesOutline(nodeObjectOutlines,&xoutlines_outline);
        return true;
    }
    std::cout<<"outlines not have outline."<<std::endl;
    return false;
}
//--------------

bool OpenDriveXmlWriter::WriteObjectOutlinesOutline(TiXmlElement *node, Object_outlines_outline *pObject_outlines_outline)
{
    TiXmlElement * nodeObjectOutlinesOutline = new TiXmlElement("outline");
    node->LinkEndChild(nodeObjectOutlinesOutline);
    int id;
    string fillType,strouter,strclosed,laneType;
    bool outer,closed;
    if(pObject_outlines_outline->Getid(id) == 1)
    {
        nodeObjectOutlinesOutline->SetAttribute("id",id);
    }
    if(pObject_outlines_outline->GetfillType(fillType) == 1)
    {
        nodeObjectOutlinesOutline->SetAttribute("fillType",fillType);
    }
    if(pObject_outlines_outline->Getouter(outer) == 1)
    {
        if(outer)
        {
            strouter = "true";
        }
        else
        {
            strouter = "false";
        }
        nodeObjectOutlinesOutline->SetAttribute("outer",strouter);
    }
    if(pObject_outlines_outline->Getclosed(closed) == 1)
    {
        if(closed)
        {
            strclosed = "true";
        }
        else
        {
            strclosed = "false";
        }
        nodeObjectOutlinesOutline->SetAttribute("closed",strclosed);
    }
    if(pObject_outlines_outline->GetlaneType(laneType) == 1)
    {
        nodeObjectOutlinesOutline->SetAttribute("laneType",laneType);
    }

    unsigned int ncornerRoadCount = pObject_outlines_outline->GetcornerRoadCount();
    unsigned int i;
    for(i=0;i<ncornerRoadCount;i++)
    {
        Object_outlines_outline_cornerRoad * pcornerRoad = pObject_outlines_outline->GetcornerRoad(i);
        WriteObjectOutlinesOutlinecornerRoad(nodeObjectOutlinesOutline,pcornerRoad);
    }

    unsigned int ncornerLocalCount = pObject_outlines_outline->GetcornerLocalCount();
    for(i=0;i<ncornerLocalCount;i++)
    {
        Object_outlines_outline_cornerLocal * pcornerLocal = pObject_outlines_outline->GetcornerLocal(i);
        WriteObjectOutlinesOutlinecornerLocal(nodeObjectOutlinesOutline,pcornerLocal);
    }
    return true;
}

//--------------

bool OpenDriveXmlWriter::WriteObjectOutlinesOutlinecornerRoad(TiXmlElement *node, Object_outlines_outline_cornerRoad *pObject_outlines_outline_cornerRoad)
{
    TiXmlElement* nodecornerRoad = new TiXmlElement("cornerRoad");
    node->LinkEndChild(nodecornerRoad);

    nodecornerRoad->SetDoubleAttribute("s",pObject_outlines_outline_cornerRoad->Gets());
    nodecornerRoad->SetDoubleAttribute("t",pObject_outlines_outline_cornerRoad->Gett());
    nodecornerRoad->SetDoubleAttribute("dz",pObject_outlines_outline_cornerRoad->Getdz());
    nodecornerRoad->SetDoubleAttribute("height",pObject_outlines_outline_cornerRoad->Getheight());

    int id;
    if(pObject_outlines_outline_cornerRoad->Getid(id) == 1)
    {
        nodecornerRoad->SetAttribute("id",id);
    }

    return true;
}

//--------------

bool OpenDriveXmlWriter::WriteObjectOutlinesOutlinecornerLocal(TiXmlElement *node, Object_outlines_outline_cornerLocal *pObject_outlines_outline_cornerLocal)
{
    TiXmlElement* nodecornerLocal = new TiXmlElement("cornerLocal");
    node->LinkEndChild(nodecornerLocal);

    nodecornerLocal->SetDoubleAttribute("u",pObject_outlines_outline_cornerLocal->Getu());
    nodecornerLocal->SetDoubleAttribute("v",pObject_outlines_outline_cornerLocal->Getv());
    nodecornerLocal->SetDoubleAttribute("z",pObject_outlines_outline_cornerLocal->Getz());
    nodecornerLocal->SetDoubleAttribute("height",pObject_outlines_outline_cornerLocal->Getheight());

    int id;
    if(pObject_outlines_outline_cornerLocal->Getid(id) == 1)
    {
        nodecornerLocal->SetAttribute("id",id);
    }
    return true;
}

//--------------

bool OpenDriveXmlWriter::WriteRoadBorrow(TiXmlElement *node, RoadBorrow *pRoadBorrow)
{
    TiXmlElement* nodeRoadBorrow = new TiXmlElement("roadborrow");
    node->LinkEndChild(nodeRoadBorrow);

    nodeRoadBorrow->SetDoubleAttribute("sOffset",pRoadBorrow->GetS());
    nodeRoadBorrow->SetDoubleAttribute("length",pRoadBorrow->GetLength());
    nodeRoadBorrow->SetAttribute("mode",pRoadBorrow->GetMode());
    return true;
}
//--------------
bool OpenDriveXmlWriter::WriteRoadBorrows(TiXmlElement *node, Road *road)
{
    TiXmlElement* nodeRoadBorrows = new TiXmlElement("userData");
    nodeRoadBorrows->SetAttribute("code","roadborrow");
    node->LinkEndChild(nodeRoadBorrows);

    unsigned int lRoadBorrowCount = road->GetRoadBorrowCount();
    for(unsigned int i=0; i<lRoadBorrowCount; i++)
    {
        WriteRoadBorrow(nodeRoadBorrows, road->GetRoadBorrow(i));
    }

    return true;
}
//--------------

bool OpenDriveXmlWriter::WriteRoadNoavoids(TiXmlElement *node, Road* road)
{
    TiXmlElement* nodeRoadNoavoids = new TiXmlElement("userData");
    nodeRoadNoavoids->SetAttribute("code","roadnoavoid");
    node->LinkEndChild(nodeRoadNoavoids);

    unsigned int lRoadNoavoidCount = road->GetRoadNoavoidCount();
    for(unsigned int i=0; i<lRoadNoavoidCount; i++)
    {
        WriteRoadNoavoid(nodeRoadNoavoids, road->GetRoadNoavoid(i));
    }

    return true;
}

//--------------

bool OpenDriveXmlWriter::WriteRoadNoavoid(TiXmlElement *node,RoadNoavoid * pRoadNoavoid)
{
    TiXmlElement* nodeRoadNoavoid = new TiXmlElement("roadnoavoid");
    node->LinkEndChild(nodeRoadNoavoid);

    nodeRoadNoavoid->SetDoubleAttribute("sOffset",pRoadNoavoid->GetS());
    nodeRoadNoavoid->SetDoubleAttribute("length",pRoadNoavoid->GetLength());
    return true;
}

//--------------

bool OpenDriveXmlWriter::WriteSignals (TiXmlElement *node, Road* road)
{
	TiXmlElement* nodeSignals = new TiXmlElement("signals");
	node->LinkEndChild(nodeSignals);

    unsigned int lSignalSectionCount = road->GetSignalCount();
    for(unsigned int i=0; i<lSignalSectionCount; i++)
    {
        WriteSignal(nodeSignals, road->GetSignal(i));
    }

	return true;
}

bool OpenDriveXmlWriter::WriteSignal(TiXmlElement *node, Signal * pSignal)
{
    TiXmlElement* nodeSignal = new TiXmlElement("signal");
    node->LinkEndChild(nodeSignal);

    nodeSignal->SetDoubleAttribute("s",pSignal->Gets());
    nodeSignal->SetDoubleAttribute("t",pSignal->Gett());
    nodeSignal->SetAttribute("id",pSignal->Getid());
    nodeSignal->SetAttribute("name",pSignal->Getname());
    if(pSignal->Getdynamic() == true)
    nodeSignal->SetAttribute("dynamic","yes");
    else
        nodeSignal->SetAttribute("dynamic","no");
    nodeSignal->SetAttribute("orientation",pSignal->Getorientation());
    nodeSignal->SetDoubleAttribute("zOffset",pSignal->GetzOffset());
    nodeSignal->SetAttribute("type",pSignal->Gettype());
    nodeSignal->SetAttribute("country",pSignal->Getcountry());
    nodeSignal->SetAttribute("countryRevision",pSignal->GetcountryRevision());
    nodeSignal->SetAttribute("subtype",pSignal->Getsubtype());
    nodeSignal->SetDoubleAttribute("hOffset",pSignal->GethOffset());
    nodeSignal->SetDoubleAttribute("pitch",pSignal->Getpitch());
    nodeSignal->SetDoubleAttribute("roll",pSignal->Getroll());
    nodeSignal->SetDoubleAttribute("height",pSignal->Getheight());
    nodeSignal->SetDoubleAttribute("width",pSignal->Getwidth());

    signal_laneValidity * psignal_lanevalidity = pSignal->GetlaneValidity();
    if(psignal_lanevalidity != 0)
    {
        WriteSignal_laneValidity(nodeSignal,psignal_lanevalidity);
    }

    signal_positionInertial * psignal_positionInertial = pSignal->GetpositionInertial();
    if(psignal_positionInertial != 0)
    {
        WriteSignal_positionInertial(nodeSignal,psignal_positionInertial);
    }

    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());

    return true;
}

bool OpenDriveXmlWriter::WriteSignal_laneValidity(TiXmlElement *node, signal_laneValidity *pSignal_laneValidity)
{
    TiXmlElement* nodelaneValidity = new TiXmlElement("validity");

    node->LinkEndChild(nodelaneValidity);

    nodelaneValidity->SetAttribute("fromLane",pSignal_laneValidity->GetfromLane());
    nodelaneValidity->SetAttribute("toLane",pSignal_laneValidity->GettoLane());

    return true;
}

//--------------

bool OpenDriveXmlWriter::WriteSurface (TiXmlElement *node, Road* road)
{
	return true;
}
//--------------

bool OpenDriveXmlWriter::WriteController (TiXmlElement *node)
{	return true;	}
//--------------

bool OpenDriveXmlWriter::WriteJunction (TiXmlElement *node, Junction *junction)
{
	string name;
	string id;

	name = junction->GetName();
	id = junction->GetId();

	TiXmlElement *nodeJunction = new TiXmlElement("junction");
	node->LinkEndChild(nodeJunction);

	nodeJunction->SetAttribute("name",name);
	nodeJunction->SetAttribute("id",id);

	//Connections
	WriteJunctionConnection(nodeJunction, junction);

	//Priorities
	WriteJunctionPriority(nodeJunction, junction);

	//Controllers
	WriteJunctionController(nodeJunction, junction);

	return true;
}
//--------------
bool OpenDriveXmlWriter::WriteJunctionConnection (TiXmlElement *node, Junction* junction)
{
	string id;
	string incomingRoad;
	string connectingRoad;
	string contactPoint;

	unsigned int junctionConnectionCount = junction->GetJunctionConnectionCount();
	for(unsigned int i=0; i<junctionConnectionCount; i++)
	{
		JunctionConnection *lJunctionConnection = junction->GetJunctionConnection(i);

		id = lJunctionConnection->GetId();
		incomingRoad = lJunctionConnection->GetIncomingRoad();
		connectingRoad = lJunctionConnection->GetConnectingRoad();
		contactPoint = lJunctionConnection->GetContactPoint();

		TiXmlElement *nodeJunctionConnection = new TiXmlElement("connection");
		node->LinkEndChild(nodeJunctionConnection);

		nodeJunctionConnection->SetAttribute("id",id);
		nodeJunctionConnection->SetAttribute("incomingRoad",incomingRoad);
		nodeJunctionConnection->SetAttribute("connectingRoad",connectingRoad);
		nodeJunctionConnection->SetAttribute("contactPoint",contactPoint);

		//Lane links
		WriteJunctionConnectionLaneLink(nodeJunctionConnection, lJunctionConnection);
	}

	return true;
}
bool OpenDriveXmlWriter::WriteJunctionConnectionLaneLink (TiXmlElement *node, JunctionConnection* junctionConnection)
{
	int from;
	int to;
	
	unsigned int junctionLaneLinkCount = junctionConnection->GetJunctionLaneLinkCount();
	for(unsigned int i=0; i<junctionLaneLinkCount; i++)
	{
		JunctionLaneLink *lJunctionLaneLink = junctionConnection->GetJunctionLaneLink(i);

		from = lJunctionLaneLink->GetFrom();
		to = lJunctionLaneLink->GetTo();

		TiXmlElement *nodeJunctionLaneLink = new TiXmlElement("laneLink");
		node->LinkEndChild(nodeJunctionLaneLink);

		nodeJunctionLaneLink->SetAttribute("from",from);
		nodeJunctionLaneLink->SetAttribute("to",to);
	}
	return true;
}

bool OpenDriveXmlWriter::WriteJunctionPriority (TiXmlElement *node, Junction* junction)
{
	string high;
	string low;

	unsigned int junctionPriorityCount = junction->GetJunctionPriorityCount();
	for(unsigned int i=0; i<junctionPriorityCount; i++)
	{
		JunctionPriorityRoad *lJunctionPriority = junction->GetJunctionPriority(i);

		high = lJunctionPriority->GetHigh();
		low = lJunctionPriority->GetLow();

		TiXmlElement *nodeJunctionPriority = new TiXmlElement("priority");
		node->LinkEndChild(nodeJunctionPriority);

		nodeJunctionPriority->SetAttribute("high",high);
		nodeJunctionPriority->SetAttribute("low",low);
	}

	return true;
}
bool OpenDriveXmlWriter::WriteJunctionController (TiXmlElement *node, Junction* junction)
{
	string id;
	string type;

	unsigned int junctionControllerCount = junction->GetJunctionControllerCount();
	for(unsigned int i=0; i<junctionControllerCount; i++)
	{
		JunctionController *lJunctionController = junction->GetJunctionController(i);

		id = lJunctionController->GetId();
		type = lJunctionController->GetType();

		TiXmlElement *nodeJunctionController = new TiXmlElement("controller");
		node->LinkEndChild(nodeJunctionController);

		nodeJunctionController->SetAttribute("id",id);
		nodeJunctionController->SetAttribute("type",type);
	}
	return true;
}


//---------------------------------------------------------------------------

/**
 * Writes the data from the OpenDrive structure to a file
 */
bool OpenDriveXmlWriter::WriteFile(std::string fileName)
{
	// XML document
	TiXmlDocument doc;

	TiXmlDeclaration* decl = new TiXmlDeclaration( "1.0", "", "" );  
	doc.LinkEndChild( decl );

	TiXmlElement *rootNode = new TiXmlElement("OpenDRIVE");
	doc.LinkEndChild(rootNode);

	// Write header
	WriteHeader(rootNode);

	// Write roads
	unsigned int roadCount = mOpenDrive->GetRoadCount();
	for(unsigned int i=0; i<roadCount; i++)
	{
		WriteRoad(rootNode, mOpenDrive->GetRoad(i));
	}

	// Write junctions
	unsigned int junctionCount = mOpenDrive->GetJunctionCount();
	for(unsigned int i=0; i<junctionCount; i++)
	{
		WriteJunction(rootNode, mOpenDrive->GetJunction(i));
	}

	// Saves the XML structure to the file
	doc.SaveFile( fileName ); 

	return true;

}
//--------------