change driver_map_xodrload.

src/driver/driver_map_xodrload/driver_map_xodrload.xml

@@ -2449,29 +2449,30 @@ static void AddSignalMark(Road * pRoad, int nlane,std::vector<PlanPoint> & xvect
     for(i=0;i<nsigcount;i++)
     {
         Signal * pSig = pRoad->GetSignal(i);
+        int nfromlane = -100;
+        int ntolane = 100;
 
-  //      signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
         if(pSig->GetlaneValidityCount()>0)
         {
-            bool bvalid = false;
-            vector<signal_laneValidity> * pvectorval = pSig->GetlaneValidityVector();
-            unsigned int nsize = static_cast<unsigned int>(pvectorval->size());
+            bool bValid = false;
             unsigned int j;
+            std::vector<signal_laneValidity> * pvectorlanevalid = pSig->GetlaneValidityVector();
+            unsigned int nsize = static_cast<unsigned int>(pvectorlanevalid->size());
             for(j=0;j<nsize;j++)
             {
-                int nfromlane = -100;
-                int ntolane = 100;
-                nfromlane = pvectorval->at(j).GetfromLane();
-                ntolane = pvectorval->at(j).GettoLane();
-                if((nlane>=nfromlane)&&(nlane<=ntolane))
+                signal_laneValidity * pSig_laneValidity = &pvectorlanevalid->at(j);
+                nfromlane = pSig_laneValidity->GetfromLane();
+                ntolane = pSig_laneValidity->GettoLane();
+                if((nlane >= nfromlane)&&(nlane<=ntolane))
                 {
-                    bvalid = true;
+                    bValid = true;
                     break;
                 }
             }
-
-            if(bvalid == false)continue;
+            if(bValid == false)continue;
         }
+
+//        signal_laneValidity * pSig_laneValidity = pSig->GetlaneValidity();
 //        if(pSig_laneValidity != 0)
 //        {
 //            nfromlane = pSig_laneValidity->GetfromLane();
@@ -2799,18 +2800,20 @@ void ChangeLane(std::vector<PlanPoint> & xvectorPP)
         else
         {
             int k;
-            for(k=i;k<(nsize-1);k++)
+            for(k=i;k<=(nsize-1);k++)
             {
-                if((xvectorPP[k].mfSecx == xvectorPP[k].x)&&(xvectorPP[k].mfSecy == xvectorPP[k].y))
+                if((fabs(xvectorPP[k].mfSecx - xvectorPP[k].x)<0.001)&&(fabs(xvectorPP[k].mfSecy - xvectorPP[k].y)<0.001))
                 {
                     break;
                 }
             }
+            if(k>(nsize-1))k=nsize-1;
             int nnum = k-i;
             int nchangepoint = 300;
             double froadlen = sqrt(pow(xvectorPP[k].x - xvectorPP[i].x,2)
                                    +pow(xvectorPP[k].y - xvectorPP[i].y,2));
             const double fMAXCHANGE = 50;
+
             if(froadlen<fMAXCHANGE)
             {
                 nchangepoint = nnum;
@@ -2837,23 +2840,29 @@ void ChangeLane(std::vector<PlanPoint> & xvectorPP)
                 xvectorPP[j].y = xvectorPP[j].mfSecy;
             }
             nnum = nend - nstart;
-            for(j=nstart;j<nend;j++)
+            int nlast = k-1;
+            if(k==(nsize-1))nlast = k;
+            for(j=nstart;j<=nlast;j++)
             {
                 double fdis = sqrt(pow(xvectorPP[j].x - xvectorPP[j].mfSecx,2)
                                    +pow(xvectorPP[j].y - xvectorPP[j].mfSecy,2));
-                double foff = fdis *(j-nstart)/nnum;
+                double foff = 0;
+                if(j<nend)
+                    foff = fdis *(j-nstart)/nnum;
+                else
+                    foff = fdis;
                 if(xvectorPP[j].nlrchange == 1)
                 {
 //                    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;
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft - foff; //+ fdis
                 }
                 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;
+                    xvectorPP[j].mfDisToRoadLeft = xvectorPP[j].mfDisToRoadLeft  +foff;//- fdis
                 }
             }
             i =k;

src/driver/driver_map_xodrload/main.cpp

@@ -860,6 +860,14 @@ void SetPlan(xodrobj xo)
     xfile_1.setFileName(strpath_1);
     xfile_1.open(QIODevice::ReadWrite);
 
+    double fpointdis = 0.1;
+    if(nSize > 2)
+    {
+        fpointdis = sqrt(pow(xPlan[0].x - xPlan[1].x,2)+pow(xPlan[0].y - xPlan[1].y,2));
+    }
+
+    if(fpointdis < 0.1)fpointdis = 0.1;
+
     for(i=0;i<nSize;i++)
     {
         iv::map::mappoint * pmappoint =  xtrace.add_point();
@@ -907,15 +915,27 @@ void SetPlan(xodrobj xo)
         iv::GPSData data(new iv::GPS_INS);
         data->roadMode = 0;
 
- //       std::cout<<"i:"<<i<<" lr:"<<xPlan[i].nlrchange<<std::endl;
-        if(xPlan[i].nlrchange == 1)
+        static int nChangeCount = 0;
+        static int nlrmode = -1;
+
+        if(nChangeCount == 0)
         {
-            data->roadMode = 14;
+
         }
-        if(xPlan[i].nlrchange == 2)
+        else
         {
-            data->roadMode = 15;
+            data->roadMode = nlrmode;
+            nChangeCount--;
         }
+ //       std::cout<<"i:"<<i<<" lr:"<<xPlan[i].nlrchange<<std::endl;
+//        if(xPlan[i].nlrchange == 1)
+//        {
+//            data->roadMode = 14;
+//        }
+//        if(xPlan[i].nlrchange == 2)
+//        {
+//            data->roadMode = 15;
+//        }
 
         if(i>50)
         {
@@ -934,6 +954,32 @@ void SetPlan(xodrobj xo)
                     }
                     if(j <=0)break;
                 }
+                int nChangeCount = 0;
+                for(j=i;j<(nSize-1);j++)
+                {
+                    if((xPlan[j].nlrchange != 0)&&(xPlan[j+1].nlrchange != 0))
+                    {
+                        nChangeCount++;
+                    }
+                }
+                if(nChangeCount < static_cast<int>(10.0/fpointdis))   //Fast Change road.
+                {
+                    nChangeCount = nChangeCount + static_cast<int>(3.0/fpointdis);
+                }
+                else
+                {
+                    nChangeCount =  static_cast<int>(5.0/fpointdis);
+                }
+
+                    if(xPlan[i].nlrchange == 1)
+                    {
+                        nlrmode = 14;
+                    }
+                    if(xPlan[i].nlrchange == 2)
+                    {
+                        nlrmode = 15;
+                    }
+
             }
         }