change driver_map_xodrload. complete poly3 and change lane plan. Need TEST.

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1607,8 +1607,8 @@ inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLW
     d = xvectorLWA[xvectorIndex[i]].D;
     off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
     if(nlane < 0) off = off*(-1.0);
-    std::cout<<"s is "<<s<<std::endl;
-    std::cout<<" off is "<<off<<std::endl;
+//    std::cout<<"s is "<<s<<std::endl;
+//    std::cout<<" off is "<<off<<std::endl;
     return off;
 
 }
@@ -1928,7 +1928,7 @@ double getoff(Road * pRoad,int nlane,const double s)
     return foff;
 }
 
-//暂不考虑多个LaneSection的情况，不考虑车道宽度变化
+
 std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,const double fvehiclewidth)
 {
     std::vector<PlanPoint> xvectorPP;
@@ -2000,7 +2000,6 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
     if(xps.mainsel < 0)
     {
 
-
         for(i=0;i<nsize;i++)
         {
             PlanPoint pp = xvPP.at(i);
@@ -2014,10 +2013,6 @@ 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 = 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);
@@ -2047,23 +2042,69 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
             xvectorPP.push_back(pp);
         }
 
+
+    }
+    else
+    {
+
+        for(i=0;i<nsize;i++)
+        {
+            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);
+
+            pp.nlrchange = 1;
+
+            if(xps.mainsel != xps.secondsel)
+            {
+                off1 = getoff(xps.mpRoad,xps.secondsel,pp.mS);
+                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)
+                {
+                    pp.nlrchange = 2;
+                }
+                else
+                {
+                    pp.nlrchange = 1;
+                }
+            }
+            else
+            {
+                pp.mfSecx = pp.x ;
+                pp.mfSecy = pp.y ;
+            }
+
+            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1;
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+            xvectorPP.push_back(pp);
+        }
+
 //        for(i=0;i<(nchange1- nchangepoint/2);i++)
 //        {
-
 //            PlanPoint pp = xvPP.at(i);
-////            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-
-//            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
 //            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
 //            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
 
 //            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
 //            pp.mfDisToLaneLeft = pp.mWidth/2.0;
 //            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off1*(-1);
+//            pp.mfDisToRoadLeft = off1;
 //            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
 //            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
 
+
 //            xvectorPP.push_back(pp);
 
 //        }
@@ -2071,17 +2112,16 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 //        {
 
 //            PlanPoint pp = xvPP.at(i);
-////            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-////            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-//            off1 = getoff(xps.mpRoad,nlane1,pp.mS);
-//            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
+//            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
 //            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
 //            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
 //            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
 
 //            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
 
-//            pp.mfDisToRoadLeft = offx*(-1);
+//            pp.mfDisToRoadLeft = offx;
 //            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
 //            if(nlane1 == nlane2)
 //            {
@@ -2094,11 +2134,11 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 //            {
 //                if(nlane1 < nlane2)
 //                {
-//                    pp.lanmp = 1;
+//                    pp.lanmp = -1;
 //                }
 //                else
 //                {
-//                    pp.lanmp = -1;
+//                    pp.lanmp = 1;
 //                }
 
 //                if(i<nchange1)
@@ -2126,19 +2166,19 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 //        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
 //        {
 //            PlanPoint pp = xvPP.at(i);
-// //           off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-
-//            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
 //            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
 //            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
 
 //            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
 //            pp.mfDisToLaneLeft = pp.mWidth/2.0;
 //            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off2*(-1);
+//            pp.mfDisToRoadLeft = off2;
 //            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
 //            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
 
+
 //            xvectorPP.push_back(pp);
 
 //        }
@@ -2146,18 +2186,16 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 //        {
 
 //            PlanPoint pp = xvPP.at(i);
-////            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-////            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-
-//            off2 = getoff(xps.mpRoad,nlane2,pp.mS);
-//            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
+//            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
 //            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
 //            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
 //            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
 
 //            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
 
-//            pp.mfDisToRoadLeft = offx*(-1);
+//            pp.mfDisToRoadLeft = offx;
 //            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
 //            if(nlane2 == nlane3)
 //            {
@@ -2170,11 +2208,11 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 //            {
 //                if(nlane2 < nlane3)
 //                {
-//                    pp.lanmp = 1;
+//                    pp.lanmp = -1;
 //                }
 //                else
 //                {
-//                    pp.lanmp = -1;
+//                    pp.lanmp = 1;
 //                }
 
 //                if(i<nchange2)
@@ -2199,18 +2237,18 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 //            xvectorPP.push_back(pp);
 
 //        }
-//        for(i=(nchange2 + nchangepoint/2);i<nsize;i++)
+//        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
 //        {
 //            PlanPoint pp = xvPP.at(i);
-//    //        off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-//            off3 = getoff(xps.mpRoad,nlane3,pp.mS);
+//            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
 //            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
 //            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
+//            pp.hdg = pp.hdg + M_PI;
 
 //            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
 //            pp.mfDisToLaneLeft = pp.mWidth/2.0;
 //            pp.lanmp = 0;
-//            pp.mfDisToRoadLeft = off3*(-1);
+//            pp.mfDisToRoadLeft = off3;
 //            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
 //            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
 
@@ -2220,176 +2258,6 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP,
 //        }
 
     }
-    else
-    {
-        for(i=0;i<(nchange1- nchangepoint/2);i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-            pp.x = pp.x + off1 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off1 *sin(pp.hdg + M_PI/2.0);
-            pp.hdg = pp.hdg + M_PI;
-
-            pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off1;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-            xvectorPP.push_back(pp);
-
-        }
-        for(i=(nchange1 - nchangepoint/2);i<(nchange1+nchangepoint/2);i++)
-        {
-
-            PlanPoint pp = xvPP.at(i);
-            off1 = getoff(nlane1,pp.mS,xvectorLWA,xvectorindex1);
-            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-            double offx = off1 + (off2 - off1) *(i-nchange1 + nchangepoint/2)/nchangepoint;
-            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
-            pp.hdg = pp.hdg + M_PI;
-
-            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-
-            pp.mfDisToRoadLeft = offx;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
-            if(nlane1 == nlane2)
-            {
-                pp.mWidth = flanewidth1;
-                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                pp.lanmp = 0;
-                GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-            }
-            else
-            {
-                if(nlane1 < nlane2)
-                {
-                    pp.lanmp = -1;
-                }
-                else
-                {
-                    pp.lanmp = 1;
-                }
-
-                if(i<nchange1)
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (i-(nchange1 - nchangepoint/2))/nchangepoint;
-                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
-                else
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (nchange1+nchangepoint/2 -i)/nchangepoint;
-                    if(nlane1<nlane2)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
-
-            }
-
-            xvectorPP.push_back(pp);
-
-        }
-        for(i=(nchange1 + nchangepoint/2);i<(nchange2-nchangepoint/2);i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-            pp.x = pp.x + off2 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off2 *sin(pp.hdg + M_PI/2.0);
-            pp.hdg = pp.hdg + M_PI;
-
-            pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off2;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-            xvectorPP.push_back(pp);
-
-        }
-        for(i=(nchange2 - nchangepoint/2);i<(nchange2+nchangepoint/2);i++)
-        {
-
-            PlanPoint pp = xvPP.at(i);
-            off2 = getoff(nlane2,pp.mS,xvectorLWA,xvectorindex2);
-            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-            double offx = off2 + (off3 - off2) *(i-nchange2 + nchangepoint/2)/nchangepoint;
-            pp.x = pp.x + offx *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + offx *sin(pp.hdg + M_PI/2.0);
-            pp.hdg = pp.hdg + M_PI;
-
-            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-
-            pp.mfDisToRoadLeft = offx;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
-            if(nlane2 == nlane3)
-            {
-                pp.mWidth = flanewidth1;
-                pp.mfDisToLaneLeft = pp.mWidth/2.0;
-                pp.lanmp = 0;
-                GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-            }
-            else
-            {
-                if(nlane2 < nlane3)
-                {
-                    pp.lanmp = -1;
-                }
-                else
-                {
-                    pp.lanmp = 1;
-                }
-
-                if(i<nchange2)
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (i-(nchange2 - nchangepoint/2))/nchangepoint;
-                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  - fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 + fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
-                else
-                {
-                    pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-                    double fmove = pp.mWidth * (nchange2+nchangepoint/2 -i)/nchangepoint;
-                    if(nlane2<nlane3)pp.mfDisToLaneLeft = pp.mWidth/2.0  + fmove;
-                    else pp.mfDisToLaneLeft = pp.mWidth/2.0 - fmove;
-                    GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-                }
-
-            }
-
-            xvectorPP.push_back(pp);
-
-        }
-        for(i=(nchange2+ nchangepoint/2);i<nsize;i++)
-        {
-            PlanPoint pp = xvPP.at(i);
-            off3 = getoff(nlane3,pp.mS,xvectorLWA,xvectorindex3);
-            pp.x = pp.x + off3 *cos(pp.hdg + M_PI/2.0);
-            pp.y = pp.y + off3 *sin(pp.hdg + M_PI/2.0);
-            pp.hdg = pp.hdg + M_PI;
-
-            pp.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
-            pp.mfDisToLaneLeft = pp.mWidth/2.0;
-            pp.lanmp = 0;
-            pp.mfDisToRoadLeft = off3;
-            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
-            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
-
-
-            xvectorPP.push_back(pp);
-
-        }
-    }
 
 
 
@@ -2746,7 +2614,7 @@ void ChangeLane(std::vector<PlanPoint> & xvectorPP)
         else
         {
             int k;
-            for(k=i;k<nsize;k++)
+            for(k=i;k<(nsize-1);k++)
             {
                 if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
                 {
@@ -2757,13 +2625,14 @@ void ChangeLane(std::vector<PlanPoint> & xvectorPP)
             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)
+            const double fMAXCHANGE = 100;
+            if(froadlen<fMAXCHANGE)
             {
                 nchangepoint = nnum;
             }
             else
             {
-                nchangepoint = (100/froadlen) * nnum;
+                nchangepoint = (fMAXCHANGE/froadlen) * nnum;
             }
 
             qDebug(" road %d len is %f changepoint is %d nnum is %d",
@@ -2793,11 +2662,13 @@ void ChangeLane(std::vector<PlanPoint> & xvectorPP)
 //                    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);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft + fdis - foff;
                 }
                 else
                 {
                     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);
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  - fdis +foff;
                 }
             }
             i =k;