change driver_map_xodrload. add multi station support.

src/driver/driver_map_xodrload/driver_map_xodrload.pro

@@ -18,6 +18,7 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp     \
+    ../../include/msgtype/v2x.pb.cc \
 OpenDrive/OpenDriveXmlWriter.cpp \
 OpenDrive/OtherStructures.cpp \
 OpenDrive/OpenDrive.cpp \
@@ -44,6 +45,7 @@ TinyXML/tinyxmlerror.cpp \
 
 HEADERS += \
     ../../../include/ivexit.h \
+    ../../include/msgtype/v2x.pb.h \
 OpenDrive/OpenDriveXmlParser.h \
 OpenDrive/RoadGeometry.h \
 OpenDrive/Road.h \

src/driver/driver_map_xodrload/main.cpp

@@ -11,6 +11,8 @@
 
 #include "gpsimu.pb.h"
 
+#include "v2x.pb.h"
+
 #include "modulecomm.h"
 
 #include "xmlparam.h"
@@ -36,6 +38,7 @@ void * gpasrc;
 void * gpmap;
 void * gpagps;
 void * gpasimple;
+void * gpav2x;
 
 iv::Ivfault *gfault = nullptr;
 iv::Ivlog *givlog = nullptr;
@@ -408,6 +411,8 @@ static void ToGPSTrace(std::vector<PlanPoint> xPlan)
 
     ShareMap(mapdata);
 }
+
+
 void SetPlan(xodrobj xo)
 {
 
@@ -482,6 +487,111 @@ void SetPlan(xodrobj xo)
     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;
+
+        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)
 {
 
@@ -505,6 +615,25 @@ void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int
 
 }
 
+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)
 {
 
@@ -580,6 +709,8 @@ int main(int argc, char *argv[])
 
     std::string strgpsmsg = xp.GetParam("gpsmsg","hcp2_gpsimu");
 
+    std::string strv2xmsg = xp.GetParam("v2xmsg","v2x");
+
 
     glat0 = atof(strlat0.data());
     glon0 = atof(strlon0.data());
@@ -592,6 +723,7 @@ int main(int argc, char *argv[])
     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);