change driver/driver_map_xodrload. solve find road section error.

src/driver/driver_map_xodrload/globalplan.cpp

@@ -2836,6 +2836,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
     if(bNeedDikstra)
     {
     std::vector<int> xpath = pxd->getpath(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,fs1,fs2);
+    if(xpath.size()<1)return -1;
     double flen = pxd->getpathlength(xpath);
     std::vector<pathsection> xpathsection = pxd->getgpspoint(atoi(pRoad->GetRoadId().data()),lr_start,atoi(pRoad_obj->GetRoadId().data()),lr_end,xpath,nlanesel);
 

src/driver/driver_map_xodrload/main.cpp

@@ -393,7 +393,7 @@ static void ToGPSTrace(std::vector<PlanPoint> xPlan)
     strpath = getenv("HOME");
     strpath = strpath + "/map/maptrace.txt";
     xfile.setFileName(strpath);
-    xfile.open(QIODevice::ReadWrite);
+    bool bFileOpen = xfile.open(QIODevice::ReadWrite);
 
     for(i=0;i<nSize;i++)
     {
@@ -427,11 +427,11 @@ static void ToGPSTrace(std::vector<PlanPoint> xPlan)
         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);
+        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;
 
     }
-    xfile.close();
+    if(bFileOpen)xfile.close();
 
     ShareMap(mapdata);
 }
@@ -799,7 +799,6 @@ int main(int argc, char *argv[])
 
     RegisterIVBackTrace();
 
-
     gfault = new iv::Ivfault("driver_map_xodrload");
     givlog = new iv::Ivlog("driver_map_xodrload");
 

src/driver/driver_map_xodrload/xodrdijkstra.cpp

@@ -1087,7 +1087,7 @@ int xodrdijkstra::GetRoadSectionIndexByS(Road *pRoad, const double s)
 {
     int nrtn = -1;
     if(s<0)return nrtn;
-    if(s>pRoad->GetRoadLength())return nrtn;
+    if(s>(pRoad->GetRoadLength()+30.0))return nrtn;
     int i;
     int nseccount = pRoad->GetLaneSectionCount();
     if(nseccount== 1)return 0;
@@ -1126,7 +1126,8 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
     nsecdst = GetRoadSectionIndexByS(GetRoadByID(dstroadid),s_obj);
     if((nsecsrc<0)||(nsecdst<0))
     {
-        qDebug("getpath section error.");
+        qDebug("getpath section error. %d:%f %d:%f",srcroadid,s_src,
+               dstroadid,s_obj);
         return rtnpath;
     }
     int srcedge = getroadedge(srcroadid,nsrclr,nsecsrc);