change view_ndtmatching. near complete.

src/tool/view_ndtmatching/mainwindow.cpp

@@ -132,8 +132,13 @@ MainWindow::MainWindow(QWidget *parent) :
 {
     ui->setupUi(this);
 
+    mpLabelStatus = new QLabel("No NDT Pos",ui->statusbar);
+    mpLabelStatus->setMinimumWidth(350);
+
     ui->pushButton_EnableRelocation->setEnabled(false);
 
+    ui->pushButton_Test->setVisible(false);
+
     ui->horizontalSlider->setRange(5,300);
     ui->horizontalSlider->setValue(100);
 
@@ -151,6 +156,12 @@ MainWindow::MainWindow(QWidget *parent) :
 \
     mpthreadpcd = new std::thread(&MainWindow::threadpcdview,this);
 
+
+    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);
+
+    connect(this,SIGNAL(ndtposupdate()),this,SLOT(onndtposupdate()));
+
     setWindowTitle("NDT Matching View & Relocation Enable");
 
 }
@@ -159,6 +170,7 @@ MainWindow::~MainWindow()
 {
     mbRun = false;
     mpthreadpcd->join();
+    iv::modulecomm::Unregister(mpandtpos);
     delete ui;
 }
 
@@ -243,3 +255,53 @@ void MainWindow::on_comboBox_currentIndexChanged(int index)
 {
     gnViewMode = index;
 }
+
+void MainWindow::UpdateNDTPos(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    (void)index;
+    (void)dt;
+    (void)strmemname;
+
+    iv::lidar::ndtpos xndtpos;
+    if(xndtpos.ParseFromArray(strdata,nSize))
+    {
+        mmutexndtpos.lock();
+        mndtpos.CopyFrom(xndtpos);
+        mbndtposupdate = true;
+        mmutexndtpos.unlock();
+        emit ndtposupdate();
+    }
+    else
+    {
+        std::cout<<"MainWindow::UpdateNDTPos Parse NDTPos Fail."<<std::endl;
+    }
+
+}
+
+void MainWindow::onndtposupdate()
+{
+    if(mbndtposupdate)
+    {
+        iv::lidar::ndtpos xndtpos;
+        mmutexndtpos.lock();
+        xndtpos.CopyFrom(mndtpos);
+        mbndtposupdate = false;
+        mmutexndtpos.unlock();
+        gCurPose.x = xndtpos.pose_x();
+        gCurPose.y = xndtpos.pose_y();
+        gCurPose.z = xndtpos.pose_z();
+        gCurPose.yaw = xndtpos.pose_yaw();
+        gCurPose.pitch = xndtpos.pose_pitch();
+        gCurPose.roll = xndtpos.pose_roll();
+        ui->lineEdit_prob->setText(QString::number(xndtpos.trans_probability(),'f',3));
+        ui->lineEdit_score->setText(QString::number(xndtpos.fitness_score(),'f',3));
+        ui->lineEdit_x->setText(QString::number(gCurPose.x,'f',3));
+        ui->lineEdit_y->setText(QString::number(gCurPose.y,'f',3));
+        ui->lineEdit_z->setText(QString::number(gCurPose.z,'f',3));
+        ui->lineEdit_yaw->setText(QString::number(gCurPose.yaw,'f',3));
+        ui->lineEdit_pitch->setText(QString::number(gCurPose.pitch,'f',3));
+        ui->lineEdit_roll->setText(QString::number(gCurPose.roll,'f',3));
+
+        mpLabelStatus->setText("NDT Update Time:" + QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss:zzz"));
+    }
+}

src/tool/view_ndtmatching/mainwindow.h

@@ -10,6 +10,12 @@
 #include <QCoreApplication>
 
 #include <thread>
+#include <mutex>
+
+#include "modulecomm.h"
+#include "ndtpos.pb.h"
+
+#include <QLabel>
 
 struct pose
 {
@@ -35,7 +41,8 @@ public:
     explicit MainWindow(QWidget *parent = 0);
     ~MainWindow();
 
-
+signals:
+    void ndtposupdate();
 private slots:
     void on_pushButton_Test_clicked();
 
@@ -47,9 +54,14 @@ private slots:
 
     void on_comboBox_currentIndexChanged(int index);
 
+    void onndtposupdate();
+
 private:
     void threadpcdview();
 
+private:
+    void UpdateNDTPos(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+
 private:
     Ui::MainWindow *ui;
 
@@ -59,8 +71,16 @@ private:
 
     bool mbRun = true;
 
+    void * mpandtpos;
+
     pose mCurPose;
 
+    iv::lidar::ndtpos mndtpos;
+    std::mutex mmutexndtpos;
+    bool mbndtposupdate = false;
+
+    QLabel * mpLabelStatus;
+
 
 };
 

src/tool/view_ndtmatching/mainwindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>673</width>
-    <height>478</height>
+    <width>686</width>
+    <height>499</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -17,9 +17,9 @@
    <widget class="QPushButton" name="pushButton_Test">
     <property name="geometry">
      <rect>
-      <x>250</x>
-      <y>380</y>
-      <width>201</width>
+      <x>610</x>
+      <y>300</y>
+      <width>71</width>
       <height>51</height>
      </rect>
     </property>
@@ -169,7 +169,7 @@
     <property name="geometry">
      <rect>
       <x>184</x>
-      <y>300</y>
+      <y>369</y>
       <width>181</width>
       <height>51</height>
      </rect>
@@ -179,7 +179,7 @@
     <property name="geometry">
      <rect>
       <x>400</x>
-      <y>301</y>
+      <y>370</y>
       <width>201</width>
       <height>51</height>
      </rect>
@@ -192,7 +192,7 @@
     <property name="geometry">
      <rect>
       <x>63</x>
-      <y>240</y>
+      <y>309</y>
       <width>521</width>
       <height>31</height>
      </rect>
@@ -205,7 +205,7 @@
     <property name="geometry">
      <rect>
       <x>60</x>
-      <y>300</y>
+      <y>369</y>
       <width>111</width>
       <height>51</height>
      </rect>
@@ -214,13 +214,65 @@
      <string>Auto</string>
     </property>
    </widget>
+   <widget class="QLineEdit" name="lineEdit_score">
+    <property name="geometry">
+     <rect>
+      <x>465</x>
+      <y>220</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="readOnly">
+     <bool>true</bool>
+    </property>
+   </widget>
+   <widget class="QLineEdit" name="lineEdit_prob">
+    <property name="geometry">
+     <rect>
+      <x>130</x>
+      <y>220</y>
+      <width>121</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="readOnly">
+     <bool>true</bool>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_7">
+    <property name="geometry">
+     <rect>
+      <x>395</x>
+      <y>220</y>
+      <width>61</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>score</string>
+    </property>
+   </widget>
+   <widget class="QLabel" name="label_8">
+    <property name="geometry">
+     <rect>
+      <x>60</x>
+      <y>220</y>
+      <width>61</width>
+      <height>31</height>
+     </rect>
+    </property>
+    <property name="text">
+     <string>prob</string>
+    </property>
+   </widget>
   </widget>
   <widget class="QMenuBar" name="menubar">
    <property name="geometry">
     <rect>
      <x>0</x>
      <y>0</y>
-     <width>673</width>
+     <width>686</width>
      <height>28</height>
     </rect>
    </property>

src/tool/view_ndtmatching/view_ndtmatching.pro

@@ -17,7 +17,8 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += main.cpp \
-    mainwindow.cpp
+    mainwindow.cpp \
+    ../../include/msgtype/ndtpos.pb.cc
 
 QMAKE_LFLAGS += -no-pie
 
@@ -63,5 +64,15 @@ FORMS += \
     mainwindow.ui
 
 HEADERS += \
-    mainwindow.h
+    mainwindow.h \
+    ../../include/msgtype/ndtpos.pb.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivprotobuf.pri ) {
+    error( "Couldn't find the ivprotobuf.pri file!" )
+}
+