modularization/src/driver/driver_map_xodrload/main.cpp

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

#include <QFile>

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

#include "globalplan.h"

#include "gpsimu.pb.h"

#include "mapdata.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"

#include <signal.h>

#include "service_roi_xodr.h"
#include "ivservice.h"

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

double glat0,glon0,ghead0;

double gvehiclewidth = 2.0;

bool gbExtendMap = true;

static bool gbSideEnable = false;

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

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

static QCoreApplication * gApp;

service_roi_xodr gsrx;


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;

    gsrx.SetXODR(&mxodr);
}

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;
    float 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((int)(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,0,data->ins_heading_angle,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 CalcSide(std::vector<PlanPoint> & xPlan)
{
    const double fsidedis = 0.3;
    const int nChangePoint = 150;
    const int nStopPoint = 50;
    if(xPlan.size()<nChangePoint)return;
    bool bChange = true;
    int i;
    int nsize = xPlan.size();
    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
    {
        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
        {
            std::cout<<" Because Lane is narrow, not change."<<std::endl;
            bChange = false;
            break;
        }
    }

    if(bChange == false)return;

    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
    {
        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
        double xold = xPlan[i].x;
        double yold = xPlan[i].y;
        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
    }

    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
    {
        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
        double xold = xPlan[i].x;
        double yold = xPlan[i].y;
        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
    }
    return;
}

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(gbSideEnable)
    {
        CalcSide(xPlan);
    }

    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;
        if(gbExtendMap)
        {
            xPlan.push_back(pp);
        }

    }
    

   iv::map::tracemap xtrace;

    nSize = xPlan.size();

    std::vector<iv::GPSData> mapdata;
    //maptrace
    QFile xfile;
    QString strpath;
    strpath = getenv("HOME");
    strpath = strpath + "/map/maptrace.txt";
    xfile.setFileName(strpath);
    xfile.open(QIODevice::ReadWrite);

    //maptrace_add
    QFile xfile_1;
    QString strpath_1;
    strpath_1 = getenv("HOME");
    strpath_1 = strpath_1 + "/map/mapadd.txt";
    xfile_1.setFileName(strpath_1);
    xfile_1.open(QIODevice::ReadWrite);

    for(i=0;i<nSize;i++)
    {
        iv::map::mappoint * pmappoint =  xtrace.add_point();
        double gps_lon,gps_lat,gps_lon_avoidleft,gps_lat_avoidleft,gps_lat_avoidright,gps_lon_avoidright,gps_heading;
        CalcLatLon(glat0,glon0,ghead0,xPlan[i].x,xPlan[i].y,xPlan[i].hdg,gps_lat,
                  gps_lon,gps_heading);
        CalcLatLon(glat0,glon0,xPlan[i].mx_left,xPlan[i].my_left,
                   gps_lat_avoidleft,gps_lon_avoidleft);
        CalcLatLon(glat0,glon0,xPlan[i].mx_right,xPlan[i].my_right,
                   gps_lat_avoidright,gps_lon_avoidright);

        pmappoint->set_gps_lat(gps_lat);
        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
        pmappoint->set_gps_lat_avoidright(gps_lat_avoidright);
        pmappoint->set_gps_lng(gps_lon);
        pmappoint->set_gps_lat_avoidleft(gps_lat_avoidleft);
        pmappoint->set_gps_lng_avoidright(gps_lon_avoidright);
        pmappoint->set_ins_heading_angle(gps_heading);

        double gps_x,gps_y,gps_x_avoidleft,gps_y_avoidleft,gps_x_avoidright,gps_y_avoidright;

        GaussProjCal(gps_lon,gps_lat,&gps_x,&gps_y);
        GaussProjCal(gps_lon_avoidleft,gps_lat_avoidleft,&gps_x_avoidleft,&gps_y_avoidleft);
        GaussProjCal(gps_lon_avoidright,gps_lat_avoidright,&gps_x_avoidright,&gps_y_avoidright);

        pmappoint->set_gps_x(gps_x);
        pmappoint->set_gps_x_avoidleft(gps_x_avoidleft);
        pmappoint->set_gps_x_avoidright(gps_x_avoidright);
        pmappoint->set_gps_y(gps_y);
        pmappoint->set_gps_y_avoidleft(gps_y_avoidleft);
        pmappoint->set_gps_y_avoidright(gps_y_avoidright);

        pmappoint->set_speed(xPlan[i].speed);
        pmappoint->set_index(i);

        pmappoint->set_roadori(xPlan[i].mnLaneori);
        pmappoint->set_roadsum(xPlan[i].mnLaneTotal);
        pmappoint->set_mfdistolaneleft(xPlan[i].mfDisToLaneLeft);
        pmappoint->set_mfdistoroadleft(xPlan[i].mfDisToRoadLeft);
        pmappoint->set_mflanewidth(xPlan[i].mWidth);
        pmappoint->set_mfroadwidth(xPlan[i].mfRoadWidth);
        pmappoint->set_mbinlaneavoid(xPlan[i].bInlaneAvoid);


        iv::GPSData data(new iv::GPS_INS);
        data->gps_lat = gps_lat;
        data->gps_lat_avoidleft = gps_lat_avoidleft;
        data->gps_lat_avoidright = gps_lat_avoidright;
        data->gps_lng = gps_lon;
        data->gps_lng_avoidleft = gps_lon_avoidleft;
        data->gps_lng_avoidright =  gps_lon_avoidright;
        data->ins_heading_angle = gps_heading;
        data->gps_x = gps_x;
        data->gps_x_avoidleft = gps_x_avoidleft;
        data->gps_x_avoidright = gps_x_avoidright;
        data->gps_y = gps_y;
        data->gps_y_avoidleft = gps_y_avoidleft;
        data->gps_y_avoidright = gps_y_avoidright;
        pmappoint->set_mbnoavoid(xPlan[i].mbNoavoid);
        pmappoint->set_mfcurvature(xPlan[i].mfCurvature);


//        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;
            pmappoint->set_roadori(0);
            pmappoint->set_roadsum(2);
        }


        data->mbnoavoid = xPlan[i].mbNoavoid;
        if(data->mbnoavoid)
        {
            qDebug("no avoid i = %d",i);
        }
        data->mfCurvature = xPlan[i].mfCurvature;

        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;
                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=3000))
                {
                 continue;
                }
                mapdata.at(k)->mode2 = 3000;
            }
         }
        if(data->mode2 == 1000001)
        {
            int k;
            for(k=(mapdata.size()-1);k>(mapdata.size()-10);k--)
            {
                if(k<0)break;
                if((mapdata.at(k)->mode2 != -1)&&(mapdata.at(k)->mode2 !=1000001))
                {
                 continue;
                }
                mapdata.at(k)->mode2 = 1000001;
            }
         }

       pmappoint->set_mode2(data->mode2);
       pmappoint->set_rel_x(xPlan[i].x);
       pmappoint->set_rel_y(xPlan[i].y);
       pmappoint->set_rel_z(0);
       pmappoint->set_rel_yaw(xPlan[i].hdg);
       pmappoint->set_laneid(-1);
       pmappoint->set_roadid(xPlan[i].nRoadID);

#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

        mapdata.push_back(data);




//        char strline[255];
//  //      snprintf(strline,255,"%d\t%11.7f\t%11.7f\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%11.7f\n",
//  //               i,data->gps_lng,data->gps_lat,0,data->ins_heading_angle,0,0,0,data->roadOri,data->roadSum,data->mode2,data->mfCurvature);
//              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\t%d\n",
//                      i,data->gps_lng,data->gps_lat,0,0,data->ins_heading_angle,0,0,data->roadSum,data->roadOri,data->mode2);
//        xfile.write(strline);
    }


    for(int j=0;j<nSize;j++)
    {
        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",
                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri);
        xfile.write(strline);
     //mapttrace1
        char strline_1[255];
        snprintf(strline_1,255,"%d\t%11.7f\t%11.7f\t%d\t%d\t%11.3f\t%d\t%d\t%d\t%d\t%d\n",
                      j,mapdata.at(j)->gps_lng,mapdata.at(j)->gps_lat,0,0,mapdata.at(j)->ins_heading_angle,0,0,mapdata.at(j)->roadSum,mapdata.at(j)->roadOri,mapdata.at(j)->mode2);
        xfile_1.write(strline_1);
    }

    xfile.close();
    xfile_1.close();
    ShareMap(mapdata);

    int nnewmapsize = xtrace.ByteSize();
    std::shared_ptr<char> str_ptr = std::shared_ptr<char>(new char[nnewmapsize]);
    if(xtrace.SerializeToArray(str_ptr.get(),nnewmapsize))
    {
        iv::modulecomm::ModuleSendMsg(gpanewtrace ,str_ptr.get(),nnewmapsize);
    }
    else
    {
        std::cout<<" new trace map serialize fail."<<std::endl;
    }




    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;
}

void ExitFunc()
{
//    gApp->quit();
    iv::modulecomm::Unregister(gpasimple);
    iv::modulecomm::Unregister(gpav2x);
    iv::modulecomm::Unregister(gpagps);
    iv::modulecomm::Unregister(gpasrc);
    iv::modulecomm::Unregister(gpmap);
    iv::modulecomm::Unregister(gpa);
    std::cout<<"driver_map_xodrload exit."<<std::endl;
    gApp->quit();
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
//    std::this_thread::sleep_for(std::chrono::milliseconds(900));
}

void signal_handler(int sig)
{
    if(sig == SIGINT)
    {
        ExitFunc();
    }
}

void ProcROIReq(std::shared_ptr<char> pstr_req,const int nreqsize,std::shared_ptr<char> & pstr_res,int & nressize)
{
    (void)pstr_req;
    (void)nreqsize;
    std::shared_ptr<iv::roi::request> pstr_roireq = std::shared_ptr<iv::roi::request>(new iv::roi::request);
    if(pstr_roireq->ParseFromArray(pstr_req.get(),nreqsize))
    {
        std::shared_ptr<iv::roi::resultarray> pstr_roires;
        gsrx.GetROI(pstr_roireq,pstr_roires);
        int nbytesize = pstr_roires->ByteSize();
        pstr_res = std::shared_ptr<char>(new char[nbytesize]);
        if(pstr_roires->SerializeToArray(pstr_res.get(),nbytesize))
        {
            nressize = nbytesize;
            std::cout<<"return res."<<std::endl;
            return;
        }
        else
        {
            std::cout<<" ProcROIReq fail serizlizetoarray"<<std::endl;
        }
    }
    else
    {
        std::cout<<" ProcROIReq parse pstr_req fail."<<std::endl;
        return;
    }
}

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

    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 = "./driver_map_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");

    std::string strextendmap = xp.GetParam("extendmap","false");

    std::string strsideenable = xp.GetParam("sideenable","true");


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

    gvehiclewidth = atof(strvehiclewidth.data());

    if(strextendmap == "true")
    {
        gbExtendMap = true;
    }
    else
    {
        gbExtendMap = false;
    }

    if(strsideenable == "true")
    {
        gbSideEnable = true;
    }


    LoadXODR(strmapth);

    iv::service::Server serviceroi("ServiceROI",ProcROIReq);
    (void)serviceroi;

    gpmap = iv::modulecomm::RegisterSend("tracemap",20000000,1);
    gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
    gpanewtrace = iv::modulecomm::RegisterSend("newtracemap",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);




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

    signal(SIGINT,signal_handler);

    int nrc = a.exec();

    std::cout<<"driver_map_xodr app exit. code :"<<nrc<<std::endl;

    return nrc;
}