change ndt. complete view_ndtmatching.

src/detection/detection_ndt_matching/detection_ndt_matching.pro

@@ -30,7 +30,8 @@ SOURCES += main.cpp \
     ../../include/msgtype/gpsimu.pb.cc \
     ../../include/msgtype/gps.pb.cc \
     ../../include/msgtype/ndtgpspos.pb.cc \
-    globalrelocation.cpp
+    globalrelocation.cpp \
+    ../../include/msgtype/pcdmap.pb.cc
 
 HEADERS += \
     ndt_matching.h \
@@ -40,7 +41,8 @@ HEADERS += \
     ../../include/msgtype/gpsimu.pb.h \
     ../../include/msgtype/gps.pb.h \
     ../../include/msgtype/ndtgpspos.pb.h \
-    globalrelocation.h
+    globalrelocation.h \
+    ../../include/msgtype/pcdmap.pb.h
 
 INCLUDEPATH += /opt/ros/melodic/include
 INCLUDEPATH += /opt/ros/kinetic/include

src/detection/detection_ndt_matching/main.cpp

@@ -15,6 +15,7 @@
 #include "ivlog.h"
 #include "ndt_matching.h"
 #include "gpsimu.pb.h"
+#include "pcdmap.pb.h"
 
 #include "ivexit.h"
 #include "ivbacktrace.h"
@@ -34,6 +35,9 @@ void * gpa,* gpb ,* gpc, * gpd;   //Modulecomm pointer
 
 void * gparelocate;
 
+
+void * gpapcdmap;
+
 iv::Ivfault *gfault = nullptr;   //fault diag
 iv::Ivlog *givlog = nullptr;   //log
 
@@ -436,6 +440,7 @@ void pausecomm()
     iv::modulecomm::PauseComm(gpc);
     iv::modulecomm::PauseComm(gpd);
     iv::modulecomm::PauseComm(gparelocate);
+    iv::modulecomm::PauseComm(gpapcdmap);
 }
 
 /**
@@ -448,6 +453,7 @@ void continuecomm()
     iv::modulecomm::ContintuComm(gpc);
     iv::modulecomm::ContintuComm(gpd);
     iv::modulecomm::ContintuComm(gparelocate);
+    iv::modulecomm::ContintuComm(gpapcdmap);
 }
 
 /**
@@ -570,6 +576,7 @@ void exitfunc()
     iv::modulecomm::Unregister(gpc);
     iv::modulecomm::Unregister(gpd);
     iv::modulecomm::Unregister(gparelocate);
+    iv::modulecomm::Unregister(gpapcdmap);
     if(gpFileLastPos != 0)gpFileLastPos->close();
     std::cout<<"Complete exitfunc."<<std::endl;
 }
@@ -598,6 +605,8 @@ int main(int argc, char *argv[])
 
    iv::xmlparam::Xmlparam xparam(strpath.toStdString());
 
+    gpapcdmap = iv::modulecomm::RegisterSend("pcdmap",1000,1);
+
    gstr_lidarmsgname = xparam.GetParam("lidarmsg","lidar_pc");
    gstr_gpsmsgname = xparam.GetParam("gpsmsg","hcp2_gpsimu");
    gstr_ndtposmsgname = xparam.GetParam("ndtposmsg","ndtpos");
@@ -662,6 +671,7 @@ int main(int argc, char *argv[])
     gpc = iv::modulecomm::RegisterSend(gstr_ndtposmsgname.data(),10000,1);
     gpd = iv::modulecomm::RegisterSend(gstr_ndtgpsposmsgname.data(),10000,1);
     gparelocate = iv::modulecomm::RegisterRecv("relocate",ListenRelocate);
+
     ndt_match_Init_nomap();
 
     iv::ivexit::RegIVExitCall(exitfunc);

src/detection/detection_ndt_matching/ndt_matching.cpp

@@ -60,6 +60,7 @@
 //#include <ndt_gpu/NormalDistributionsTransform.h>
 #include "ndtpos.pb.h"
 #include "ndtgpspos.pb.h"
+#include "pcdmap.pb.h"
 
 //#include <pcl_ros/point_cloud.h>
 //#include <pcl_ros/transforms.h>
@@ -106,6 +107,8 @@
 extern iv::Ivfault *gfault ;
 extern iv::Ivlog *givlog;
 
+extern void * gpapcdmap;
+
 static int gindex = 0;
 iv::lidar_pose glidar_pose;
 
@@ -480,6 +483,8 @@ static void UseMap(int index)
     xmap = boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
 
 
+
+
     QTime xTime;
     xTime.start();
     if(0 == pcl::io::loadPCDFile(gvector_trace.at(index).mstrpcdpath.data(),*xmap))
@@ -559,6 +564,8 @@ void LocalMapUpdate(int n)
     while(gbLMRun)
     {
 
+        bool bsharemap = false;
+
         getmindistrace(index,ftracedis);
 
         if(g_hasmap == false)
@@ -571,6 +578,10 @@ void LocalMapUpdate(int n)
                     UseMap(index);
                     ncurindex = index;
                     g_hasmap = true;
+
+                    bsharemap = true;
+
+
                 }
             }
         }
@@ -585,6 +596,7 @@ void LocalMapUpdate(int n)
                    UseMap(index);
                    ncurindex = index;
                    g_hasmap = true;
+                   bsharemap = true;
                }
            }
         }
@@ -598,6 +610,26 @@ void LocalMapUpdate(int n)
 
 
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
+        if(bsharemap)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            iv::ndt::pcdmap xpcdmappath;
+            xpcdmappath.set_strpath(gvector_trace.at(index).mstrpcdpath);
+            int nbytesize = xpcdmappath.ByteSize();
+            std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nbytesize]);
+            if(xpcdmappath.SerializeToArray(pstr_ptr.get(),nbytesize))
+            {
+                std::cout<<" share pcd map info ."<<std::endl;
+                iv::modulecomm::ModuleSendMsg(gpapcdmap,pstr_ptr.get(),nbytesize);
+                std::this_thread::sleep_for(std::chrono::milliseconds(100));
+                iv::modulecomm::ModuleSendMsg(gpapcdmap,pstr_ptr.get(),nbytesize);
+            }
+            else
+            {
+                std::cout<<" pcdmap SerializeToArray faile. "<<std::endl;
+            }
+        }
     }
 }
 

src/include/proto/pcdmap.proto

@@ -0,0 +1,12 @@
+syntax = "proto2";
+
+package iv.ndt;
+
+message pcdmap
+{
+	required string strpath = 1;
+};
+
+
+
+

src/tool/view_ndtmatching/mainwindow.cpp

@@ -86,7 +86,7 @@ void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
 
 void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
 {
-    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" psycho."<<std::endl;
+//    std::cout<<std::chrono::system_clock::now().time_since_epoch().count()/1000000<<" psycho."<<std::endl;
 
     viewer.removeShape("car");
 
@@ -161,6 +161,9 @@ MainWindow::MainWindow(QWidget *parent) :
     ModuleFun fun1 =std::bind(&MainWindow::UpdateNDTPos,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     mpandtpos = iv::modulecomm::RegisterRecvPlus("ndtpos",fun1);
 
+    ModuleFun fun2 =std::bind(&MainWindow::UpdatePCDMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpapcdmap = iv::modulecomm::RegisterRecvPlus("pcdmap",fun2);
+
     mparelocate = iv::modulecomm::RegisterSend("relocate",10000,1);
 
     connect(this,SIGNAL(ndtposupdate()),this,SLOT(onndtposupdate()));
@@ -175,6 +178,7 @@ MainWindow::~MainWindow()
     mpthreadpcd->join();
     iv::modulecomm::Unregister(mpandtpos);
     iv::modulecomm::Unregister(mparelocate);
+    iv::modulecomm::Unregister(mpapcdmap);
     delete ui;
 }
 
@@ -199,6 +203,8 @@ void MainWindow::threadpcdview()
      mpviewer->showCloud(point_cloud);
 
 
+
+
      //This will only get called once
      mpviewer->runOnVisualizationThreadOnce (viewerOneOff);
 
@@ -209,6 +215,23 @@ void MainWindow::threadpcdview()
 
      while((!mpviewer->wasStopped()) && mbRun)
      {
+
+         if(mbpcdmapupdate)
+         {
+             std::string strpcdmap;
+             mmutexpcdmap.lock();
+             strpcdmap = mstrpcdmappath;
+             mbpcdmapupdate = false;
+             mmutexpcdmap.unlock();
+
+             pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud2(
+                         new pcl::PointCloud<pcl::PointXYZI>());
+
+              pcl::io::loadPCDFile<pcl::PointXYZI>(strpcdmap,*point_cloud2);
+              mpviewer->showCloud(point_cloud2);
+              std::cout<<" use new pcd map. path: "<<strpcdmap.data()<<std::endl;
+         }
+
          std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
      }
@@ -351,3 +374,24 @@ void MainWindow::onndtposupdate()
         mpLabelStatus->setText("NDT Update Time:" + QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss:zzz"));
     }
 }
+
+void MainWindow::UpdatePCDMap(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+    iv::ndt::pcdmap xpcdmap;
+    std::cout<<" update ndt map "<<std::endl;
+    if(xpcdmap.ParseFromArray(strdata,nSize))
+    {
+        mmutexpcdmap.lock();
+        mstrpcdmappath = xpcdmap.strpath();
+        mbpcdmapupdate = true;
+        mmutexpcdmap.unlock();
+
+    }
+    else
+    {
+        std::cout<<"UpdatePCDMap Fail."<<std::endl;
+    }
+}

src/tool/view_ndtmatching/mainwindow.h

@@ -15,6 +15,7 @@
 #include "modulecomm.h"
 #include "ndtpos.pb.h"
 #include "relocate.pb.h"
+#include "pcdmap.pb.h"
 
 #include <QLabel>
 
@@ -62,6 +63,7 @@ private:
 
 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);
 
 private:
     Ui::MainWindow *ui;
@@ -74,6 +76,7 @@ private:
 
     void * mpandtpos;
     void * mparelocate;
+    void * mpapcdmap;
 
     pose mCurPose;
 
@@ -86,6 +89,10 @@ private:
 
     double mfTransProb = 0.0;
 
+    std::string mstrpcdmappath;
+    std::mutex mmutexpcdmap;
+    bool mbpcdmapupdate =false;
+
 };
 
 #endif // MAINWINDOW_H

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -19,7 +19,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 SOURCES += main.cpp \
     mainwindow.cpp \
     ../../include/msgtype/ndtpos.pb.cc \
-    ../../include/msgtype/relocate.pb.cc
+    ../../include/msgtype/relocate.pb.cc \
+    ../../include/msgtype/pcdmap.pb.cc
 
 QMAKE_LFLAGS += -no-pie
 
@@ -67,7 +68,8 @@ FORMS += \
 HEADERS += \
     mainwindow.h \
     ../../include/msgtype/ndtpos.pb.h \
-    ../../include/msgtype/relocate.pb.h
+    ../../include/msgtype/relocate.pb.h \
+    ../../include/msgtype/pcdmap.pb.h
 
 !include(../../../include/common.pri ) {
     error( "Couldn't find the common.pri file!" )