change tool/map_lanetoxodr. change rtk to road function.

src/tool/map_lanetoxodr/dialogaddroadfromrtk.cpp

@@ -332,7 +332,7 @@ void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()
         return;
     }
 
-    double LINE_ERROR = 0.1;
+    double LINE_ERROR = 0.05;
 
 
     std::vector<geobase> xvectorgeo;
@@ -341,6 +341,9 @@ void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()
     int j;
     int ncurpos = 0;
     int nrange = mvectorrtkdata.size();
+    double fXLast;
+    double fYLast;
+    bool bFirst = true;
     while(bComplete == false)
     {
 
@@ -412,6 +415,19 @@ void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()
                 xgeo.mfY = y0;
                 xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
                 xgeo.mnType = 0;
+                if(bFirst)
+                {
+                    xgeo.mfX = x_veh[0];
+                    xgeo.mfY = y_veh[0];
+                    bFirst = false;
+                }
+                else
+                {
+                    xgeo.mfX = fXLast;
+                    xgeo.mfY = fYLast;
+                }
+                fXLast = xgeo.mfX + xgeo.mfLen * cos(xgeo.mfHdg);
+                fYLast = xgeo.mfY + xgeo.mfLen * sin(xgeo.mfHdg);
                 xvectorgeo.push_back(xgeo);
             }
             else
@@ -432,6 +448,23 @@ void DialogAddRoadFromRTK::on_pushButton_CreateRoad_clicked()
                     xgeo.mfEndX = ep.x();
                     xgeo.mfEndY = ep.y();
                     xgeo.mR = fR;
+                    if(bFirst)
+                    {
+                        xgeo.mfX = x_veh[0];
+                        xgeo.mfY = y_veh[0];
+                        xgeo.mfEndX = ep.x() + (x_veh[0] - sp.x());
+                        xgeo.mfEndY = ep.y() + (y_veh[0] - sp.y());
+                        bFirst = false;
+                    }
+                    else
+                    {
+                        xgeo.mfX = fXLast;
+                        xgeo.mfY = fYLast;
+                        xgeo.mfEndX = ep.x() + (fXLast - sp.x());
+                        xgeo.mfEndY = ep.y() + (fYLast - sp.y());
+                    }
+                    fXLast = xgeo.mfEndX;
+                    fYLast = xgeo.mfEndY;
                     xvectorgeo.push_back(xgeo);
                 }
             }