change gps_type.h. change driver_map_xodrload for add lane widht lane ori lane count and road count speed.

src/decition/common/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-        double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/decition/common/common_sf/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/decition/decition_external/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/detection/detection_ndt_slam_hcp2/common/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/driver/driver_map_trace/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-        double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/driver/driver_map_xodrload/globalplan.cpp

@@ -1093,19 +1093,41 @@ double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlw
     return frtn;
 }
 
+
+
 std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
 {
     std::vector<iv::lanewidthabcd> xvectorlwa;
     if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
     int i;
-    int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
+
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+//    if(pRoad->GetLaneSectionCount() > 1)
+//    {
+//        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+//        {
+//            if(s>pRoad->GetLaneSection(i+1)->GetS())
+//            {
+//                pLS = pRoad->GetLaneSection(i+1);
+//            }
+//            else
+//            {
+//                break;
+//            }
+//        }
+//    }
+
+    int nlanecount = pLS->GetLaneCount();
+
     for(i=0;i<nlanecount;i++)
     {
         iv::lanewidthabcd xlwa;
 
 
-        LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
-        xlwa.nlane = pRoad->GetLaneSection(0)->GetLane(i)->GetId();
+        LaneWidth* pLW = pLS->GetLane(i)->GetLaneWidth(0);
+        xlwa.nlane = pLS->GetLane(i)->GetId();
         if(pLW != 0)
         {
 
@@ -1123,8 +1145,8 @@ std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
             xlwa.D = 0;
         }
 
-
-        xvectorlwa.push_back(xlwa);
+ //       if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+        xvectorlwa.push_back(xlwa);   //Calculate Road Width, not driving need add in.
 
     }
     return xvectorlwa;
@@ -1166,6 +1188,112 @@ inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLW
 
 }
 
+double GetRoadWidth(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane,double s)
+{
+    double fwidth = 0;
+    int i;
+    double a,b,c,d;
+
+    int nsize = xvectorLWA.size();
+    for(i=0;i<nsize;i++)
+    {
+        if(nlane*(xvectorLWA[i].nlane)>0)
+        {
+            a = xvectorLWA[i].A;
+            b = xvectorLWA[i].B;
+            c = xvectorLWA[i].C;
+            d = xvectorLWA[i].D;
+            fwidth = fwidth + a + b*s +c*s*s + d*s*s*s;
+        }
+    }
+    return fwidth;
+
+}
+
+int GetLaneOriTotal(Road * pRoad, int nlane,double s,int & nori, int & ntotal,double & fSpeed)
+{
+    if(pRoad->GetLaneSectionCount() < 1)return -1;
+
+    LaneSection * pLS = pRoad->GetLaneSection(0);
+
+    int i;
+
+    if(pRoad->GetLaneSectionCount() > 1)
+    {
+        for(i=0;i<(pRoad->GetLaneSectionCount()-1);i++)
+        {
+            if(s>pRoad->GetLaneSection(i+1)->GetS())
+            {
+                pLS = pRoad->GetLaneSection(i+1);
+            }
+            else
+            {
+                break;
+            }
+        }
+    }
+
+    nori = 0;
+    ntotal = 0;
+    fSpeed = -1; //if -1 no speedlimit for map
+
+    int nlanecount = pLS->GetLaneCount();
+    for(i=0;i<nlanecount;i++)
+    {
+        int nlaneid = pLS->GetLane(i)->GetId();
+
+        if(nlaneid == nlane)
+        {
+            Lane * pLane = pLS->GetLane(i);
+            if(pLane->GetLaneSpeedCount() > 0)
+            {
+                LaneSpeed * pLSpeed = pLane->GetLaneSpeed(0);
+                int j;
+                int nspeedcount = pLane->GetLaneSpeedCount();
+                for(j=0;j<(nspeedcount -1);j++)
+                {
+                    if((s-pLS->GetS())>=pLane->GetLaneSpeed(j+1)->GetS())
+                    {
+                        pLSpeed = pLane->GetLaneSpeed(j+1);
+                    }
+                    else
+                    {
+                        break;
+                    }
+                }
+                fSpeed = pLSpeed->GetMax();
+            }
+        }
+        if(nlane<0)
+        {
+
+            if(nlaneid < 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid<nlane)nori++;
+                }
+            }
+
+
+        }
+        else
+        {
+            if(nlaneid > 0)
+            {
+                if(strncmp(pLS->GetLane(i)->GetType().data(),"driving",256) == 0)
+                {
+                    ntotal++;
+                    if(nlaneid > nlane)nori++;
+                }
+            }
+        }
+    }
+
+    return 0;
+}
+
 std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
 {
     std::vector<int> xvectorindex;
@@ -1283,6 +1411,14 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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.mWidth = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off1*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane1,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,nlane1,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
             xvectorPP.push_back(pp);
 
         }
@@ -1295,6 +1431,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
+
+            double flanewidth1 = getwidth(xps.mpRoad,nlane1,xvectorLWA,pp.mS);
+
+            pp.mfDisToRoadLeft = offx*(-1);
+            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);
 
         }
@@ -1304,6 +1482,14 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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.mWidth = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off2*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane2,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,nlane2,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
             xvectorPP.push_back(pp);
 
         }
@@ -1316,6 +1502,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
+
+            double flanewidth1 = getwidth(xps.mpRoad,nlane2,xvectorLWA,pp.mS);
+
+            pp.mfDisToRoadLeft = offx*(-1);
+            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);
 
         }
@@ -1325,6 +1553,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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.mWidth = getwidth(xps.mpRoad,nlane3,xvectorLWA,pp.mS);
+            pp.mfDisToLaneLeft = pp.mWidth/2.0;
+            pp.lanmp = 0;
+            pp.mfDisToRoadLeft = off3*(-1);
+            pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane3,pp.mS);
+            GetLaneOriTotal(xps.mpRoad,nlane3,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
             xvectorPP.push_back(pp);
 
         }
@@ -1339,6 +1576,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1352,6 +1598,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1362,6 +1650,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1375,6 +1672,48 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1385,6 +1724,15 @@ std::vector<PlanPoint> GetLanePoint(pathsection xps,std::vector<PlanPoint> xvPP)
             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);
 
         }
@@ -1651,6 +1999,9 @@ void checktrace(std::vector<PlanPoint> & xPlan)
     }
 }
 
+
+#include <QFile>
+
 /**
  * @brief MakePlan         全局路径规划
  * @param pxd              有向图
@@ -1754,6 +2105,13 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                     foff = getoff(nlane,pp.mS,xvectorLWA,xvectorindex1);
                     pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                     pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
+                    pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                    pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                    pp.lanmp = 0;
+                    pp.mfDisToRoadLeft = foff *(-1.0);
+                    pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                    GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
                     xvectorPP.push_back(pp);
                     i++;
                 }
@@ -1764,6 +2122,15 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
                     pp.x = pp.x + foff *cos(pp.hdg + M_PI/2.0);
                     pp.y = pp.y + foff *sin(pp.hdg + M_PI/2.0);
                     pp.hdg = pp.hdg + M_PI;
+
+                    pp.mWidth = getwidth(pRoad,nlane,xvectorLWA,pp.mS);
+                    pp.mfDisToLaneLeft = pp.mWidth/2.0;
+                    pp.lanmp = 0;
+                    pp.mfDisToRoadLeft = foff;
+                    pp.mfRoadWidth = GetRoadWidth(xvectorLWA,nlane,pp.mS);
+                    GetLaneOriTotal(pRoad,nlane,pp.mS,pp.mnLaneori,pp.mnLaneTotal,pp.speed);
+
+
                     xvectorPP.push_back(pp);
                     i--;
                 }
@@ -1785,6 +2152,22 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
 
     }
 
+    int i;
+
+//    QFile xFile;
+//    xFile.setFileName("/home/yuchuli/tempgps.txt");
+//    xFile.open(QIODevice::ReadWrite);
+
+//    for(i<0;i<xPlan.size();i++)
+//    {
+//        char strout[1000];
+//        snprintf(strout,1000,"%d %f %f %d %d  %d %f \n",i,xPlan[i].mfDisToLaneLeft,
+//                 xPlan[i].mfDisToRoadLeft,xPlan[i].mnLaneori,xPlan[i].mnLaneTotal,
+//                 xPlan[i].lanmp,xPlan[i].mWidth);
+//        xFile.write(strout,strnlen(strout,1000));
+//    }
+//    xFile.close();
+
     checktrace(xPlan);
 //    std::cout<<"pp size is "<<xvectorPP.size()<<std::endl;
     return 0;
@@ -1813,7 +2196,7 @@ int MakePlan(xodrdijkstra * pxd,OpenDrive * pxodr,const double x_now,const doubl
 
     bianxiandis = pRoad->GetLaneSection(0)->GetLane(1)->GetWidthValue(1)/2.0;
 
-    int i = 1;
+//    int i = 1;
     for(i=1;i<nlane;i++)
     {
         bianxiandis = bianxiandis + pRoad->GetLaneSection(0)->GetLane(i)->GetWidthValue(1)/2.0

src/driver/driver_map_xodrload/globalplan.h

@@ -18,6 +18,12 @@ public:
     double mWidth;
     double mLeftWidth[5];
     double mRightWidth[5];
+
+    double mfRoadWidth;
+    double mfDisToRoadLeft;
+    double mfDisToLaneLeft;
+    int mnLaneori = 0;  //from Right 0
+    int mnLaneTotal = 1; //Lane Total
 };
 
 int GetNearPoint(const double x,const double y,OpenDrive * pxodr,Road ** pObjRoad,GeometryBlock ** pgeo, double & s,double & nearx,

src/driver/driver_map_xodrload/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/driver/driver_map_xodrload/main.cpp

@@ -391,9 +391,15 @@ static void ToGPSTrace(std::vector<PlanPoint> xPlan)
         givlog->debug(" %d %11.7f %11.7f %11.3f ",i,data->gps_lat,
                data->gps_lng,data->ins_heading_angle);
 
-        data->roadSum = 1;
+        data->roadSum = xPlan[i].mnLaneTotal;
         data->roadMode = 0;
-        data->roadOri = 0;
+        data->roadOri = xPlan[i].mnLaneori;
+
+        data->mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft;
+        data->mfDisToRoadLeft = xPlan[i].mfDisToRoadLeft;
+        data->mfLaneWidth = xPlan[i].mWidth;
+        data->mfRoadWidth = xPlan[i].mfRoadWidth;
+        data->mnLaneChangeMark = xPlan[i].lanmp;
 
         if(xPlan[i].lanmp == -1)data->roadMode = 15;
         if(xPlan[i].lanmp == 1)data->roadMode = 14;

src/driver/driver_radio_collision/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 	
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 };
 
 typedef boost::shared_ptr<iv::GPS_INS> GPSData;

src/driver/driver_radio_p900/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 };
 
 typedef boost::shared_ptr<iv::GPS_INS> GPSData;

src/tool/data_serials/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/tool/ivmapmake/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/tool/ivmapmake_sharemem/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/tool/map_lanetoxodr/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/tool/map_lanetoxodr_graphscene/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/tool/tool_mapcreate/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/tool/tool_xodrobj/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Road Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/tool/tracetoxodr/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/ui/ui_ads_hmi/common/gps_type.h

@@ -51,7 +51,12 @@ struct GPS_INS
     int roadSum;
     int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width
 
 
 

src/v2x/platform/common/gps_type.h

@@ -51,7 +51,12 @@ namespace iv {
         int roadSum;
         int roadOri;
 
-	double mfRoadWidth = 3.5; // Current Road Width
+    double mfLaneWidth = 3.5; // Current Lane Width
+
+    double mfDisToLaneLeft = 1.8; //Distance to Lane Left
+    int mnLaneChangeMark = 0; //1 to Left 0 not change   -1 to right
+    double mfDisToRoadLeft = 1.8; //Distance to Road Left
+    double mfRoadWidth = 3.5; // Road Width