change detection_lidar_localizer_ekf for ndt.

src/common/ndt_gpu/ndt_gpu.pro

@@ -53,9 +53,9 @@ LIBS += -L"/usr/local/lib" \
         -lcudart \
         -lcufft
 
-CUDA_SDK = "/usr/local/cuda-10.0/"   # cudaSDK路径
+CUDA_SDK = "/usr/local/cuda-10.2/"   # cudaSDK路径
 
-CUDA_DIR = "/usr/local/cuda-10.0/"            # CUDA tookit路径
+CUDA_DIR = "/usr/local/cuda-10.2/"            # CUDA tookit路径
 
 SYSTEM_NAME = linux         # 自己系统环境 'Win32', 'x64', or 'Win64'
 

src/detection/detection_lidar_localizer_ekf/ekf_localizer.cpp

@@ -22,12 +22,14 @@
 #define DEBUG_INFO(...) {if (show_debug_info_){ printf(__VA_ARGS__); } }
 #define DEBUG_PRINT_MAT(X) { if (show_debug_info_) { std::cout << #X << ": " << X << std::endl; } }
 
+
+static int mnNoData = 100;
 // clang-format on
 EKFLocalizer::EKFLocalizer()
 {
     dim_x_ = 6;
     show_debug_info_ = false;
-    ekf_rate_ = 50.0;
+    ekf_rate_ = 100.0;
     ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1);
     tf_rate_ = 1.0;
     enable_yaw_bias_estimation_ = true;
@@ -117,6 +119,13 @@ EKFLocalizer::EKFLocalizer()
 
   initEKF();
 
+  mpandtekf = iv::modulecomm::RegisterSend("ndtekf",100000,1);
+
+  mpthread = new std::thread(&EKFLocalizer::timerthread,this);
+
+  ModuleFun fungps = std::bind(&EKFLocalizer::UpdateGPS,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+  mpa = iv::modulecomm::RegisterRecvPlus("ndtpos",fungps);
+
   /* debug */
 //  pub_debug_ = pnh_.advertise<std_msgs::Float64MultiArray>("debug", 1);
 //  pub_measured_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("debug/measured_pose", 1);
@@ -416,6 +425,7 @@ void EKFLocalizer::predictKinematicsModel()
   X_next(IDX::VX) = vx;
   X_next(IDX::WZ) = wz;
 
+
   X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW)));
 
   /* Set A matrix for latest state */
@@ -433,6 +443,7 @@ void EKFLocalizer::predictKinematicsModel()
 
   Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
 
+
   if (dvx < 10.0 && dyaw < 1.0)
   {
     // auto covariance calculate for x, y assuming vx & yaw estimation covariance is small
@@ -456,6 +467,8 @@ void EKFLocalizer::predictKinematicsModel()
     Q(IDX::Y, IDX::Y) = 0.05;
   }
 
+
+
   Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_;         // for yaw
   Q(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_;  // for yaw bias
   Q(IDX::VX, IDX::VX) = proc_cov_vx_d_;            // for vx
@@ -690,6 +703,7 @@ bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd
   DEBUG_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max);
   if (mahalanobis_squared(0) > dist_max * dist_max)
   {
+      mbFirst = true;
     return false;
   }
 
@@ -781,7 +795,7 @@ void EKFLocalizer::UpdateGPS(const char *strdata, const unsigned int nSize, cons
 
     if(nSize<1)return;
 
-    static bool bFirst;
+    static bool bFirst = true;
 
 
     iv::lidar::ndtpos xndtpos;
@@ -791,6 +805,10 @@ void EKFLocalizer::UpdateGPS(const char *strdata, const unsigned int nSize, cons
        return;
     }
 
+    mMutexndtpos.lock();
+    mndtpos.CopyFrom(xndtpos);
+    mMutexndtpos.unlock();
+
     mMutexCurPose.lock();
     mCurPose.x = xndtpos.pose_x();
     mCurPose.y = xndtpos.pose_y();
@@ -801,7 +819,9 @@ void EKFLocalizer::UpdateGPS(const char *strdata, const unsigned int nSize, cons
     mbCurPoseUpdate = true;
     mMutexCurPose.unlock();
 
-    if(bFirst == true)
+    std::cout<<" ndt x: "<<mCurPose.x<<" y:"<<mCurPose.y<<std::endl;
+
+    if(mbFirst == true)
     {
           Eigen::MatrixXd X(dim_x_, 1);
           Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
@@ -821,9 +841,11 @@ void EKFLocalizer::UpdateGPS(const char *strdata, const unsigned int nSize, cons
           P(IDX::WZ, IDX::WZ) = 0.01;
 
           ekf_.init(X, P, extend_state_step_);
-        bFirst = false;
+        mbFirst = false;
     }
 
+    mnNoData = 0;
+
 
 }
 
@@ -956,6 +978,7 @@ void EKFLocalizer::timerproc()
       auto start = std::chrono::system_clock::now();
       DEBUG_INFO("------------------------- start prediction -------------------------");
       predictKinematicsModel();
+
       double elapsed =
           std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
       DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6);
@@ -1008,4 +1031,59 @@ void EKFLocalizer::getcurrentpose()
       mekfpose.pitch = mCurPose.pitch;
 
 
+      std::cout<<"      ekf x: "<<mekfpose.x<<" y: "<<mekfpose.y<<std::endl;
+
+      iv::lidar::ndtpos xndtpos;
+      mMutexndtpos.lock();
+      xndtpos.CopyFrom(mndtpos);
+      mMutexndtpos.unlock();
+
+
+      xndtpos.set_pose_x(mekfpose.x);
+      xndtpos.set_pose_y(mekfpose.y);
+      xndtpos.set_pose_yaw(yaw);
+
+      int ndatasize = xndtpos.ByteSize();
+      char * strdata = new char[ndatasize];
+      if(xndtpos.SerializeToArray(strdata,ndatasize))
+      {
+          iv::modulecomm::ModuleSendMsg(mpandtekf,strdata,ndatasize);
+      }
+      delete strdata;
+
+
+
+}
+
+
+#include <thread>
+#include <QTime>
+
+void EKFLocalizer::timerthread()
+{
+
+    const int ninterval = 10;
+    qint64 nlastproc = 0;
+    while(1)
+    {
+        if((QDateTime::currentMSecsSinceEpoch() - nlastproc)<ninterval)
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+            continue;
+        }
+        nlastproc = QDateTime::currentMSecsSinceEpoch();
+        if(mnNoData>=100)
+        {
+//            qDebug("continue. %d",mnNoData);
+            continue;
+        }
+
+
+        timerproc();
+
+        mnNoData++;
+
+
+
+    }
 }

src/detection/detection_lidar_localizer_ekf/ekf_localizer.h

@@ -43,6 +43,8 @@
 
 #include "modulecomm.h"
 
+#include <thread>
+
 namespace iv {
 
 
@@ -244,6 +246,9 @@ private:
 
 
 private:
+
+  void timerthread();
+
   void UpdateGPS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
   iv::pose mCurPose;
@@ -252,9 +257,21 @@ private:
 
   iv::pose mekfpose;
 
+  bool mbFirst = true;
+
   void measurementUpdatePose();
 
   void timerproc();
 
   void getcurrentpose();
+
+  std::thread * mpthread;
+
+  void * mpa;
+  void * mpandtekf;
+
+  iv::lidar::ndtpos mndtpos;
+  QMutex mMutexndtpos;
+
+
 };

src/detection/detection_lidar_localizer_ekf/main.cpp

@@ -3,10 +3,15 @@
 #include "modulecomm.h"
 #include "ivbacktrace.h"
 
+#include "ekf_localizer.h"
+
+EKFLocalizer * gpekf;
+
 int main(int argc, char *argv[])
 {
     RegisterIVBackTrace();
     QCoreApplication a(argc, argv);
 
+    gpekf = new EKFLocalizer();
     return a.exec();
 }

src/detection/detection_ndt_matching_gpu_multi/detection_ndt_matching_gpu_multi.pro

@@ -47,6 +47,7 @@ INCLUDEPATH += /usr/include/eigen3
 INCLUDEPATH += $$PWD/../../include/msgtype/
 
 INCLUDEPATH += /usr/local/cuda-10.0/targets/aarch64-linux/include
+INCLUDEPATH += /usr/local/cuda-10.2/targets/aarch64-linux/include
 INCLUDEPATH += /usr/local/cuda-10.2/targets/x86_64-linux/include
 
 DEFINES += CUDA_FOUND