change view_ndtmatching.

src/tool/view_ndtmatching/mainwindow.cpp

@@ -8,6 +8,8 @@
 
 #include "gnss_coordinate_convert.h"
 
+#include "gpsimu.pb.h"
+
 #include <QMessageBox>
 #include <QFile>
 
@@ -208,6 +210,12 @@ MainWindow::MainWindow(QWidget *parent) :
 
     mparelocate = iv::modulecomm::RegisterSend("relocate",10000,1);
 
+    ModuleFun fungpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpagpsimu = iv::modulecomm::RegisterRecvPlus("hcp2_gpsimu",fungpsimu);
+
+    ModuleFun funfusiongpsimu =std::bind(&MainWindow::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpafusiongpsimu  = iv::modulecomm::RegisterRecvPlus("fusion_gpsimu",funfusiongpsimu);
+
     connect(this,SIGNAL(ndtposupdate()),this,SLOT(onndtposupdate()));
 
     setWindowTitle("NDT Matching View & Relocation Enable");
@@ -249,10 +257,21 @@ void MainWindow::threadpcdview()
      mpviewer->runOnVisualizationThreadOnce (viewerOneOff);
 
 
+     std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
 
      mpviewer->runOnVisualizationThread (viewerPsycho);
 
      pcl::io::loadPCDFile<pcl::PointXYZI>(strpcpath,*point_cloud);
+
+     double flon,flat,fheading;
+     ReadNDTOrigin(strpcpath,flon,flat,fheading);
+
+     if(mbLoadXODR)
+     {
+         ChangePointCloud(point_cloud);
+     }
+
      mpviewer->showCloud(point_cloud);
 
 
@@ -271,7 +290,12 @@ void MainWindow::threadpcdview()
                          new pcl::PointCloud<pcl::PointXYZI>());
 
               pcl::io::loadPCDFile<pcl::PointXYZI>(strpcdmap,*point_cloud2);
+              ReadNDTOrigin(strpcpath,flon,flat,fheading);
               mpviewer->showCloud(point_cloud2);
+              if(mbLoadXODR)
+              {
+                  ChangePointCloud(point_cloud2);
+              }
               std::cout<<" use new pcd map. path: "<<strpcdmap.data()<<std::endl;
          }
 
@@ -366,6 +390,51 @@ void MainWindow::on_comboBox_currentIndexChanged(int index)
     gnViewMode = index;
 }
 
+void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    iv::gps::gpsimu  xgpsimu;
+    if(!xgpsimu.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" UpdateGPSIMU Parse fail."<<std::endl;
+        return;
+    }
+
+    if(strcmp("hcp2_gpsimu",strmemname) == 0)
+    {
+        int64_t nmsnow = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+        if(abs(nmsnow - mnLastFusionGPSIMUUpdate)<3000)
+        {
+            return;
+        }
+    }
+    else
+    {
+        mnLastFusionGPSIMUUpdate = std::chrono::system_clock::now().time_since_epoch().count()/1000000;
+    }
+
+
+
+    if(mbLoadXODR)
+    {
+
+        double fx0,fy0;
+        double fx,fy;
+        GaussProjCal(mflon0,mflat0,&fx0,&fy0);
+        GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&fx,&fy);
+
+        gCurPose.x = fx - fx0;
+        gCurPose.y = fy - fy0;
+        gCurPose.z = 0;
+        gCurPose.yaw = (90 - xgpsimu.heading())*M_PI/180.0;
+        gCurPose.pitch = 0;
+        gCurPose.roll = 0;
+    }
+
+
+}
+
+
 void MainWindow::UpdateNDTPos(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
     (void)index;
@@ -461,7 +530,7 @@ bool MainWindow::ReadNDTOrigin(std::string strpcdpath,double & flon, double & fl
     QString str2;
     if(str1.length()<5)return false;
     str2 = str1.left(str1.length() -4);
-    str2.append(".txt");
+    str2.append(".ori");
 //    qDebug(str2.toLatin1().data());
 
     QFile xFile;;
@@ -529,7 +598,7 @@ void MainWindow::ChangePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_c
         x = rawx * cos(mfRelBaseYaw) - rawy* sin(mfRelBaseYaw) + mfRelBaseX;
         y = rawx * sin(mfRelBaseYaw) + rawy* cos(mfRelBaseYaw) + mfRelBaseY;
         point_cloud->points.at(i).x = x;
-        point_cloud->points.at(i).x = y;
+        point_cloud->points.at(i).y = y;
 
     }
 }

src/tool/view_ndtmatching/mainwindow.h

@@ -67,6 +67,9 @@ private:
     void UpdateNDTPos(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
     void UpdatePCDMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
+    void UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
+
     void ChangePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
 
 private:
@@ -81,6 +84,8 @@ private:
     void * mpandtpos;
     void * mparelocate;
     void * mpapcdmap;
+    void * mpagpsimu;
+    void *mpafusiongpsimu;
 
     pose mCurPose;
 
@@ -101,6 +106,8 @@ private:
     double mfRelBaseY;
     double mfRelBaseYaw;
 
+    int64_t mnLastFusionGPSIMUUpdate = 0;
+
 
 
 

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -21,7 +21,10 @@ SOURCES += main.cpp \
     ../../include/msgtype/ndtpos.pb.cc \
     ../../include/msgtype/relocate.pb.cc \
     ../../include/msgtype/pcdmap.pb.cc \
-    ../../common/common/math/gnss_coordinate_convert.cpp
+    ../../common/common/math/gnss_coordinate_convert.cpp \
+    ../../include/msgtype/gps.pb.cc \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/imu.pb.cc
 
 QMAKE_LFLAGS += -no-pie
 
@@ -77,7 +80,10 @@ HEADERS += \
     ../../include/msgtype/ndtpos.pb.h \
     ../../include/msgtype/relocate.pb.h \
     ../../include/msgtype/pcdmap.pb.h \
-    ../../common/common/math/gnss_coordinate_convert.h
+    ../../common/common/math/gnss_coordinate_convert.h \
+    ../../include/msgtype/gps.pb.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/imu.pb.h
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )