change driver_map_xodrload. add side stop.

src/driver/driver_map_xodrload/main.cpp

@@ -47,6 +47,8 @@ double gvehiclewidth = 2.0;
 
 bool gbExtendMap = true;
 
+static bool gbSideEnable = false;
+
 void * gpa;
 void * gpasrc;
 void * gpmap;
@@ -470,6 +472,49 @@ inline bool isboringroad(int nroadid)
     return brtn;
 }
 
+
+void CalcSide(std::vector<PlanPoint> & xPlan)
+{
+    const double fsidedis = 0.3;
+    const int nChangePoint = 150;
+    const int nStopPoint = 50;
+    if(xPlan.size()<nChangePoint)return;
+    bool bChange = true;
+    int i;
+    int nsize = xPlan.size();
+    for(i=(nsize-1);i>=(nsize - nChangePoint);i--)
+    {
+        if(xPlan[i].mWidth<(2.0*(gvehiclewidth/2.0+fsidedis)))
+        {
+            std::cout<<" Because Lane is narrow, not change."<<std::endl;
+            bChange = false;
+            break;
+        }
+    }
+
+    if(bChange == false)return;
+
+    for(i=(nsize-1);i>=(nsize - nStopPoint);i--)
+    {
+        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);
+    }
+
+    for(i=(nsize-nStopPoint-1);i>=(nsize - nChangePoint);i--)
+    {
+        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);
+    }
+    return;
+}
+
 void SetPlan(xodrobj xo)
 {
 
@@ -496,6 +541,11 @@ void SetPlan(xodrobj xo)
     int i;
     int nSize = xPlan.size();
 
+    if(gbSideEnable)
+    {
+        CalcSide(xPlan);
+    }
+
     if(nSize<1)
     {
         qDebug("plan fail.");
@@ -1058,6 +1108,8 @@ int main(int argc, char *argv[])
 
     std::string strextendmap = xp.GetParam("extendmap","false");
 
+    std::string strsideenable = xp.GetParam("sideenable","false");
+
 
     glat0 = atof(strlat0.data());
     glon0 = atof(strlon0.data());
@@ -1074,6 +1126,11 @@ int main(int argc, char *argv[])
         gbExtendMap = false;
     }
 
+    if(strsideenable == "true")
+    {
+        gbSideEnable = true;
+    }
+
 
     LoadXODR(strmapth);
 

src/driver/driver_map_xodrload/planpoint.h

@@ -16,7 +16,7 @@ public:
     double hdg;
     double dis;
     double mS;
-    double mWidth;
+    double mWidth;  //Current Lane Width
     double mLeftWidth[5];
     double mRightWidth[5];