chagne driver_map_xodrload. add sidelane stop.

src/driver/driver_map_xodrload/main.cpp

@@ -48,6 +48,7 @@ double gvehiclewidth = 2.0;
 bool gbExtendMap = true;
 
 static bool gbSideEnable = false;
+static bool gbSideLaneEnable = false;
 
 void * gpa;
 void * gpasrc;
@@ -473,6 +474,42 @@ inline bool isboringroad(int nroadid)
 }
 
 
+void CalcLaneSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+
+    int i;
+    int nsize = xPlan.size();
+
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+   //     double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        xPlan[i].x = xold + fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fMove;
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        double fMove = xPlan[i].mfRoadWidth - xPlan[i].mfDisToRoadLeft - (gvehiclewidth/2.0 + fsidedis);
+//        double fMove = xPlan[i].mWidth/2.0 - (gvehiclewidth/2.0 + fsidedis);
+        double xold = xPlan[i].x;
+        double yold = xPlan[i].y;
+        double fRatio = 1.0 - ((nsize-nStopPoint) -i )*1.0/(nChangePoint-nStopPoint);
+        xPlan[i].x = xold + fRatio*fMove*cos(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].y = yold + fRatio*fMove*sin(xPlan[i].hdg - M_PI/2.0);
+        xPlan[i].mfDisToLaneLeft = xPlan[i].mfDisToLaneLeft + fRatio*fMove;
+    }
+    return;
+}
+
 void CalcSide(std::vector<PlanPoint> & xPlan)
 {
     const double fsidedis = 0.3;
@@ -543,9 +580,16 @@ void SetPlan(xodrobj xo)
     int i;
     int nSize = xPlan.size();
 
-    if(gbSideEnable)
+    if(gbSideLaneEnable)
     {
-        CalcSide(xPlan);
+        CalcLaneSide(xPlan);
+    }
+    else
+    {
+        if(gbSideEnable)
+        {
+            CalcSide(xPlan);
+        }
     }
 
     if(nSize<1)
@@ -1112,6 +1156,8 @@ int main(int argc, char *argv[])
 
     std::string strsideenable = xp.GetParam("sideenable","false");
 
+    std::string strsidelaneenable = xp.GetParam("sidelaneenable","false");
+
 
     glat0 = atof(strlat0.data());
     glon0 = atof(strlon0.data());
@@ -1133,6 +1179,11 @@ int main(int argc, char *argv[])
         gbSideEnable = true;
     }
 
+    if(strsidelaneenable == "true")
+    {
+        gbSideLaneEnable = true;
+    }
+
 
     LoadXODR(strmapth);