change driver_map_xodrload. fix some bug.

src/common/common/xodr/xodrfunc.cpp

@@ -177,6 +177,7 @@ double xodrfunc::GetParamPoly3Dis(GeometryParamPoly3 * parc,double xnow,double y
 double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
                         double & neary,double & nearhead,double & frels)
 {
+    frels = 0.0;
 
     if((parc->GetS()>370)&&(parc->GetS()<370.1))
     {
@@ -187,7 +188,7 @@ double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
 
     double R = fabs(1.0/parc->GetCurvature());
 
-    frels = 0.0;
+
 
     //calculate arc center
     double x_center,y_center;
@@ -294,7 +295,7 @@ double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
         }
         while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
         while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
-        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/abs(parc->GetCurvature()));
+        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/fabs(parc->GetCurvature()));
         if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
         if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
         frels = hdgdiff * R;
@@ -312,7 +313,7 @@ double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
         }
         while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
         while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
-        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/abs(parc->GetCurvature()));
+        hdgdiff = (nearhead-parc->GetHdg())*(parc->GetCurvature()/fabs(parc->GetCurvature()));
         if(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
         if(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
         frels = hdgdiff * R;
@@ -334,6 +335,7 @@ double xodrfunc::GetArcDis(GeometryArc * parc,double x,double y,double & nearx,
         break;
     }
 
+
     while(nearhead>2.0*M_PI)nearhead = nearhead -2.0*M_PI;
     while(nearhead<0)nearhead = nearhead + 2.0*M_PI;
     return fdismin;
@@ -519,6 +521,8 @@ double xodrfunc::GetLineDis(GeometryLine * pline,const double x,const double y,d
 
     frels = sqrt(pow(nearx - pline->GetX(),2)+pow(neary - pline->GetY(),2));
 
+//    qDebug("frels is %f",frels);
+
     return fRtn;
 
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -760,6 +760,7 @@ int GetNearPointWithHead(const double x,const double y,const double hdg,OpenDriv
                 nearhead = nh;
                 s = dis;
                 froads = frels;
+   //             qDebug("rels s is %f ",frels);
                 *pObjRoad = proad;
                 *pgeo = pgb;
 
@@ -2057,6 +2058,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             PlanPoint pp = xvPP.at(i);
     //            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
 
+
             off1 = getoff(xps.mpRoad,nlane2,pp.mS);
             pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
             pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
@@ -2091,6 +2093,7 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             pp.mfRoadWidth = GetRoadWidth(xvectorLWA,xps.mainsel,pp.mS);
             GetLaneOriTotal(xps.mpRoad,xps.mainsel,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
 
+            pp.hdg = pp.hdg + M_PI;
             xvectorPP.push_back(pp);
         }
 
@@ -2419,7 +2422,11 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
       int j;
       for(j=0;j<xvPP.size();j++)
       {
-          xvectorPPs.push_back(xvPP.at(j));
+
+          PlanPoint pp;
+          pp = xvPP.at(j);
+//          pp.hdg = pp.hdg +M_PI;
+          xvectorPPs.push_back(pp);
       }
   }
 
@@ -2442,6 +2449,7 @@ static std::vector<PlanPoint> GetPlanPoint(std::vector<pathsection> xpathsection
   for(i=0;i<nlastsize;i++)
   {
       xvectorPPs.push_back(xvPP.at(i));
+
   }
 
   //Find StartPoint