change driver_map_xodrload. road right lane change is ok, road left lane change not complete.

src/driver/driver_map_xodrload/globalplan.cpp

@@ -2014,9 +2014,13 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 
             if(xps.mainsel != xps.secondsel)
             {
+//                if(xps.mroadid == 103)
+//                {
+//                    double xy = 1;
+//                }
                 off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
-                pp.mfSecx = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-                pp.mfSecy = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+                pp.mfSecx = xvPP.at(i).x + off1 *cos(pp.hdg + M_PI/2.0);
+                pp.mfSecy = xvPP.at(i).y + off1 *sin(pp.hdg + M_PI/2.0);
 
                 if(xps.mainsel >xps.secondsel)
                 {
@@ -2750,7 +2754,21 @@ void ChangeLane(std::vector<PlanPoint> & xvectorPP)
                 }
             }
             int nnum = k-i;
-            const int nchangepoint = 300;
+            int nchangepoint = 300;
+            double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
+                                   +pow(xvectorPP[k].y - xvectorPP[i].y,2));
+            if(froadlen<100)
+            {
+                nchangepoint = nnum;
+            }
+            else
+            {
+                nchangepoint = (100/froadlen) * nnum;
+            }
+
+            qDebug(" road %d len is %f changepoint is %d nnum is %d",
+                   xvectorPP[k-1].nRoadID,froadlen,nchangepoint,nnum);
+
             int nstart = i + nnum/2 -nchangepoint/2;
             int nend = i+nnum/2 + nchangepoint/2;
             if(nnum<300)
@@ -2772,7 +2790,7 @@ void ChangeLane(std::vector<PlanPoint> & xvectorPP)
                 double foff = fdis *(j-nstart)/nnum;
                 if(xvectorPP[j].nlrchange == 1)
                 {
-                    std::cout<<"foff is "<<foff<<std::endl;
+//                    std::cout<<"foff is "<<foff<<std::endl;
                     xvectorPP[j].x = xvectorPP[j].mfSecx + foff *cos(xvectorPP[j].hdg + M_PI/2.0);
                     xvectorPP[j].y = xvectorPP[j].mfSecy + foff *sin(xvectorPP[j].hdg + M_PI/2.0);
                 }

src/driver/driver_map_xodrload/xodrdijkstra.cpp

@@ -1618,7 +1618,7 @@ std::vector<pathsection> xodrdijkstra::getgpspoint(int srcroadid, int nsrclr, in
     for(i=0;i<xpathsection.size();i++)
     {
         std::cout<<"path "<<i<<" road:"<<xpathsection[i].mroadid<<" section "<<xpathsection[i].msectionid
-                <<"  lane  "<<xpathsection[i].mainsel<<std::endl;
+                <<"  lane  "<<xpathsection[i].mainsel<<" sec lane is "<<xpathsection[i].secondsel<<std::endl;
     }
 
     return xpathsection;