modularization/src/driver/driver_map_xodrload/main.cpp

#include <QCoreApplication>
#include <math.h>
#include <string>

#include <QFile>

#include "OpenDrive/OpenDrive.h"
#include "OpenDrive/OpenDriveXmlParser.h"

#include "globalplan.h"

#include "gpsimu.pb.h"

#include "v2x.pb.h"

#include "modulecomm.h"

#include "xmlparam.h"

#include "gps_type.h"

#include "xodrdijkstra.h"

#include "gnss_coordinate_convert.h"

#include "ivexit.h"

#include "ivversion.h"

#include "ivbacktrace.h"

OpenDrive mxodr;
xodrdijkstra * gpxd;
bool gmapload = false;

double glat0,glon0,ghead0;

double gvehiclewidth = 2.0;

void * gpa;
void * gpasrc;
void * gpmap;
void * gpagps;
void * gpasimple;
void * gpav2x;

iv::Ivfault *gfault = nullptr;
iv::Ivlog *givlog = nullptr;


namespace iv {
struct simpletrace
{
    double gps_lat = 0;//纬度
    double gps_lng = 0;//经度

    double gps_x = 0;
    double gps_y = 0;
    double gps_z = 0;


    double ins_heading_angle = 0;	//航向角
};

}
/**
 *
 *
 *
 *
 * */
bool LoadXODR(std::string strpath)
{
    OpenDriveXmlParser xp(&mxodr);
    xp.ReadFile(strpath);
    std::cout<<"road cout is "<<mxodr.GetRoadCount()<<std::endl;
    if(mxodr.GetRoadCount() < 1)
    {
        gmapload = true;
        return false;
    }


    xodrdijkstra * pxd = new xodrdijkstra(&mxodr);
    gpxd = pxd;
//    std::vector<int> xpath = pxd->getpath(10001,2,30012,2);

//    pxd->getgpspoint(10001,2,30012,2,xpath);

    int i;
    double  nlenth = 0;
    int nroadsize = mxodr.GetRoadCount();
    for(i=0;i<nroadsize;i++)
    {
        Road * px = mxodr.GetRoad(i);
        nlenth = nlenth + px->GetRoadLength();
        int bloksize = px->GetGeometryBlockCount();
        if(px->GetGeometryBlock(0)->GetGeometryAt(0)->GetGeomType() == 4)
        {
            GeometryParamPoly3 * p = (GeometryParamPoly3 *) px->GetGeometryBlock(0)->GetGeometryAt(0);
            double a = p->GetuA();
            a = p->GetuB();
            a = p->GetuC();
            a = p->GetuD();
            a = p->GetvA();
        }


    }

//    void Header::GetAllParams(unsigned short int &revMajor, unsigned short int &revMinor, string &name, float &version, string &date,
//                              double &north, double &south, double &east,double &west,double &lat0,double &lon0, double & hdg0)


    unsigned short int revMajor,revMinor;
    std::string name,date;
    float version;
    double north,south,east,west,lat0,lon0,hdg0;
    if(mxodr.GetHeader() != 0)
    {
        mxodr.GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
        glat0 = lat0;
        glon0 = lon0;
    }



    Road * proad1 = mxodr.GetRoad(0);
    givlog->info("road name is %s",proad1->GetRoadName().data());
    std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
}

class roadx
{
  public:
    roadx * para;
    std::vector<roadx> child;
    int nlen;
};


#define EARTH_RADIUS 6370856.0

//从1到2的距离和方向
int CalcDisAngle(double lat1,double lon1,double lat2,double lon2,double * pLatDis,double * pLonDis,double * pDis,double * pangle)
{
    double a,b;
    double LonDis,LatDis;
    double LonRadius;
    double Dis;
    double angle;
    if((lat1 == lat2)&&(lon1 == lon2))return -1;

    LonRadius = EARTH_RADIUS * cos(lat2/(180.0/M_PI));
    a = (EARTH_RADIUS * M_PI*2.0/360.0)/(100000.0);
    b = lat2 - lat1; b = b*100000.0;
    LatDis = a*b;
    a = (LonRadius * M_PI*2.0/360.0)/100000.0;
    b = lon2 - lon1; b = b*100000.0;
    LonDis = a*b;
    Dis = sqrt(LatDis*LatDis + LonDis *LonDis);
    angle = acos(fabs(LonDis)/Dis);
    angle = angle * 180.0/M_PI;

    if(LonDis > 0)
    {
        if(LatDis > 0)angle = 90.0 - angle;
        else angle= 90.0+angle;
    }
    else
    {
        if(LatDis > 0)angle = 270.0+angle;
        else angle = 270.0-angle;
    }

    if(pLatDis != 0)*pLatDis = LatDis;
    if(pLonDis != 0)*pLonDis = LonDis;
    if(pDis != 0)*pDis = Dis;
    if(pangle != 0)*pangle = angle;
}



//==========================================================

//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
//void GaussProjCal(double longitude, double latitude, double *X, double *Y)
//{
//    int ProjNo = 0; int ZoneWide; ////带宽
//    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
//    double a, f, e2, ee, NN, T, C, A, M, iPI;
//    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
//    ZoneWide = 6; ////6度带宽
//    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
//                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
//    ProjNo = (int)(longitude / ZoneWide);
//    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
//    longitude0 = longitude0 * iPI;
//    latitude0 = 0;
//    longitude1 = longitude * iPI; //经度转换为弧度
//    latitude1 = latitude * iPI; //纬度转换为弧度
//    e2 = 2 * f - f * f;
//    ee = e2 * (1.0 - e2);
//    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
//    T = tan(latitude1)*tan(latitude1);
//    C = ee * cos(latitude1)*cos(latitude1);
//    A = (longitude1 - longitude0)*cos(latitude1);
//    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
//        *e2 / 1024)*sin(2 * latitude1)
//        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
//    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
//    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
//        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
//    X0 = 1000000L * (ProjNo + 1) + 500000L;
//    Y0 = 0;
//    xval = xval + X0; yval = yval + Y0;
//    *X = xval;
//    *Y = yval;
//}

#include <math.h>
static void OffLngLat(double fLat0,double fLng0,double & fLat,double & fLng,double fHeading,double x,double y)
{
    double fxdiff,fydiff;

    double xoff = y*(-1);
    double yoff = x;

    fxdiff = 0+xoff * cos(fHeading*M_PI/180.0)+ yoff*sin(fHeading*M_PI/180.0);  //East
    fydiff = 0-xoff * sin(fHeading*M_PI/180.0)+ yoff*cos(fHeading*M_PI/180.0);  //South

    double fEarthRadius = 6378245.0;

    double ns1d = fEarthRadius*2*M_PI/360.0;
    double fewRadius = fEarthRadius * cos(fLat0*M_PI/180.0);
    double ew1d = fewRadius * 2*M_PI/360.0;

    fLat = fLat0 + fydiff/ns1d;
    fLng = fLng0 + fxdiff/ew1d;


}

void CalcXY(const double lat0,const double lon0,const double hdg0,
              const double lat,const double lon,
              double & x,double & y)
{

    double x0,y0;
    GaussProjCal(lon0,lat0,&x0,&y0);
    GaussProjCal(lon,lat,&x,&y);
    x = x - x0;
    y = y-  y0;

//    double ang,dis;
//    CalcDisAngle(lat0,lon0,lat,lon,0,0,&dis,&ang);
//    double xang = hdg0 - ang;
//    while(xang<0)xang = xang + 360.0;
//    x = dis * cos(xang * M_PI/180.0);
//    y = dis * sin(xang * M_PI/180.0);
}

//void CalcLatLon(const double lat0,const double lon0,const double hdg0,
//                const double x,const double y,const double xyhdg,
//                double &lat,double & lon, double & hdg)
//{
//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
//    hdg = hdg0 - xyhdg * 180.0/M_PI;
//    while(hdg < 0)hdg = hdg + 360;
//    while(hdg >= 360)hdg = hdg - 360;
//}


void CalcLatLon(const double lat0,const double lon0,
                const double x,const double y,
                double &lat,double & lon)
{
    double x0,y0;
    GaussProjCal(lon0,lat0,&x0,&y0);
    double x_gps,y_gps;
    x_gps = x0+x;
    y_gps = y0+y;
    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
}


void CalcLatLon(const double lat0,const double lon0,const double hdg0,
                const double x,const double y,const double xyhdg,
                double &lat,double & lon, double & hdg)
{
    double x0,y0;
    GaussProjCal(lon0,lat0,&x0,&y0);
    double x_gps,y_gps;
    x_gps = x0+x;
    y_gps = y0+y;
    GaussProjInvCal(x_gps,y_gps,&lon,&lat);
 //   hdg = hdg0 -xyhdg * 270/M_PI;
    hdg = 90 -  xyhdg* 180.0/M_PI;

//    OffLngLat(lat0,lon0,lat,lon,hdg0,x,y);
//    hdg = hdg0 - xyhdg * 180.0/M_PI;
    while(hdg < 0)hdg = hdg + 360;
    while(hdg >= 360)hdg = hdg - 360;
}

class xodrobj
{
public:
    double flatsrc;
    double flonsrc;
    double fhgdsrc;
    double flat;
    double flon;
    int lane;
};

xodrobj gsrc;

void ShareMap(std::vector<iv::GPSData> navigation_data)
{
    if(navigation_data.size()<1)return;
    iv::GPS_INS x;
    x = *(navigation_data.at(0));
    char * data = new char[sizeof(iv::GPS_INS)*navigation_data.size()];
    int gpssize = sizeof(iv::GPS_INS);
    int i;
    for(i=0;i<navigation_data.size();i++)
    {
        x = *(navigation_data.at(i));
        memcpy(data+i*gpssize,&x,gpssize);
    }

    iv::modulecomm::ModuleSendMsg(gpmap,data,navigation_data.size()*gpssize);


    int nsize = 100;
    int nstep = 1;
    if(navigation_data.size() < 100)
    {
        nsize = navigation_data.size();

    }
    else
    {
        nstep = navigation_data.size()/100;
    }

    iv::simpletrace psim[100];
    for(i=0;i<nsize;i++)
    {
        x = *(navigation_data.at(i*nstep));
        psim[i].gps_lat = x.gps_lat;
        psim[i].gps_lng = x.gps_lng;
        psim[i].gps_z = x.gps_z;
        psim[i].gps_x = x.gps_x;
        psim[i].gps_y = x.gps_y;
        psim[i].ins_heading_angle = x.ins_heading_angle;
    }
    if(navigation_data.size()>100)
    {
        int nlast = 99;
        x = *(navigation_data.at(navigation_data.size()-1));
        psim[nlast].gps_lat = x.gps_lat;
        psim[nlast].gps_lng = x.gps_lng;
        psim[nlast].gps_z = x.gps_z;
        psim[nlast].gps_x = x.gps_x;
        psim[nlast].gps_y = x.gps_y;
        psim[nlast].ins_heading_angle = x.ins_heading_angle;
    }

    iv::modulecomm::ModuleSendMsg(gpasimple,(char *)psim,nsize * sizeof(iv::simpletrace));

    delete data;
}

static void ToGPSTrace(std::vector<PlanPoint> xPlan)
{
//    double x_src,y_src,x_dst,y_dst;



//    x_src = -26;y_src = 10;
//    x_dst = -50;y_dst = -220;

    int i;
    int nSize = xPlan.size();

    std::vector<iv::GPSData> mapdata;


    QFile xfile;
    QString strpath;
    strpath = getenv("HOME");
    strpath = strpath + "/map/maptrace.txt";
    xfile.setFileName(strpath);
    bool bFileOpen = xfile.open(QIODevice::ReadWrite);

    for(i=0;i<nSize;i++)
    {
        iv::GPSData data(new iv::GPS_INS);
        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
                   data->gps_lng,data->ins_heading_angle);

        data->index = i;
        data->speed = xPlan[i].speed;
        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);


        givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
               data->gps_lng,data->ins_heading_angle);

        data->roadSum = xPlan[i].mnLaneTotal;
        data->roadMode = 0;
        data->roadOri = xPlan[i].mnLaneori;

        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
        data->mfLaneWidth = xPlan[i].mWidth;
        data->mfRoadWidth = xPlan[i].mfRoadWidth;
        data->mnLaneChangeMark = xPlan[i].lanmp;

        if(xPlan[i].lanmp == -1)data->roadMode = 15;
        if(xPlan[i].lanmp == 1)data->roadMode = 14;

        mapdata.push_back(data);

        char strline[255];
        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
        if(bFileOpen)  xfile.write(strline);
    //                             fout << gps_index << "\t" << data->gps_lng << "\t" << data->gps_lat << "\t" << ServiceCarStatus.location->speed_mode << "\t" << ServiceCarStatus.location->mode2 << "\t" << data->ins_heading_angle << "\t" << obs_modes << "\t" << speed_modes << "\t" << lane_num << "\t" << lane_status <<"\t" <<road_width <<std::endl;

    }
    if(bFileOpen)xfile.close();

    ShareMap(mapdata);
}

int avoidroadid[] = {10002,10019,10003,10098,10099,10063,10099,10100,10104,10110,10111};

inline bool isboringroad(int nroadid)
{
    int i;
    bool brtn = false;
    for(i=0;i<11;i++)
    {
        if(avoidroadid[i] == nroadid)
        {
            brtn = true;
            break;
        }
    }
    return brtn;
}

void SetPlan(xodrobj xo)
{

    double x_src,y_src,x_dst,y_dst;

    CalcXY(glat0,glon0,ghead0,xo.flatsrc,xo.flonsrc,x_src,y_src);
    CalcXY(glat0,glon0,ghead0,xo.flat,xo.flon,x_dst,y_dst);


    std::vector<PlanPoint> xPlan;

    double s;

//    x_src = -5;y_src = 6;
//    x_dst = -60;y_src = -150;
    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - xo.fhgdsrc)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,xo.lane,xPlan);

    if(nRtn < 0)
    {
        qDebug("plan fail.");
        return;
    }

    int i;
    int nSize = xPlan.size();

    if(nSize<1)
    {
        qDebug("plan fail.");
        return;
    }
    PlanPoint xLastPoint = xPlan[nSize -1];
    for(i=0;i<600;i++)
    {
        PlanPoint pp = xLastPoint;
        double fdis = 0.1*(i+1);
        pp.mS = pp.mS + i*0.1;
        pp.x = pp.x + fdis * cos(pp.hdg);
        pp.y = pp.y + fdis * sin(pp.hdg);
        pp.nSignal = 23;
//        xPlan.push_back(pp);

    }

    nSize = xPlan.size();

    std::vector<iv::GPSData> mapdata;

    QFile xfile;
    QString strpath;
    strpath = getenv("HOME");
    strpath = strpath + "/map/maptrace.txt";
    xfile.setFileName(strpath);
    xfile.open(QIODevice::ReadWrite);

    for(i=0;i<nSize;i++)
    {
        iv::GPSData data(new iv::GPS_INS);
        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
                   data->gps_lng,data->ins_heading_angle);
        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
                   data->gps_lat_avoidleft,data->gps_lng_avoidleft);
        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
                   data->gps_lat_avoidright,data->gps_lng_avoidright);
        data->index = i;
        data->speed = xPlan[i].speed;
 //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
        GaussProjCal(data->gps_lng_avoidleft,data->gps_lat_avoidleft,&data->gps_x_avoidleft,&data->gps_y_avoidleft);
        GaussProjCal(data->gps_lng_avoidright,data->gps_lat_avoidright,&data->gps_x_avoidright,&data->gps_y_avoidright);

        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
               data->gps_lng,data->ins_heading_angle);

        data->roadOri = xPlan[i].mnLaneori;
        data->roadSum = xPlan[i].mnLaneTotal;
        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
        data->mfLaneWidth = xPlan[i].mWidth;
        data->mfRoadWidth = xPlan[i].mfRoadWidth;


        data->mbInLaneAvoid = xPlan[i].bInlaneAvoid;

        if(xPlan[i].mbBoringRoad)
        {
            data->roadOri = 0;
            data->roadSum = 2;
        }

        data->mode2 = xPlan[i].nSignal;
        if(data->mode2 == 3000)
        {
            int k;
            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
            {
                if(k<0)break;
                mapdata.at(k)->mode2 = 3000;
            }
        }

#ifdef BOAVOID
    if(isboringroad(xPlan[i].nRoadID))
    {
        const int nrangeavoid = 300;
         if((i+(nrangeavoid + 10))<nSize)
         {
             double fhdg1 = xPlan[i].hdg;
             bool bavoid = true;
//             int k;
//             for(k=0;k<=10;k++)
//             {
//                 double fhdg5 = xPlan[i+nrangeavoid*k/10].hdg;
//                 double fhdgdiff1 = fhdg5 - fhdg1;
//                 while(fhdgdiff1<0)fhdgdiff1 = fhdgdiff1 + 2.0*M_PI;
//                 if((fhdgdiff1>(M_PI/3.0))&&(fhdgdiff1<(5.0*M_PI/3.0)))
//                 {
//                     bavoid = false;
//                     break;
//                 }

//             }
             if(bavoid)
             {
                 data->roadSum = 2;
                 data->roadOri = 0;
             }

         }
         else
         {
             int a = 1;
             a++;
         }
    }
#endif

//        data->roadSum = 1;
//        data->roadMode = 0;
//        data->roadOri = 0;

//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
//        if(xPlan[i].lanmp == 1)data->roadMode = 14;

        mapdata.push_back(data);

        char strline[255];
        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum);
        xfile.write(strline);

    }

    xfile.close();

    ShareMap(mapdata);




    s = 1;
}

void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double fsrcheading,int nlane)
{

    std::vector<PlanPoint> xPlan;

    int i;

    double fLastHdg = 0;

    int ndeflane =nlane;

    for(i=0;i<pxv2x->stgps_size();i++)
    {

        double x_src,y_src,x_dst,y_dst;

        if(i==0)
        {
            CalcXY(glat0,glon0,ghead0,fsrclat,fsrclon,x_src,y_src);
        }
        else
        {
            CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i-1).lat(),pxv2x->stgps(i-1).lon(),x_src,y_src);
        }
        CalcXY(glat0,glon0,ghead0,pxv2x->stgps(i).lat(),pxv2x->stgps(i).lon(),x_dst,y_dst);


        std::vector<PlanPoint> xPlanPart;

        double s;

        //    x_src = -5;y_src = 6;
        //    x_dst = -60;y_src = -150;
        int nRtn = -1;
        if(i==0)
        {
            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,(90 - fsrcheading)*M_PI/180.0,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
        }
        else
        {
            nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,fLastHdg,x_dst,y_dst,s,30.0,100.0,ndeflane,xPlanPart);
        }



        if(nRtn < 0)
        {
            qDebug("plan fail.");
            return;
        }

        int j;
        for(j=0;j<xPlanPart.size();j++)xPlan.push_back(xPlanPart.at(j));
        fLastHdg = xPlanPart.at(xPlanPart.size()-1).hdg;

    }



    int nSize = xPlan.size();

    std::vector<iv::GPSData> mapdata;

    QFile xfile;
    QString strpath;
    strpath = getenv("HOME");
    strpath = strpath + "/map/maptrace.txt";
    xfile.setFileName(strpath);
    xfile.open(QIODevice::ReadWrite);

    for(i=0;i<nSize;i++)
    {
        iv::GPSData data(new iv::GPS_INS);
        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,data->gps_lat,
                   data->gps_lng,data->ins_heading_angle);
        data->index = i;
        data->speed = xPlan[i].speed;
 //       ZBGaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);
        GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);

        givlog->verbose(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
               data->gps_lng,data->ins_heading_angle);

//        data->roadSum = 1;
//        data->roadMode = 0;
//        data->roadOri = 0;

//        if(xPlan[i].lanmp == -1)data->roadMode = 15;
//        if(xPlan[i].lanmp == 1)data->roadMode = 14;
        data->roadOri = xPlan[i].mnLaneori;
        data->roadSum = xPlan[i].mnLaneTotal;
        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
        data->mfLaneWidth = xPlan[i].mWidth;
        data->mfRoadWidth = xPlan[i].mfRoadWidth;

        data->mode2 = xPlan[i].nSignal;
        if(data->mode2 == 3000)
        {
            int k;
            for(k=(mapdata.size()-1);k>(mapdata.size()-150);k--)
            {
                if(k<0)break;
                mapdata.at(k)->mode2 = 3000;
            }
        }

        mapdata.push_back(data);

        char strline[255];
        snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\n",
                 i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,0,0);
        xfile.write(strline);

    }

    xfile.close();

    ShareMap(mapdata);

}

void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    if(nSize<sizeof(xodrobj))
    {
        std::cout<<"ListenCmd small."<<std::endl;
        return;
    }

    xodrobj xo;
    memcpy(&xo,strdata,sizeof(xodrobj));

    givlog->debug("lat is %f", xo.flat);

    xo.fhgdsrc = gsrc.fhgdsrc;
    xo.flatsrc = gsrc.flatsrc;
    xo.flonsrc = gsrc.flonsrc;

    SetPlan(xo);


}

void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::v2x::v2x xv2x;
    if(!xv2x.ParseFromArray(strdata,nSize))
    {
        givlog->warn("ListernV2X Parse Error.");
        std::cout<<"ListenV2X Parse Error."<<std::endl;
        return;
    }
    if(xv2x.stgps_size()<1)
    {
        givlog->debug("ListenV2X no gps station.");
        std::cout<<"ListenV2X no gps station."<<std::endl;
        return;
    }

    MultiStationPlan(&xv2x,gsrc.flatsrc,gsrc.flonsrc,gsrc.fhgdsrc,1);
}

void ListenSrc(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    if(nSize<sizeof(xodrobj))
    {
        givlog->warn("ListenSrc small");
        std::cout<<"ListenSrc small."<<std::endl;
        return;
    }

    memcpy(&gsrc,strdata,sizeof(xodrobj));

    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;

}

void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    iv::gps::gpsimu  xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
        return;
    }

    xodrobj xo;
    xo.fhgdsrc = xgpsimu.heading();
    xo.flatsrc = xgpsimu.lat();
    xo.flonsrc = xgpsimu.lon();

    gsrc = xo;

    givlog->debug("src hdg is %f", gsrc.fhgdsrc);
    std::cout<<"src hdg is "<<gsrc.fhgdsrc<<std::endl;
}

int main(int argc, char *argv[])
{
    showversion("driver_map_xodrload");
    QCoreApplication a(argc, argv);

    RegisterIVBackTrace();

    gfault = new iv::Ivfault("driver_map_xodrload");
    givlog = new iv::Ivlog("driver_map_xodrload");

    std::string strmapth,strparapath;
    if(argc<3)
    {
//        strmapth = "./map.xodr";
        strmapth = getenv("HOME");
        strmapth = strmapth + "/map/map.xodr";
//        strmapth = "/home/yuchuli/1226.xodr";
        strparapath = "./ADCIntelligentVehicle-xodrload.xml";
    }
    else
    {
        strmapth = argv[1];
        strparapath = argv[2];
    }







    iv::xmlparam::Xmlparam xp(strparapath);
    xp.GetParam(std::string("he"),std::string("1"));
    std::string strlat0 = xp.GetParam("lat0","39");
    std::string strlon0 = xp.GetParam("lon0","117.0");
    std::string strhdg0 = xp.GetParam("hdg0","360.0");

    std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");

    std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");

    std::string strvehiclewidth = xp.GetParam("vehiclewidth","2.0");


    glat0 = atof(strlat0.data());
    glon0 = atof(strlon0.data());
    ghead0 = atof(strhdg0.data());

    gvehiclewidth = atof(strvehiclewidth.data());


    LoadXODR(strmapth);

    gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
    gpa = iv::modulecomm::RegisterRecv("xodrreq",ListenCmd);
    gpasrc =iv::modulecomm::RegisterRecv("xodrsrc",ListenSrc);
    gpagps = iv::modulecomm::RegisterRecv(strgpsmsg.data(),UpdateGPSIMU);
    gpav2x = iv::modulecomm::RegisterRecv(strv2xmsg.data(),ListenV2X);

    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);


    double x_src,y_src,x_dst,y_dst;

    x_src = -26;y_src = 10;
    x_dst = -50;y_dst = -220;

    x_src = 0;y_src = 0;
    x_dst = -23;y_dst = -18;
    x_dst = 21;y_dst =-21;
    x_dst =5;y_dst = 0;

    x_src = -20; y_src = -1000;
    x_dst = 900; y_dst = -630;

//    x_dst = 450; y_dst = -640;
//    x_dst = -190; y_dst = -690;

//    x_src = 900; y_src = -610;
//    x_dst = -100; y_dst = -680;
    std::vector<PlanPoint> xPlan;
    double s;
//    int nRtn = MakePlan(gpxd,&mxodr,x_src,y_src,3.14,x_dst,y_dst,s,30.0,100.0,1,xPlan);

//    ToGPSTrace(xPlan);


//    double lat = 39.1443880;
//    double lon = 117.0812543;

//    xodrobj xo;
//    xo.fhgdsrc = 340;
//    xo.flatsrc = lat; xo.flonsrc = lon;
//    xo.flat = 39.1490196;
//    xo.flon = 117.0806979;
//    xo.lane = 1;
//    SetPlan(xo);


    void * pivexit = iv::ivexit::RegIVExitCall(0);

    return a.exec();
}