include/ivpcl.pri

@@ -8,3 +8,4 @@ unix:INCLUDEPATH += /usr/include/pcl-1.7
 unix:INCLUDEPATH += /usr/include/pcl-1.8
 unix:INCLUDEPATH += /usr/include/pcl-1.12
 unix:INCLUDEPATH += /usr/include/pcl-1.10
+unix:INCLUDEPATH += /usr/include/pcl-1.14

src/driver/driver_lidar_bk_16z/common/utility.h

@@ -110,7 +110,7 @@ struct InputPara_S
   scnSpeed = 0;
   dual_echo = false;
   cfg_path = "";
-  read_once;
+  read_once = true;
   device_start = 0;
   read_fast = false;
   save_xyz = false;

src/driver/driver_lidar_bk_16z/driver_lidar_bk_16z.pro

@@ -9,6 +9,7 @@ CONFIG += c++17 cmdline
 SOURCES += \
         common/ioapi.cpp \
         common/ssFrameLib.cpp \
+        lidar_16z.cpp \
         lidar_bkdriver.cpp \
         main.cpp
 
@@ -22,9 +23,22 @@ HEADERS += \
     common/ioapi.h \
     common/ssFrameLib.h \
     common/utility.h \
+    lidar_16z.h \
     lidar_bkdriver.h
 
 
 INCLUDEPATH += $$PWD/common
 
 INCLUDEPATH += /usr/include/eigen3
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}

src/driver/driver_lidar_bk_16z/lidar_16z.cpp

@@ -0,0 +1,128 @@
+#include "lidar_16z.h"
+
+
+static float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,1,3,5,7,9,11,13,15};
+static float H_Beta[16] = {-2.5,-2.5 ,-2.5 ,-2.5,-2.5,-2.5 ,-2.5 ,-2.5,
+                    2.5,-2.5 ,-2.5 ,-2.5, -2.5,-2.5 ,-2.5 ,-2.5};
+
+lidar_16z::lidar_16z() {
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+        new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+    point_cloud->width = 0;
+    point_cloud->header.seq =0;
+    mpoint_cloud_temp = point_cloud;
+
+    mpdriver = new lidar_bkdriver();
+    mpTheadDecode = new  std::thread(&lidar_16z::ThreadDecode,this);
+}
+
+lidar_16z::~lidar_16z(){
+    mbRun = false;
+    mpTheadDecode->join();
+    delete mpdriver;
+}
+
+void lidar_16z::ThreadDecode()
+{
+    static double azutotal = 0.0;
+    static int g_seq = 0;
+    while(mbRun)
+    {
+        std::shared_ptr<unsigned char> pdata_ptr;
+        int64_t nrecvtime;
+        int ndatasize = 0;
+        int nread = mpdriver->GetRecvData(pdata_ptr,nrecvtime,ndatasize);
+        if(nread == 1)
+        {
+            std::cout<<"read "<<ndatasize<<std::endl;
+            if(ndatasize % 1184 != 0)
+            {
+                std::cout<<" data not complete. datasize: "<<ndatasize<<std::endl;
+            }
+            else
+            {
+                unsigned char * p = pdata_ptr.get();
+                unsigned short * pang_start = (unsigned short * )(p+12);
+                unsigned short * pang_end = (unsigned short *)(p+14);
+                (void)pang_end;
+                double ang_space = 0.2;
+                int i;
+                int nheadlen = 32;
+                double fAzuAng = static_cast<double>(*pang_start) * 0.01;
+                for(i=0;i<12;i++)
+                {
+                    int j;
+                    for(j=0;j<16;j++)
+                    {
+                        unsigned char * pecho = (unsigned char *)(p +nheadlen +  i*96 + j*6);
+                        unsigned short * prange;
+                        unsigned char * pchch;
+                        double frange;
+                        unsigned char  nch;
+                        unsigned char intensity;
+                        pchch = pecho;
+                        prange = (unsigned short *)(pecho +3);
+                        intensity = *(pecho + 5);
+                        nch = * pchch;
+                        frange = static_cast<double>(*prange);
+                        frange = frange * 0.004;
+                        if(nch<=16){
+                            double fAng = (-fAzuAng - H_Beta[nch] - mfRollAng) * M_PI/180.0;
+                            double vtheta = V_theta[nch] * M_PI/180.0;
+                            double x = frange * cos(fAng)* cos(vtheta);
+                            double y = frange * sin(fAng)* cos(vtheta);
+                            double z = frange * sin(vtheta);
+
+                            pcl::PointXYZI point;
+                            point.x = x;
+                            point.y = y;
+                            point.z = z;
+                            point.intensity = intensity;
+
+                            mpoint_cloud_temp->points.push_back(point);
+                            ++mpoint_cloud_temp->width;
+                        }
+                        else
+                        {
+                            std::cout<<" channel error."<<std::endl;
+                        }
+                    }
+                    fAzuAng = fAzuAng + ang_space;
+
+                    azutotal = azutotal + ang_space;
+                    if(azutotal>=360.0)
+                    {
+                        //share pointcloud
+                        mmutexpc.lock();
+                        mpoint_cloud = mpoint_cloud_temp;
+                        mbpcupdate = true;
+                        mmutexpc.unlock();
+                        mcv.notify_all();
+                        pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                            new pcl::PointCloud<pcl::PointXYZI>());
+
+                        point_cloud->header.frame_id = "velodyne";
+                        point_cloud->height = 1;
+                        point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+                        point_cloud->width = 0;
+                        point_cloud->header.seq =g_seq;
+                        g_seq++;
+                        mpoint_cloud_temp = point_cloud;
+                        azutotal = 0;
+                    }
+                }
+            }
+        }
+    }
+}
+
+int lidar_16z::GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    if(mbpcupdate == false)return 0;
+    return 1;
+}

src/driver/driver_lidar_bk_16z/lidar_16z.h

@@ -0,0 +1,42 @@
+#ifndef LIDAR_16Z_H
+#define LIDAR_16Z_H
+
+#include <mutex>
+#include <thread>
+#include <condition_variable>
+
+#include "lidar_bkdriver.h"
+
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+class lidar_16z
+{
+public:
+    lidar_16z();
+    ~lidar_16z();
+
+    int GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
+
+private:
+    lidar_bkdriver * mpdriver;
+
+    void ThreadDecode();
+
+private:
+    bool mbRun = true;
+    std::thread * mpTheadDecode;
+
+    double mfRollAng = 0.0;
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_temp;
+
+    std::mutex mmutexpc;
+    std::mutex mmutexcv;
+    std::condition_variable mcv;
+    bool mbpcupdate = false;
+};
+
+#endif // LIDAR_16Z_H

src/driver/driver_lidar_bk_16z/lidar_bkdriver.cpp

@@ -42,11 +42,13 @@ lidar_bkdriver::lidar_bkdriver() {
     configDeviceParams();
 
     mpThreadRaw = new std::thread(&lidar_bkdriver::RecvRawThread,this);
+    mpThreadHeartbeat = new std::thread(&lidar_bkdriver::HeartBeatThread,this);
 }
 
 lidar_bkdriver::~lidar_bkdriver()
 {
     mbRun = false;
+    mpThreadHeartbeat->join();
     mpThreadRaw->join();
 }
 
@@ -169,6 +171,7 @@ int lidar_bkdriver::progSet(lidarAPi::DEB_PROGRM_S &program)
         case ANGLE_SPEED_0HZ:
             tmpData |= CMD_SCAN_DISABLE;
             tmpData |= CMD_SCAN_SPEED_10HZ;
+            break;
         case ANGLE_SPEED_10HZ:
             tmpData |= CMD_SCAN_ENABLE;
             tmpData |= CMD_SCAN_SPEED_10HZ;
@@ -278,7 +281,7 @@ uint32_t isCrc32(uint8_t const *pIn, uint16_t len)
 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));
+    memset((void *)&cmd.commander, 0, sizeof(LIDAR_CMDRPT_U));
     cmd.commander.cmdId = eMsgProgam;
     cmd.commander.isAsk = 0;
     cmd.commander.prgCtrl.cmdID = me_cmd;
@@ -325,9 +328,92 @@ int lidar_bkdriver::RecvRawThread()
             continue;
         }
 
+        mmutexrawdata.lock();
+        if(mvecterraw.size()>100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" raw vector full. skip one."<<std::endl;
+        }
+        else
+        {
+            iv::lidarraw xraw;
+            xraw.mnrecvtime = std::chrono::system_clock::now().time_since_epoch().count();
+            xraw.mpraw_ptr = pread;
+            xraw.ndatasize = ret;
+            mvecterraw.push_back(xraw);
+        }
+        mmutexrawdata.unlock();
+        mcv.notify_all();
+
         std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" recv "<<ret<<std::endl;
         std::cout<<" mod: "<<ret%1248<<std::endl;
 
     }
+
+    return 1;
+}
+
+int lidar_bkdriver::HeartBeatThread()
+{
+    float temper;
+    while(mbRun)
+    {
+        if (m_heart_socket->read((unsigned char *)&m_heart, sizeof(m_heart)) == 256)
+        {
+            temper = m_heart.temperature / 100.0;
+            (void)temper;
+            // if (m_input_para.save_isf && !m_input_para.isf_path.empty())
+            // {
+            //     m_heartbeat_vec.push_back(m_heart);
+            // }
+        }
+
+        static int num = 0;
+        if (0 == num++)
+        {
+            std::cout<<"temperature = "<<temper<<"motoer speed: "<<m_heart.motorspd<<std::endl;
+            std::cout<<"DEVICE_id = "<<m_heart.device_id<<std::endl;
+            // ROS_WARN("temperature=%f", temper);
+            // ROS_INFO("DEVICE_id=%d", m_heart.device_id);
+        }
+        num %= 10;
+
+        std::this_thread::sleep_for(chrono::seconds(1));
+    }
+
+    return 1;
+
+}
+
+int lidar_bkdriver::GetRecvData(std::shared_ptr<unsigned char> &pdata_ptr, int64_t &nRecvTime, int & ndatasize)
+{
+    int nread = 0;
+    nread = ExecGetRecvData(pdata_ptr,nRecvTime,ndatasize);
+    if(nread > 0)return nread;
+
+    std::unique_lock<std::mutex> lk(mmutexcv);
+    if(mcv.wait_for(lk,std::chrono::milliseconds(100)) == std::cv_status::timeout){
+        lk.unlock();
+    }
+    else {
+        lk.unlock();
+    }
+
+    return ExecGetRecvData(pdata_ptr,nRecvTime,ndatasize);
+}
+
+int lidar_bkdriver::ExecGetRecvData(std::shared_ptr<unsigned char> & pdata_ptr, int64_t & nRecvTime, int & ndatasize)
+{
+    int nread = 0;
+    mmutexrawdata.lock();
+    if(mvecterraw.size() > 0)
+    {
+        pdata_ptr = mvecterraw[0].mpraw_ptr;
+        nRecvTime = mvecterraw[0].mnrecvtime;
+        ndatasize = mvecterraw[0].ndatasize;
+        mvecterraw.erase(mvecterraw.begin());
+        nread = 1;
+    }
+    mmutexrawdata.unlock();
+    return nread;
 }
 

src/driver/driver_lidar_bk_16z/lidar_bkdriver.h

@@ -7,15 +7,31 @@
 
 #include <thread>
 #include <memory>
+#include <condition_variable>
+#include <mutex>
+#include <vector>
 
 #include "ICD_LiDAR_API.h"
 
+namespace  iv {
+
+struct lidarraw
+{
+    std::shared_ptr<unsigned char> mpraw_ptr;
+    int ndatasize;
+    int64_t  mnrecvtime;
+};
+}
+
 class lidar_bkdriver
 {
 public:
     lidar_bkdriver();
     ~lidar_bkdriver();
 
+public:
+    int GetRecvData(std::shared_ptr<unsigned char> & pdata_ptr, int64_t & nRecvTime,int & ndatasize);
+
 private:
     rfans_driver::IOAPI *m_ctl_socket;
     rfans_driver::IOAPI *m_data_socket;
@@ -31,10 +47,21 @@ private:
     int setState(lidarAPi::DEB_PROGRM_S &program);
 
     int RecvRawThread();
+    int HeartBeatThread();
+
+    int ExecGetRecvData(std::shared_ptr<unsigned char> & pdata_ptr, int64_t & nRecvTime, int & ndatasize);
 
 private:
     bool mbRun = true;
     std::thread * mpThreadRaw;
+    std::thread * mpThreadHeartbeat;
+
+    std::mutex mmutexrawdata;
+    std::vector<iv::lidarraw> mvecterraw;
+
+    std::mutex mmutexcv;
+    std::condition_variable mcv;
+
 
     InputPara_S m_input_para;