change driver_map_xodrload. fix near point.

src/common/common/xodr/xodrfunc/xodrfunc.cpp

@@ -224,6 +224,8 @@ double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
 
     double hdgltoa = CalcHdg(QPointF(x,y),QPointF(x_center,y_center));
 
+//    double hdgltoa = CalcHdg(QPointF(x_center,y_center),QPointF(x,y));
+
 
     QPointF arcpoint;
     arcpoint.setX(x_center);arcpoint.setY(y_center);

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1152,6 +1152,7 @@ static std::vector<PlanPoint> getarcpoint(GeometryArc * parc,const double fspace
     int nsize = parc->GetLength()/fspace;
     if(nsize == 0)nsize = 1;
     int i;
+
     for(i=0;i<nsize;i++)
     {
         double x,y;
@@ -1504,10 +1505,11 @@ int indexinroadpoint(std::vector<PlanPoint> xvectorPP,double x,double y)
 {
     int nrtn = 0;
     int i;
-    int dismin = 1000;
+    double dismin = 1000;
     for(i=0;i<xvectorPP.size();i++)
     {
         double dis = sqrt(pow(xvectorPP.at(i).x - x,2) + pow(xvectorPP.at(i).y -y,2));
+
         if(dis <dismin)
         {
             dismin = dis;

src/driver/driver_map_xodrload/main.cpp

@@ -1415,6 +1415,22 @@ void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
         return;
     }
 
+    static int64_t nLastFusionUpdate = 0;
+
+    int64_t nNow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+
+    if(strncmp(strmemname,"fusion_gpsimu",256) != 0 )
+    {
+        if(abs(nNow - nLastFusionUpdate)<3000)
+        {
+            return;
+        }
+    }
+    else
+    {
+        nLastFusionUpdate = nNow;
+    }
+
     xodrobj xo;
     xo.fhgdsrc = xgpsimu.heading();
     xo.flatsrc = xgpsimu.lat();

src/tool/map_collectfromveh/commif.cpp

@@ -4,6 +4,7 @@
 
 #include <memory>
 #include <thread>
+#include <iostream>
 
 
 static mcommCall  gpgpscall;
@@ -21,6 +22,7 @@ void UpdateGPS(const char *strdata, const unsigned int nSize, const unsigned int
     {
         if(abs(nNow - gnLastFusionGPSUpdate)<1000)
         {
+//            std::cout<<"use fusion."<<std::endl;
             return;
         }
     }