complete view_showxodrinvtk for test xodr show in vtk mode.

src/tool/map_lanetoxodr/view/roadviewitem.cpp

@@ -472,7 +472,7 @@ void roadviewitem::CalcLine()
             xline.mstartx = x;
             xline.mstarty = y *(-1.0);
             int ncount = pg->GetLength() * 10.0/mfratio;
-            double fstep;
+            double fstep = 10.0;
             if(ncount > 0)fstep = pg->GetLength()/ncount;
             int i;
             for(i=1;i<ncount;i++)

src/tool/view_showxodrinvtk/main.cpp

@@ -4,18 +4,12 @@
 
 #include <pcl/visualization/cloud_viewer.h>
 
+#include "showxodrinvtk.h"
+
 pcl::visualization::CloudViewer viewer1("Cloud Viewer");//创建viewer对象
 
-void ShowXODR(pcl::visualization::PCLVisualizer& viewer)
-{
-    pcl::PointXYZ mass_center(0,0,0);
-    pcl::PointXYZ kinectZ(0,0,-100);
 
-     viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,"xodr1");
 
-   pcl::PointXYZ kinectZ2(0,0,100);
-    viewer.addLine(mass_center,kinectZ2,1.0f,1.0f,1.0f,"xodr2");
-}
 
 void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
 {
@@ -23,7 +17,7 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
    viewer.setBackgroundColor(0.0,0.0,0.0);
    viewer.resetCamera();
 
-   ShowXODR(viewer);
+   ShowXODRINVTK(viewer);
 
 
 

src/tool/view_showxodrinvtk/showxodrinvtk.cpp

@@ -0,0 +1,128 @@
+
+#include "showxodrinvtk.h"
+
+#include <OpenDrive/OpenDrive.h>
+#include <OpenDrive/OpenDriveXmlParser.h>
+
+#include "xodrfunc.h"
+
+
+static int gnLineIndex = 0;
+
+
+void DrawLine(std::vector<iv::LanePoint> & xveclastlp,std::vector<iv::LanePoint> & xveclp,pcl::visualization::PCLVisualizer& viewer)
+{
+
+
+    unsigned int i,j;
+    for(i=0;i<xveclp.size();i++)
+    {
+        iv::LanePoint xNow = xveclp[i];
+        for(j=0;j<xveclastlp.size();j++)
+        {
+            if(xveclastlp[j].mnlane == xNow.mnlane)
+            {
+                iv::LanePoint xLast = xveclastlp[j];
+
+                pcl::PointXYZ mass_center(xLast.mfX,xLast.mfY,0);
+                pcl::PointXYZ kinectZ(xNow.mfX,xNow.mfY,0);
+
+                char strgeoname[256];
+                snprintf(strgeoname,256,"xodr%d",gnLineIndex);
+                gnLineIndex++;
+                if(xNow.mnlane != 0)
+                    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,strgeoname);
+                else
+                    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,strgeoname);
+
+                break;
+            }
+        }
+    }
+}
+
+void ShowRoad(pcl::visualization::PCLVisualizer& viewer,Road * pRoad)
+{
+
+    if(pRoad->GetLaneSectionCount()<1)
+    {
+        qDebug("no lane,return;");
+        return;
+    }
+
+
+
+    double fRoadLen = pRoad->GetRoadLength();
+
+    double s = 0;
+    double fSpace = 0.1;
+
+    double x,y,fhdg;
+    double flasthdg = 0;
+    double flastS = 0;
+    pRoad->GetGeometryCoords(s,x,y,fhdg);
+    std::vector<iv::LanePoint> xveclp = xodrfunc::GetAllLanePoint(pRoad,s,x,y,fhdg);
+    std::vector<iv::LanePoint> xveclastlp = xveclp;
+    flasthdg = fhdg;
+    flastS = s;
+    s = s+ fSpace;
+
+    while(s<fRoadLen)
+    {
+        pRoad->GetGeometryCoords(s,x,y,fhdg);
+        xveclp = xodrfunc::GetAllLanePoint(pRoad,s,x,y,fhdg);
+
+        double fhdgdiff =  fhdg - flasthdg;
+        double fSDiff = s - flastS;
+
+        if((fabs(fhdgdiff) > 0.01) || (fSDiff>= 10.0))
+        {
+
+            //Draw Line
+            DrawLine(xveclastlp,xveclp,viewer);
+            xveclastlp = xveclp;
+            flasthdg = fhdg;
+            flastS = s;
+        }
+
+        s= s + fSpace;
+    }
+
+    if(flastS<fRoadLen)
+    {
+        pRoad->GetGeometryCoords(fRoadLen,x,y,fhdg);
+        xveclp = xodrfunc::GetAllLanePoint(pRoad,s,x,y,fhdg);
+        DrawLine(xveclastlp,xveclp,viewer);
+    }
+
+
+
+
+}
+
+void ShowXODRINVTK(pcl::visualization::PCLVisualizer& viewer)
+{
+
+    char strpath[256];
+    snprintf(strpath,256,"%s/map/map.xodr",getenv("HOME"));
+
+    OpenDrive xxodr;
+
+    OpenDriveXmlParser xp(&xxodr);
+    xp.ReadFile(strpath);
+
+    unsigned int i;
+    for(i=0;i<xxodr.GetRoadCount();i++)
+    {
+        ShowRoad(viewer,xxodr.GetRoad(i));
+    }
+
+
+//    pcl::PointXYZ mass_center(0,0,0);
+//    pcl::PointXYZ kinectZ(0,0,-100);
+
+//    viewer.addLine(mass_center,kinectZ,1.0f,1.0f,1.0f,"xodr1");
+
+//    pcl::PointXYZ kinectZ2(0,0,100);
+//    viewer.addLine(mass_center,kinectZ2,1.0f,1.0f,1.0f,"xodr2");
+}

src/tool/view_showxodrinvtk/showxodrinvtk.h

@@ -0,0 +1,8 @@
+#ifndef SHOWXODRINVTK_H
+#define SHOWXODRINVTK_H
+
+#include <pcl/visualization/cloud_viewer.h>
+
+void ShowXODRINVTK(pcl::visualization::PCLVisualizer& viewer);
+
+#endif // SHOWXODRINVTK_H

src/tool/view_showxodrinvtk/view_showxodrinvtk.pro

@@ -17,7 +17,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 SOURCES += main.cpp \
     const.cpp \
     fresnl.cpp \
-    polevl.c
+    polevl.c \
+    showxodrinvtk.cpp
 
 !include(../../common/common/xodr/OpenDrive/OpenDrive.pri ) {
     error( "Couldn't find the OpenDrive.pri file!" )
@@ -27,10 +28,14 @@ SOURCES += main.cpp \
     error( "Couldn't find the TinyXML.pri file!" )
 }
 
+!include(../../common/common/xodr/xodrfunc/xodrfunc.pri ) {
+    error( "Couldn't find the xodrfunc.pri file!" )
+}
 
-INCLUDEPATH += $$PWD/../../common/common/xodr
 
+INCLUDEPATH += $$PWD/../../common/common/xodr
 
+INCLUDEPATH += $$PWD/../../common/common/xodr/xodrfunc
 
 QMAKE_LFLAGS += -no-pie
 
@@ -42,6 +47,9 @@ INCLUDEPATH += /usr/include/vtk-6.3
 INCLUDEPATH += /usr/include/vtk-6.2
 INCLUDEPATH += /usr/include/vtk-7.1
 
+
+DEFINES += NOTINPILOT
+
 unix:LIBS +=  -lpcl_common\
         -lpcl_features\
         -lpcl_filters\
@@ -69,4 +77,5 @@ LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3
 #        -lvtkFiltersSources-7.1
 
 HEADERS += \
-    mconf.h
+    mconf.h \
+    showxodrinvtk.h