change map_lanetoxodr, for add road extend.

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)
+void vtd_pilot::setOwnState(double x, double y, double z,double vx,double vy)
 {
     sOwnObjectState.base.id       = 1;
     sOwnObjectState.base.category = RDB_OBJECT_CATEGORY_PLAYER;
@@ -92,8 +92,8 @@ void vtd_pilot::setOwnState(double x, double y, double z)
     sOwnObjectState.base.pos.r     = 0.0;
     sOwnObjectState.base.pos.flags = RDB_COORD_FLAG_POINT_VALID | RDB_COORD_FLAG_ANGLES_VALID;
 
-    sOwnObjectState.ext.speed.x     = 0;
-    sOwnObjectState.ext.speed.y     = 0;
+    sOwnObjectState.ext.speed.x     = vx;
+    sOwnObjectState.ext.speed.y     = vy;
     sOwnObjectState.ext.speed.z     = 0;
     sOwnObjectState.ext.speed.h     = 0.0;
     sOwnObjectState.ext.speed.p     = 0.0;
@@ -135,10 +135,12 @@ void vtd_pilot::threadego()
 
         double x,y,z;
         CalcXY(xgpsimu.lat(),xgpsimu.lon(),mflat0,mflon0,x,y);
+        double vx = xgpsimu.ve();
+        double vy = xgpsimu.vn();
         z = 0;
-        setOwnState(x,y,z);
+        setOwnState(x,y,z,vx,vy);
 
-        RDB_OBJECT_STATE_t * pr = &sOwnObjectState;
+ //       RDB_OBJECT_STATE_t * pr = &sOwnObjectState;
 
         iv::modulecomm::ModuleSendMsg(mpaegostate,(char *)&sOwnObjectState,sizeof(RDB_OBJECT_STATE_t));
 

src/driver/vtd_pilot_if/vtd_pilot.h

@@ -24,7 +24,7 @@ private:
 
     void threadego();
 
-    void setOwnState(double x, double y, double z);
+    void setOwnState(double x, double y, double z,double vx,double vy);
 
 private:
     void * mpfromvtd, * mptovtd;

src/tool/map_lanetoxodr/createextendroad.cpp

@@ -0,0 +1,75 @@
+#include "createextendroad.h"
+
+#include <math.h>
+
+#include "mainwindow.h"
+
+extern MainWindow * gw;
+
+CreateExtendRoad::CreateExtendRoad()
+{
+
+}
+
+
+int CreateExtendRoad::CreateBeforeRoad(OpenDrive *pxodr, Road *pRoad, double fLength)
+{
+    std::string strroadid = pRoad->GetRoadId();
+    double x,y,hdg;
+    pRoad->GetGeometryCoords(0,x,y,hdg);
+
+    double xnew,ynew,hdgnew;
+    xnew = x + fLength * cos(hdg + M_PI);
+    ynew = y + fLength * sin(hdg + M_PI);
+    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)
+    {
+        LaneOffset * planeoff = pRoad->GetLaneOffset(0);
+        pnewroad->AddLaneOffset(0,planeoff->Geta(),0,0,0);
+    }
+
+
+
+    if(pRoad->GetLaneSectionCount()>0)
+    {
+        LaneSection * pLS = pRoad->GetLaneSection(0);
+        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(0),0,0,0);
+        }
+    }
+
+    if(pRoad->GetElevationCount()>0)
+    {
+        pRoad->AddElevation(0,pRoad->GetElevationValue(0),0,0,0);
+    }
+
+
+    return 0;
+}

src/tool/map_lanetoxodr/createextendroad.h

@@ -0,0 +1,14 @@
+#ifndef CREATEEXTENDROAD_H
+#define CREATEEXTENDROAD_H
+
+#include "OpenDrive/OpenDrive.h"
+
+class CreateExtendRoad
+{
+public:
+    CreateExtendRoad();
+
+    static int CreateBeforeRoad(OpenDrive * pxodr,Road * pRoad,double fLength);
+};
+
+#endif // CREATEEXTENDROAD_H

src/tool/map_lanetoxodr/mainwindow.cpp

@@ -4,6 +4,7 @@
 #include <iomanip>//不要忘记包含此头文件
 #include <QScrollBar>
 #include <QFileInfo>
+#include <QInputDialog>
 
 #include "gpsimu.pb.h"
 
@@ -5851,3 +5852,66 @@ void MainWindow::on_actionAdd_Road_From_NDS_triggered()
     DialogAddRoadFromNDS rnd(&mxodr, this);
     rnd.exec();
 }
+
+void MainWindow::on_CreateBefore_triggered()
+{
+    bool ok;
+    QString text = QInputDialog::getText(this, tr("Get Road ID"),
+                                         tr("Road ID:"), QLineEdit::Normal,
+                                         "10001", &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::CreateBeforeRoad(&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

@@ -84,6 +84,8 @@ using namespace Eigen;
 #include "xodrscenfunc.h"
 #include "autoroadcontact.h"
 
+#include "createextendroad.h"
+
 namespace Ui {
 class MainWindow;
 }
@@ -232,6 +234,8 @@ private slots:
 
     void on_actionAdd_Road_From_NDS_triggered();
 
+    void on_CreateBefore_triggered();
+
 private:
 
 

src/tool/map_lanetoxodr/mainwindow.ui

@@ -29,14 +29,14 @@
    </property>
    <widget class="QMenu" name="menuFile">
     <property name="title">
-     <string>File</string>
+     <string>Fi&amp;le</string>
     </property>
     <addaction name="actionLoad"/>
     <addaction name="actionSave"/>
    </widget>
    <widget class="QMenu" name="menuFunction">
     <property name="title">
-     <string>Function</string>
+     <string>F&amp;unction</string>
     </property>
     <addaction name="actionAutoConnect"/>
     <addaction name="actionSet_Speed"/>
@@ -58,10 +58,12 @@
     <addaction name="actionSummary_Road"/>
     <addaction name="actionDraw_Road"/>
     <addaction name="actionAdd_Road_From_NDS"/>
+    <addaction name="CreateBefore"/>
+    <addaction name="CreateAfter"/>
    </widget>
    <widget class="QMenu" name="menuView">
     <property name="title">
-     <string>View</string>
+     <string>&amp;View</string>
     </property>
     <addaction name="actionHide_Road"/>
     <addaction name="actionHide_Selected"/>
@@ -88,7 +90,7 @@
   <widget class="QStatusBar" name="statusBar"/>
   <action name="actionLoad">
    <property name="text">
-    <string>Load</string>
+    <string>&amp;Load</string>
    </property>
    <property name="toolTip">
     <string>Load XODR File</string>
@@ -96,7 +98,7 @@
   </action>
   <action name="actionSave">
    <property name="text">
-    <string>Save</string>
+    <string>&amp;Save</string>
    </property>
    <property name="toolTip">
     <string>Save XODR File</string>
@@ -104,22 +106,22 @@
   </action>
   <action name="actionAutoConnect">
    <property name="text">
-    <string>AutoConnect</string>
+    <string>&amp;AutoConnect</string>
    </property>
   </action>
   <action name="actionSet_Speed">
    <property name="text">
-    <string>Set Speed</string>
+    <string>&amp;Set Speed</string>
    </property>
   </action>
   <action name="actionSet_Traffic_Light">
    <property name="text">
-    <string>Edit Traffic Light</string>
+    <string>&amp;Edit Traffic Light</string>
    </property>
   </action>
   <action name="actionEdit_Road">
    <property name="text">
-    <string>Edit Road</string>
+    <string>&amp;Edit Road</string>
    </property>
    <property name="toolTip">
     <string>编辑道路</string>
@@ -127,17 +129,17 @@
   </action>
   <action name="actionAdd_Road_From_RTK">
    <property name="text">
-    <string>Add Road From RTK</string>
+    <string>&amp;Add Road From RTK</string>
    </property>
   </action>
   <action name="actionSummary_Road">
    <property name="text">
-    <string>Summary Road</string>
+    <string>&amp;Summary Road</string>
    </property>
   </action>
   <action name="actionCalc_Road_S">
    <property name="text">
-    <string>Calc Road S</string>
+    <string>&amp;Calc Road S</string>
    </property>
   </action>
   <action name="actionBack">
@@ -150,17 +152,17 @@
   </action>
   <action name="actionEdit_Road_Borrow">
    <property name="text">
-    <string>Edit Road Borrow</string>
+    <string>Edit &amp;Road Borrow</string>
    </property>
   </action>
   <action name="actionMake_All_Road_Contact">
    <property name="text">
-    <string>Make All Road Contact</string>
+    <string>&amp;Make All Road Contact</string>
    </property>
   </action>
   <action name="actionHide_Road">
    <property name="text">
-    <string>Hide Road</string>
+    <string>&amp;Hide Road</string>
    </property>
   </action>
   <action name="actionRoad_Optimize">
@@ -170,22 +172,38 @@
   </action>
   <action name="actionHide_Selected">
    <property name="text">
-    <string>Hide Selected</string>
+    <string>Hide &amp;Selected</string>
    </property>
   </action>
   <action name="actionDraw_Road">
    <property name="text">
-    <string>Draw Road</string>
+    <string>&amp;Draw Road</string>
    </property>
   </action>
   <action name="actionEdit_Road_Noavoid">
    <property name="text">
-    <string>Edit Road Noavoid</string>
+    <string>Edit Road &amp;Noavoid</string>
    </property>
   </action>
   <action name="actionAdd_Road_From_NDS">
    <property name="text">
-    <string>Add Road From NDS</string>
+    <string>Add &amp;Road From NDS</string>
+   </property>
+  </action>
+  <action name="CreateBefore">
+   <property name="text">
+    <string>CreateBefore</string>
+   </property>
+   <property name="toolTip">
+    <string>Create A road Before this road</string>
+   </property>
+  </action>
+  <action name="CreateAfter">
+   <property name="text">
+    <string>CreateAfter</string>
+   </property>
+   <property name="toolTip">
+    <string>Create a road after this road</string>
    </property>
   </action>
  </widget>

src/tool/map_lanetoxodr/map_lanetoxodr.pro

@@ -90,7 +90,8 @@ SOURCES += \
     geofit.cpp \
     circlefitting.cpp \
     xodrmake.cpp \
-    xodrscenfunc.cpp
+    xodrscenfunc.cpp \
+    createextendroad.cpp
 
 HEADERS += \
     autoconnect.h \
@@ -151,7 +152,8 @@ HEADERS += \
     geofit.h \
     circlefitting.h \
     xodrmake.h \
-    xodrscenfunc.h
+    xodrscenfunc.h \
+    createextendroad.h
 
 FORMS += \
         dialogaddroadfromnds.ui \