change simple_planning_simulator, add trafficlight show, complete.

src/tool/map_lanetoxodr/view/roaddigit.cpp

@@ -20,12 +20,39 @@ std::vector<iv::ParkingPoint> * RoadDigit::GetParkingPoint()
     return &mvectorParkingPoint;
 }
 
+std::vector<iv::SignlaPoint> * RoadDigit::GetSignalPoint()
+{
+    return &mvectorSignalPoint;
+}
+
 void RoadDigit::UpdateSpace(double fspace)
 {
     if(mpRoad == 0)return;
     CalcLine(fspace);
     CalcLane();
     CalcParkingPoint();
+
+    unsigned int i;
+    unsigned int nSigCount = mpRoad->GetSignalCount();
+    for(i=0;i<nSigCount;i++)
+    {
+        Signal * pSig = mpRoad->GetSignal(i);
+        double s = pSig->Gets();
+        double t = pSig->Gett();
+        double x,y,hdg;
+        if(mpRoad->GetGeometryCoords(s,x,y,hdg)>=0)
+        {
+            iv::SignalXY sxy;
+            sxy.mX = x + t * cos(hdg +M_PI/2.0);
+            sxy.mY = y + t * sin(hdg + M_PI/2.0);
+            iv::SignlaPoint spoint;
+            spoint.mSignalXY = sxy;
+            spoint.strtype = pSig->Gettype();
+            spoint.strsubtype = pSig->Getsubtype();
+            spoint.mfHdg = hdg;
+            mvectorSignalPoint.push_back(spoint);
+        }
+    }
 }
 
 int RoadDigit::GetSectionIndex(Road *pRoad, double s)

src/tool/map_lanetoxodr/view/roaddigit.h

@@ -48,6 +48,20 @@ struct ParkingPoint
     double mfHdg;
 };
 
+struct SignalXY
+{
+    double mX;
+    double mY;
+};
+
+struct SignlaPoint
+{
+    SignalXY mSignalXY;
+    string strtype;
+    string strsubtype;
+    double mfHdg;
+};
+
 }
 
 class RoadDigit
@@ -59,6 +73,7 @@ private:
     std::vector<iv::RoadDigitUnit> mvectorRDU;
     Road * mpRoad = 0;
     std::vector<iv::ParkingPoint> mvectorParkingPoint;
+    std::vector<iv::SignlaPoint> mvectorSignalPoint;
 
 private:
     void CalcLine(double fspace);
@@ -70,6 +85,7 @@ public:
     void UpdateSpace(double fspace);
 
     std::vector<iv::ParkingPoint> * GetParkingPoint();
+    std::vector<iv::SignlaPoint> * GetSignalPoint();
 
 private:
     int GetSectionIndex(Road * pRoad,double s);

src/tool/map_lanetoxodr/view/xodrscenfunc.cpp

@@ -172,6 +172,26 @@ int xodrscenfunc::DefineMarkType(std::string strmarktype)
     return -1;
 }
 
+std::vector<QGraphicsPixmapItem * > xodrscenfunc::GetTrafficLightItem(RoadDigit * prd,std::vector<iv::SignlaPoint> & xsigpoint)
+{
+    std::vector<QGraphicsPixmapItem  *> xvectorpixmapitem;
+
+    std::vector<iv::SignlaPoint> * pvectorSP  = prd->GetSignalPoint();
+    unsigned int i;
+    unsigned int nsize = static_cast<unsigned int> (pvectorSP->size());
+    for(i=0;i<nsize;i++)
+    {
+
+        QGraphicsPixmapItem * ppix;
+        ppix = new QGraphicsPixmapItem();
+        ppix->setPixmap(QPixmap(":/TrafficLight.png"));
+        xsigpoint.push_back( pvectorSP->at(i));
+        xvectorpixmapitem.push_back(ppix);
+
+    }
+    return xvectorpixmapitem;
+}
+
 std::vector<QGraphicsPathItem *> xodrscenfunc::GetRoadViewItem(iv::RoadSample * pRS)
 {
     std::vector<QGraphicsPathItem *> xvectorgrapath;

src/tool/map_lanetoxodr/view/xodrscenfunc.h

@@ -5,6 +5,8 @@
 
 #include <QGraphicsPathItem>
 
+#include <QGraphicsPixmapItem>
+
 #include "xodrmake.h"
 
 #include "roaddigit.h"
@@ -26,6 +28,8 @@ public:
 
     static std::vector<QGraphicsPathItem *> GetRoadViewItem(iv::RoadSample * pRS);
 
+    static std::vector<QGraphicsPixmapItem * > GetTrafficLightItem(RoadDigit * prd,std::vector<iv::SignlaPoint> & xsigpoint);
+
 public:
     static bool IsDrawMark(double s);
 

src/tool/simple_planning_simulator/mainwindow.cpp

@@ -578,6 +578,40 @@ void MainWindow::UpdateScene()
         }
     }
 
+    for(i=0;i<nsize;i++)
+    {
+        std::vector<iv::SignlaPoint> xvectorsigpoint;
+        std::vector<QGraphicsPixmapItem *> xvectorpix = xodrscenfunc::GetTrafficLightItem(&(xvectorrd[i]),xvectorsigpoint);
+
+        int j;
+        int ncount = xvectorpix.size();
+        for(j=0;j<ncount;j++)
+        {
+            QGraphicsPixmapItem * ppix = xvectorpix[j];
+            QRectF xr = ppix->boundingRect();
+            double fvehwidth = 2.0;
+            double fvehlen = fvehwidth*(xr.width()/xr.height());
+            double fhdg = xvectorsigpoint.at(j).mfHdg + M_PI/2.0;
+            double fhead = fhdg * 180.0/M_PI *(-1.0);
+            double fscale =  2.0/xr.height();
+
+            double x0,y0;
+            x0 = fvehlen/2.0;
+            y0 = fvehwidth/2.0 *(-1.0);
+            double x1,y1;
+            double hdgrt = fhdg;//法 fhead *M_PI/180.0;
+            x1 = x0 * cos(hdgrt) - y0 * sin(hdgrt);
+            y1 = x0 * sin(hdgrt) + y0 * cos(hdgrt);
+
+            ppix->setRotation(fhead );
+            ppix->setScale(fscale);
+            ppix->setPos(mfViewMoveX +VIEW_WIDTH/2.0 + xvectorsigpoint.at(j).mSignalXY.mX - x1 ,-mfViewMoveY+VIEW_HEIGHT/2.0 - xvectorsigpoint.at(j).mSignalXY.mY+ y1);
+            mpscene->addItem(ppix);
+            ppix->setZValue(90.0);
+        }
+
+    }
+
 
     SetCarPos(30,10,M_PI/2.0);
 

src/tool/view_ndtmatching/mainwindow.cpp

@@ -458,7 +458,7 @@ void MainWindow::threadpcdview()
 
 
      //This will only get called once
-     mpviewer->runOnVisualizationThreadOnce (viewerOneOff);
+ //    mpviewer->runOnVisualizationThreadOnce (viewerOneOff);
 
 
      std::this_thread::sleep_for(std::chrono::milliseconds(1000));

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -34,10 +34,12 @@ INCLUDEPATH += /opt/ros/kinetic/include
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/pcl-1.10
 INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/pcl-1.12
 INCLUDEPATH += /usr/include/eigen3
 INCLUDEPATH += /usr/include/vtk-6.3
 INCLUDEPATH += /usr/include/vtk-6.2
 INCLUDEPATH += /usr/include/vtk-7.1
+INCLUDEPATH += /usr/include/vtk-9.1
 
 INCLUDEPATH += $$PWD/../view_showxodrinvtk
 
@@ -68,8 +70,11 @@ LIBS += -lshowxodrinvtk
 INCLUDEPATH += $$PWD/../../common/common/math/
 
 LIBS += -lboost_system
-LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
-        -lvtkFiltersSources-6.3
+#LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
+#        -lvtkFiltersSources-6.3
+
+LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+        -lvtkFiltersSources-9.1
 
 #LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
 #        -lvtkFiltersSources-7.1

src/tool/view_showxodrinvtk/showxodrinvtk.pro

@@ -43,13 +43,16 @@ QMAKE_LFLAGS += -no-pie
 INCLUDEPATH += /usr/include/eigen3
 INCLUDEPATH += /usr/include/pcl-1.8
 INCLUDEPATH += /usr/include/pcl-1.10
+INCLUDEPATH += /usr/include/pcl-1.12
 INCLUDEPATH += /usr/include/pcl-1.7
 INCLUDEPATH += /usr/include/vtk-6.3
 INCLUDEPATH += /usr/include/vtk-6.2
 INCLUDEPATH += /usr/include/vtk-7.1
+INCLUDEPATH += /usr/include/vtk-9.1
 
 
 DEFINES += NOTINPILOT
+DEFINES += XODRLOAD
 
 unix:LIBS +=  -lpcl_common\
         -lpcl_features\
@@ -71,8 +74,11 @@ unix:LIBS +=  -lpcl_common\
         -lpcl_visualization
 
 LIBS += -lboost_system
-LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
-        -lvtkFiltersSources-6.3
+#LIBS += -lvtkCommonExecutionModel-6.3 -lvtkCommonCore-6.3 -lvtkRenderingLOD-6.3 -lvtkRenderingCore-6.3 \
+#        -lvtkFiltersSources-6.3
+
+LIBS += -lvtkCommonExecutionModel-9.1 -lvtkCommonCore-9.1 -lvtkRenderingLOD-9.1 -lvtkRenderingCore-9.1 \
+        -lvtkFiltersSources-9.1
 
 #LIBS += -lvtkCommonExecutionModel-7.1 -lvtkCommonCore-7.1 -lvtkRenderingLOD-7.1 -lvtkRenderingCore-7.1 \
 #        -lvtkFiltersSources-7.1