change map_lanetoxodr, add road extend before or after.

src/driver/vtd_pilot_if/vtd_pilot.cpp

@@ -66,7 +66,7 @@ void vtd_pilot::UpdateVTD(const char *strdata, const unsigned int nSize, const u
     }
 }
 
-void vtd_pilot::setOwnState(double x, double y, double z,double vx,double vy)
+void vtd_pilot::setOwnState(double x, double y, double z,double vx,double vy,double fheading)
 {
     sOwnObjectState.base.id       = 1;
     sOwnObjectState.base.category = RDB_OBJECT_CATEGORY_PLAYER;
@@ -87,7 +87,7 @@ void vtd_pilot::setOwnState(double x, double y, double z,double vx,double vy)
     sOwnObjectState.base.pos.x     = x;
     sOwnObjectState.base.pos.y     = y;
     sOwnObjectState.base.pos.z     = z;
-    sOwnObjectState.base.pos.h     = 0.0;
+    sOwnObjectState.base.pos.h     = fheading;
     sOwnObjectState.base.pos.p     = 0.0;
     sOwnObjectState.base.pos.r     = 0.0;
     sOwnObjectState.base.pos.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;
@@ -134,11 +134,12 @@ void vtd_pilot::threadego()
         mmutex.unlock();
 
         double x,y,z;
-        CalcXY(xgpsimu.lat(),xgpsimu.lon(),mflat0,mflon0,x,y);
+        CalcXY(mflat0,mflon0,xgpsimu.lat(),xgpsimu.lon(),x,y);
         double vx = xgpsimu.ve();
         double vy = xgpsimu.vn();
         z = 0;
-        setOwnState(x,y,z,vx,vy);
+        double fheading = (90 - xgpsimu.heading())*M_PI/180.0;
+        setOwnState(x,y,z,vx,vy,fheading);
 
  //       RDB_OBJECT_STATE_t * pr = &sOwnObjectState;
 

src/driver/vtd_pilot_if/vtd_pilot.h

@@ -24,7 +24,7 @@ private:
 
     void threadego();
 
-    void setOwnState(double x, double y, double z,double vx,double vy);
+    void setOwnState(double x, double y, double z,double vx,double vy,double fheading);
 
 private:
     void * mpfromvtd, * mptovtd;
@@ -34,8 +34,8 @@ private:
 
     std::thread * mpthread;
 
-    double mflat0 = 39.12017769;
-    double mflon0 = 117.0280226;
+    double mflat0 = 39.1235363;
+    double mflon0 = 117.0272538;
 
     iv::gps::gpsimu mgpsimu;
     bool mbupdate_gps = false;

src/driver/vtd_rdb/rdbconn.h

@@ -53,7 +53,7 @@ private:
     std::mutex mmutexcv;
     std::condition_variable mcv;
     std::vector<iv::rdbitem> mvectorrdbitem;
-    const  unsigned int rdbitembufsize = 1000;
+    const  unsigned int rdbitembufsize = 10000;
 
 private:
     void threadconn(char * strserip,int nport);

src/tool/map_lanetoxodr/createextendroad.cpp

@@ -71,5 +71,66 @@ int CreateExtendRoad::CreateBeforeRoad(OpenDrive *pxodr, Road *pRoad, double fLe
     }
 
 
+    return 0;
+}
+
+int CreateExtendRoad::CreateAfterRoad(OpenDrive *pxodr, Road *pRoad, double fLength)
+{
+    std::string strroadid = pRoad->GetRoadId();
+    double x,y,hdg;
+    pRoad->GetGeometryCoords(pRoad->GetRoadLength() - 0.00001,x,y,hdg);
+
+    double xnew,ynew,hdgnew;
+    xnew = x ;
+    ynew = y ;
+    hdgnew = hdg;
+
+    int nroadid = gw->CreateRoadID();
+    pxodr->AddRoad("",fLength,QString::number(nroadid).toStdString(),"-1");
+
+    int i;
+
+    for(i=0;i<(int)pxodr->GetRoadCount();i++)
+    {
+        if(pxodr->GetRoad(i)->GetRoadId() == strroadid)
+        {
+            pRoad = pxodr->GetRoad(i);
+            break;
+        }
+    }
+
+    Road * pnewroad = pxodr->GetLastAddedRoad();
+
+    pnewroad->AddGeometryBlock();
+    GeometryBlock * pgeob = pnewroad->GetLastAddedGeometryBlock();
+    pgeob->AddGeometryLine(0,xnew,ynew,hdgnew,fLength);
+
+    if(pRoad->GetLaneOffsetCount()>0)
+    {
+        pnewroad->AddLaneOffset(0,pRoad->GetLaneOffsetValue(pRoad->GetRoadLength()-0.00001),0,0,0);
+    }
+
+
+
+    if(pRoad->GetLaneSectionCount()>0)
+    {
+        LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
+        pnewroad->AddLaneSection(0);
+        LaneSection * pLSnew = pnewroad->GetLastAddedLaneSection();
+        for(i=0;i<(int)pLS->GetLaneCount();i++)
+        {
+            Lane * pLane = pLS->GetLane(i);
+            pLSnew->AddLane(pLane->GetSide(),pLane->GetId(),pLane->GetType(),pLane->GetLevel());
+            Lane * pnewlane = pLSnew->GetLastAddedLane();
+            pnewlane->AddWidthRecord(0,pLane->GetWidthValue(pRoad->GetRoadLength()-0.00001),0,0,0);
+        }
+    }
+
+    if(pRoad->GetElevationCount()>0)
+    {
+        pRoad->AddElevation(0,pRoad->GetElevationValue(pRoad->GetRoadLength() - 0.00001),0,0,0);
+    }
+
+
     return 0;
 }

src/tool/map_lanetoxodr/createextendroad.h

@@ -9,6 +9,8 @@ public:
     CreateExtendRoad();
 
     static int CreateBeforeRoad(OpenDrive * pxodr,Road * pRoad,double fLength);
+
+    static int CreateAfterRoad(OpenDrive * pxodr,Road * pRoad,double fLength);
 };
 
 #endif // CREATEEXTENDROAD_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -2962,6 +2962,11 @@ void MainWindow::onClickLoad()
     mbRefresh = true;
     update();
 
+    if(mpCBViewMode->currentIndex() == 1)
+    {
+        UpdateScene();
+    }
+
     mpfb->Activate(strfilepath);
 
     delete pxodr;
@@ -5858,7 +5863,7 @@ void MainWindow::on_CreateBefore_triggered()
     bool ok;
     QString text = QInputDialog::getText(this, tr("Get Road ID"),
                                          tr("Road ID:"), QLineEdit::Normal,
-                                         "10001", &ok);
+                                         mpCBRoad->currentText(), &ok);
     Road * pRoad =NULL;
     if (ok && !text.isEmpty())
     {
@@ -5915,3 +5920,64 @@ void MainWindow::on_CreateBefore_triggered()
 
 
 }
+
+void MainWindow::on_CreateAfter_triggered()
+{
+    bool ok;
+    QString text = QInputDialog::getText(this, tr("Get Road ID"),
+                                         tr("Road ID:"), QLineEdit::Normal,
+                                         mpCBRoad->currentText(), &ok);
+    Road * pRoad =NULL;
+    if (ok && !text.isEmpty())
+    {
+        std::string strroadid = text.toStdString();
+        int i;
+        for(i=0;i<(int)mxodr.GetRoadCount();i++)
+        {
+            if(mxodr.GetRoad(i)->GetRoadId() == strroadid)
+            {
+                pRoad = mxodr.GetRoad(i);
+                break;
+            }
+        }
+        if(pRoad == NULL)
+        {
+            QMessageBox::warning(this,tr("Warning"),tr("Road is not exist."),QMessageBox::YesAll);
+            return;
+        }
+    }
+    else
+    {
+        return;
+    }
+
+    double fLength = 0.0;
+
+    text = QInputDialog::getText(this, tr("Set Length"),
+                                         tr("Length:"), QLineEdit::Normal,
+                                         "100.0", &ok);
+    if (ok && !text.isEmpty())
+    {
+        fLength = text.toDouble();
+        if(fLength<=0)
+        {
+            QMessageBox::warning(this,tr("Warning"),tr("Road Lenth is negative or zero."),QMessageBox::YesAll);
+            return;
+        }
+    }
+
+    mpfb->SetOpenDrive(mxodr);
+
+    CreateExtendRoad::CreateAfterRoad(&mxodr,pRoad,fLength);
+
+    std::cout<<"create road."<<std::endl;
+
+    updateJunction();
+    updateCBRoad();
+    mbRefresh = true;
+    update();
+    if(mpCBViewMode->currentIndex() == 1)
+    {
+        UpdateScene();
+    }
+}

src/tool/map_lanetoxodr/mainwindow.h

@@ -236,6 +236,8 @@ private slots:
 
     void on_CreateBefore_triggered();
 
+    void on_CreateAfter_triggered();
+
 private: