change driver_lidar_bk16z, not complete.

src/driver/driver_lidar_bk_16z/driver_lidar_bk_16z.pro

@@ -9,7 +9,7 @@ CONFIG += c++17 cmdline
 SOURCES += \
         common/ioapi.cpp \
         common/ssFrameLib.cpp \
-        lidar_16zdriver.cpp \
+        lidar_bkdriver.cpp \
         main.cpp
 
 # Default rules for deployment.
@@ -22,7 +22,7 @@ HEADERS += \
     common/ioapi.h \
     common/ssFrameLib.h \
     common/utility.h \
-    lidar_16zdriver.h
+    lidar_bkdriver.h
 
 
 INCLUDEPATH += $$PWD/common

src/driver/driver_lidar_bk_16z/lidar_16zdriver.cpp → src/driver/driver_lidar_bk_16z/lidar_bkdriver.cpp

@@ -1,4 +1,4 @@
-#include "lidar_16zdriver.h"
+#include "lidar_bkdriver.h"
 
 static uint32_t s_CrcTab32[] = {
     0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
@@ -36,22 +36,28 @@ static uint32_t s_CrcTab32[] = {
 #define IS_CRC_START_32 0xFFFFFFFF
 #define PACKAGELEN 60000
 
-lidar_16zdriver::lidar_16zdriver() {
+lidar_bkdriver::lidar_bkdriver() {
     setparam();
     socketInit();
     configDeviceParams();
 
-    mpThreadRaw = new std::thread(&lidar_16zdriver::RecvRawThread,this);
+    mpThreadRaw = new std::thread(&lidar_bkdriver::RecvRawThread,this);
 }
 
-void lidar_16zdriver::socketInit()
+lidar_bkdriver::~lidar_bkdriver()
+{
+    mbRun = false;
+    mpThreadRaw->join();
+}
+
+void lidar_bkdriver::socketInit()
 {
     m_ctl_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.ctrlport, m_input_para.ctrlport);
     m_data_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.dataport, m_input_para.dataport);
     m_heart_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.heart_port, m_input_para.heart_port);
 }
 
-void lidar_16zdriver::setparam()
+void lidar_bkdriver::setparam()
 {
     m_input_para.device_name ="R-Fans-16";
     m_input_para.display_mode = "overlay";
@@ -116,7 +122,7 @@ void lidar_16zdriver::setparam()
     m_input_para.LevelAM = false;
 }
 
-void lidar_16zdriver::configDeviceParams()
+void lidar_bkdriver::configDeviceParams()
 {
 
     lidarAPi::DEB_PROGRM_S params;
@@ -153,7 +159,7 @@ void lidar_16zdriver::configDeviceParams()
 }
 
 
-int lidar_16zdriver::progSet(lidarAPi::DEB_PROGRM_S &program)
+int lidar_bkdriver::progSet(lidarAPi::DEB_PROGRM_S &program)
 {
     unsigned int tmpData = 0;
     if ((m_input_para.device_name == "R-Fans-32") || (m_input_para.device_name == "R-Fans-16"))
@@ -269,7 +275,7 @@ uint32_t isCrc32(uint8_t const *pIn, uint16_t len)
     return crc & 0xffffffffL;
 }
 
-int lidar_16zdriver::lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfreq, int mt_lsrpwr)
+int lidar_bkdriver::lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfreq, int mt_lsrpwr)
 {
     LIDAR_COMMAND_S cmd;
     memset(&cmd.commander, 0, sizeof(LIDAR_CMDRPT_U));
@@ -284,7 +290,7 @@ int lidar_16zdriver::lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfr
     return m_ctl_socket->write((unsigned char *)&cmd, sizeof(LIDAR_COMMAND_S));
 }
 
-int lidar_16zdriver::setState(lidarAPi::DEB_PROGRM_S &program)
+int lidar_bkdriver::setState(lidarAPi::DEB_PROGRM_S &program)
 {
     switch (program.cmdstat)
     {
@@ -303,7 +309,7 @@ int lidar_16zdriver::setState(lidarAPi::DEB_PROGRM_S &program)
 }
 
 
-int lidar_16zdriver::RecvRawThread()
+int lidar_bkdriver::RecvRawThread()
 {
     const int MAXPACLEN = 60000;
     while(mbRun)
@@ -313,7 +319,7 @@ int lidar_16zdriver::RecvRawThread()
         // cout << ret << " " << ros::Time::now() << endl;
         if (ret <= 0)
         {
-            std::cout<<" configure. "<<std::endl;
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" configure. "<<std::endl;
             usleep(500);
             configDeviceParams();
             continue;

src/driver/driver_lidar_bk_16z/lidar_16zdriver.h → src/driver/driver_lidar_bk_16z/lidar_bkdriver.h

@@ -1,5 +1,5 @@
-#ifndef LIDAR_16ZDRIVER_H
-#define LIDAR_16ZDRIVER_H
+#ifndef LIDAR_BKDRIVER_H
+#define LIDAR_BKDRIVER_H
 
 #include "ssFrameLib.h"
 #include "ioapi.h"
@@ -10,10 +10,11 @@
 
 #include "ICD_LiDAR_API.h"
 
-class lidar_16zdriver
+class lidar_bkdriver
 {
 public:
-    lidar_16zdriver();
+    lidar_bkdriver();
+    ~lidar_bkdriver();
 
 private:
     rfans_driver::IOAPI *m_ctl_socket;
@@ -36,6 +37,9 @@ private:
     std::thread * mpThreadRaw;
 
     InputPara_S m_input_para;
+
+    HEARTBEAT_S m_heart;
+    vector<HEARTBEAT_S> m_heartbeat_vec;
 };
 
 #endif // LIDAR_16ZDRIVER_H

src/driver/driver_lidar_bk_16z/main.cpp

@@ -1,13 +1,13 @@
 #include <QCoreApplication>
 
 
-#include "lidar_16zdriver.h"
+#include "lidar_bkdriver.h"
 
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
-    lidar_16zdriver * pdriver = new lidar_16zdriver();
+    lidar_bkdriver * pdriver = new lidar_bkdriver();
 
     return a.exec();
 }