modify map load function

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

@@ -1214,11 +1214,11 @@ xodrdijkstra::xodrdijkstra(OpenDrive  * pxodr)
 
     }
 
-    for(i=0;i<mroadedge.size();i++)
-    {
-        qDebug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
-        givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
-    }
+//    for(i=0;i<mroadedge.size();i++)
+//    {
+//        /*qDebug*/("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
+////        givlog->debug("%d %d %d: %d %d",mroadedge[i].mroadid,mroadedge[i].mnleftright,mroadedge[i].mnsectionid, mroadedge[i].mvertexstart,mroadedge[i].mvertexend);
+//    }
 
 
     int nvertexnum = xvalidvertex.size(); //vertex num
@@ -1430,7 +1430,7 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
         if(k == -1)
         {
             qDebug("i = %d not found k",i);
-            givlog->debug("i = %d not found k",i);
+//            givlog->debug("i = %d not found k",i);
             break;
         }
         // 标记"顶点k"为已经获取到最短路径
@@ -1505,8 +1505,8 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
 
     for(i=0;i<rtnpath.size();i++)
     {
-        givlog->debug("%d %d %d %d",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
-                mroadedge[rtnpath[rtnpath.size()-1-i]].mnsectionid);
+//        givlog->debug("%d %d %d %d",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
+//                mroadedge[rtnpath[rtnpath.size()-1-i]].mnsectionid);
 //        qDebug("%d %d %d %f",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
 //                mroadedge[rtnpath[rtnpath.size()-1-i]].mlen);
         rtnpath2.push_back(rtnpath[rtnpath.size()-1-i]);
@@ -1514,7 +1514,7 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
 
 /*    if(srcedge == dstedge)*/rtnpath2.push_back(dstedge);
 
-    givlog->debug("rt = %d ",rtnpath.size());
+//    givlog->debug("rt = %d ",rtnpath.size());
     //gfault->SetFaultState(0,0,"ok");
 
     return rtnpath2;

src/driver/driver_map_xodrload/main.cpp

@@ -43,6 +43,7 @@ double glat0,glon0,ghead0;
 double gvehiclewidth = 2.0;
 
 bool gbExtendMap = true;
+bool gbHasMap = false;
 
 void * gpa;
 void * gpasrc;
@@ -136,8 +137,9 @@ bool LoadXODR(std::string strpath)
 
 
     Road * proad1 = mxodr.GetRoad(0);
-    givlog->info("road name is %s",proad1->GetRoadName().c_str());
+    givlog->info("xodrload","road name is %s,map name is %s",proad1->GetRoadName().c_str(),strpath.c_str());
     std::cout<<" road name is "<<proad1->GetRoadName()<<std::endl;
+    gbHasMap = true;
 }
 
 class roadx
@@ -416,8 +418,8 @@ static void ToGPSTrace(std::vector<PlanPoint> xPlan)
         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);
+//        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;
@@ -538,8 +540,8 @@ void SetPlan(xodrobj xo)
         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);
+//        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;
@@ -719,8 +721,8 @@ void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double
  //       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);
+//        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;
@@ -763,25 +765,33 @@ void MultiStationPlan(iv::v2x::v2x * pxv2x,double fsrclat,double fsrclon,double
 
 void ListenCmd(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-
-    if(nSize<sizeof(xodrobj))
+    QTime timer;
+    timer.start();
+    int timeElapsed = 0;
+    while(!gbHasMap && timeElapsed < 10000)
     {
-        std::cout<<"ListenCmd small."<<std::endl;
-        return;
+        timeElapsed = timer.elapsed();
     }
 
-    xodrobj xo;
-    memcpy(&xo,strdata,sizeof(xodrobj));
-
-    givlog->debug("mapLoad","lat is %f, lon is %f", xo.flat, xo.flon);
+    if(gbHasMap)
+    {
+        if(nSize<sizeof(xodrobj))
+        {
+            std::cout<<"ListenCmd small."<<std::endl;
+            return;
+        }
 
-    xo.fhgdsrc = gsrc.fhgdsrc;
-    xo.flatsrc = gsrc.flatsrc;
-    xo.flonsrc = gsrc.flonsrc;
+        xodrobj xo;
+        memcpy(&xo,strdata,sizeof(xodrobj));
 
-    SetPlan(xo);
+        givlog->debug("xodrload","recv des station: lat is %f, lon is %f", xo.flat, xo.flon);
 
+        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)
@@ -789,13 +799,13 @@ void ListenV2X(const char * strdata,const unsigned int nSize,const unsigned int
     iv::v2x::v2x xv2x;
     if(!xv2x.ParseFromArray(strdata,nSize))
     {
-        givlog->warn("ListernV2X Parse Error.");
+        givlog->warn("xdorload","ListernV2X Parse Error.");
         std::cout<<"ListenV2X Parse Error."<<std::endl;
         return;
     }
     if(xv2x.stgps_size()<1)
     {
-        givlog->debug("ListenV2X no gps station.");
+        givlog->debug("xodrload","ListenV2X no gps station.");
         std::cout<<"ListenV2X no gps station."<<std::endl;
         return;
     }
@@ -807,25 +817,24 @@ void ListenHmi(const char * strdata,const unsigned int nSize,const unsigned int
     iv::hmi::hmimsg xhmimsg;
     if(!xhmimsg.ParseFromArray(strdata,nSize))
     {
-        givlog->warn("<map lodar> ListenHmi Parse Error.");
+        givlog->warn("xodrload","<map lodar> ListenHmi Parse Error.");
         std::cout<<"<map lodar> ListenHmi Parse Error."<<std::endl;
         return;
     }
     if(xhmimsg.has_mapname())
     {
         if(LoadXODR(xhmimsg.mapname()))
-            givlog->info("mapLoad","load map success: %s",xhmimsg.mapname().c_str());
+            givlog->info("xodrload","load map success: %s",xhmimsg.mapname().c_str());
         else
-            givlog->error("mapLoad","load map fail: %s",xhmimsg.mapname().c_str());
+            givlog->error("xodrload","load map fail: %s",xhmimsg.mapname().c_str());
     }
 }
 
 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");
+        givlog->warn("xodrload","ListenSrc small");
         std::cout<<"ListenSrc small."<<std::endl;
         return;
     }
@@ -843,7 +852,7 @@ void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned i
     iv::gps::gpsimu  xgpsimu;
     if(!xgpsimu.ParseFromArray(strdata,nSize))
     {
-        givlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
+        givlog->warn("xodrload","ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
         return;
     }
 
@@ -949,7 +958,7 @@ int main(int argc, char *argv[])
 
     gpasimple = iv::modulecomm::RegisterSend("simpletrace",100000,1);
 
-
+    givlog->info("xodrload","map_xodrload sharemem register success");
     double x_src,y_src,x_dst,y_dst;
 
     x_src = -26;y_src = 10;

src/ui/ui_ads_hmi_1px/ADCIntelligentVehicle.cpp

@@ -2186,7 +2186,7 @@ void ADCIntelligentVehicle::on_button_map_set_clicked()
         ShareHMIMsgPro(hmi);
 //        for(int i = 0; i < 10; i++)
 //        {for(int j = 0; j < 99999;j++){;}}
-        QThread::sleep(2);
+//        QThread::sleep(2);
         gIvlog->info("mapLoad","send des gps");
         //发送站点gps信息
         xodrobj xo;