change map_lanetoxodr. add some nds data to xodr code, loading data is ok.

src/tool/map_lanetoxodr/main.cpp

@@ -90,6 +90,8 @@ int main(int argc, char *argv[])
     w.show();
     w.resize(1280,800);
 
+
+
     qDebug("win id : %d",w.winId());
 
     return a.exec();

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -9,6 +9,8 @@
 
 #include "gnss_coordinate_convert.h"
 
+#include "ndsdataproc.h"
+
 #define VIEW_WIDTH 10000
 #define VIEW_HEIGHT 10000
 
@@ -86,6 +88,8 @@ MainWindow::MainWindow(QWidget *parent) :
 
     setWindowTitle("Create Map From Lane Info");
 
+    NDSDataProc::ProcNDSData("/home/yuchuli/下载/nds-sync-line.csv","/home/yuchuli/下载/nds-sync-vehicle.csv");
+
 }
 
 MainWindow::~MainWindow()

src/tool/map_lanetoxodr/ndsdataproc.cpp

@@ -1,6 +1,7 @@
 #include "ndsdataproc.h"
 
 #include <QFile>
+#include "gnss_coordinate_convert.h"
 
 NDSDataProc::NDSDataProc()
 {
@@ -8,7 +9,159 @@ NDSDataProc::NDSDataProc()
 }
 
 
+//-1 can't open line data file
+//-2 can't open vehicle data file
+//-3 no valid line data.
+//-4 no valid vehicle data.
 int NDSDataProc::ProcNDSData(std::string strlinepath,std::string strvehiclepath)
 {
+    QFile xFileline;
+    QFile xFilevehicle;
+    xFileline.setFileName(strlinepath.data());
+    xFilevehicle.setFileName(strvehiclepath.data());
+    if(!xFileline.open(QIODevice::ReadOnly))
+    {
+        return -1;
+    }
+    if(!xFilevehicle.open(QIODevice::ReadOnly))
+    {
+        xFileline.close();
+        return -2;
+    }
+
+
+    std::vector<iv::nds_line> xvectorline;
+    std::vector<iv::nds_vehicle> xvectorvehicle;
+
+    QByteArray ba = xFileline.readAll();
+    QList<QByteArray> baline =ba.split('\n');//x.split(QRegExp("\n ")) ;//ba.split('\n');
+    int nline = baline.size();
+    int i;
+    for(i=1;i<nline;i++)
+    {
+        QString x(baline[i]);
+        x = x.trimmed();
+        //         QList<QByteArray> badata = baline[i].split('\t');
+        QStringList badata = x.split(QRegExp("[,]"));
+
+        if(badata.size()>=36)
+        {
+            iv::nds_line xline;
+            xline.localtime = QString(badata[1]).toLongLong();
+            if(badata[6] == "Normal")xline.feature = 0;
+            if(badata[6] == "Left1")xline.feature = 1;
+            if(badata[6] == "Right1")xline.feature = 2;
+            xline.centerdeparture = badata[8].toDouble();
+            xline.lanewidth = badata[9].toDouble();
+            xline.laneheading = badata[10].toDouble();
+            xline.lanecurv = badata[11].toDouble();
+            if(xline.feature >=0)xvectorline.push_back(xline);
+
+        }
+
+    }
+
+    if(xvectorline.size() < 2)
+    {
+        xFileline.close();
+        xFilevehicle.close();
+        return -3;
+    }
+
+//    int64_t localtime;
+//    int locationmode;
+//    double lon;
+//    double lat;
+//    double height;
+//    double heading;
+    ba = xFilevehicle.readAll();
+    baline =ba.split('\n');//x.split(QRegExp("\n ")) ;//ba.split('\n');
+    nline = baline.size();
+    for(i=1;i<nline;i++)
+    {
+        QString x(baline[i]);
+        x = x.trimmed();
+        //         QList<QByteArray> badata = baline[i].split('\t');
+        QStringList badata = x.split(QRegExp("[,]"));
+
+        if(badata.size()>=75)
+        {
+            iv::nds_vehicle xvehicle;
+            xvehicle.localtime = badata[1].toLongLong();
+            xvehicle.locationmode = badata[4].toInt();
+            xvehicle.lon = badata[5].toDouble();
+            xvehicle.lat = badata[6].toDouble();
+            xvehicle.height = badata[7].toDouble();
+            xvehicle.heading = badata[9].toDouble();
+            if(xvehicle.heading<0)xvehicle.heading = xvehicle.heading + 360;
+            if(xvehicle.locationmode == 2)xvectorvehicle.push_back(xvehicle);
+        }
+
+    }
+
+    xFileline.close();
+    xFilevehicle.close();
+
+    if(xvectorvehicle.size()<2)
+    {
+        return -4 ;
+    }
+
+
+    double flon0,flat0;
+
+    flon0 = xvectorvehicle[0].lon;
+    flat0 = xvectorvehicle[0].lat;
+
+    double x0,y0;
+    GaussProjCal(flon0,flat0,&x0,&y0);
+
+    int j;
+    int nlinecount = xvectorline.size();
+    int nvehiclecount = xvectorvehicle.size();
+
+    for(i=0;i<nvehiclecount;i++)
+    {
+        iv::nds_vehicle * p;
+        p = &xvectorvehicle[i];
+        double x,y;
+        GaussProjCal(p->lon,p->lat,&x,&y);
+        p->frel_x = x - x0;
+        p->frel_y = y - y0;
+    }
+
+    double fS = 0;
+    for(i=1;i<xvectorvehicle.size();i++)
+    {
+        iv::nds_vehicle *p1,*p2;
+        p1 = &xvectorvehicle[i-1];
+        p2 = &xvectorvehicle[i];
+        fS = fS + sqrt(pow(p2->frel_x - p1->frel_x,2)+pow(p2->frel_y - p1->frel_y,2));
+    }
+
+    std::cout<<" s: "<<fS<<std::endl;
+
+    int nlinenow = 0;
+
+    for(i=0;i<nvehiclecount;i++)
+    {
+        if(nlinenow >= nlinecount)
+        {
+            break;
+        }
+
+        while(xvectorvehicle[i].localtime > xvectorline[nlinenow].localtime)
+        {
+            nlinenow++;
+        }
+
+        while(xvectorvehicle[i].localtime == xvectorline[nlinenow].localtime)
+        {
+            std::cout<<" veh: "<<i<<" line: "<<nlinenow<<std::endl;
+            nlinenow++;
+        }
+
+    }
+
     return 0;
 }

src/tool/map_lanetoxodr/ndsdataproc.h

@@ -16,12 +16,15 @@ struct  nds_vehicle
     double height;
     double heading;
 
+    double frel_x;
+    double frel_y;
+
 };
 
 struct nds_line
 {
     int64_t localtime;
-    int feature =1; //1 left1 2 ringht1
+    int feature =-1; //0 normal 1 left1 2 ringht1
     double centerdeparture;
     double lanewidth;
     double laneheading;
@@ -37,6 +40,10 @@ public:
 
 public:
     static int ProcNDSData(std::string strlinepath,std::string strvehiclepath);
+
+private:
+    double mflon0;
+    double mflat0;
 };
 
 #endif // NDSDATAPROC_H