M1激光雷达驱动程序-共享内存

src/driver/driver_rs_m1/ChannelNum.csv

@@ -0,0 +1,32 @@
+0 # distance correction offset of channel 0
+0 # distance correction offset of channel 1
+0 # distance correction offset of channel 2
+0 # distance correction offset of channel 3
+0 # distance correction offset of channel 4
+0 # distance correction offset of channel 5
+1 # pitch rate
+1 # yaw rate
+0 # pitch offset of channel 0
+0 # pitch offset of channel 1
+0 # pitch offset of channel 2
+0 # pitch offset of channel 3
+0 # pitch offset of channel 4
+0 # pitch offset of channel 5
+0 # yaw offset of channel 0
+0 # yaw offset of channel 1
+0 # yaw offset of channel 2
+0 # yaw offset of channel 3
+0 # yaw offset of channel 4
+0 # yaw offset of channel 5
+-6.75 # yaw start angle of channel 0, -6.75~6.75
+6.75 # yaw end angle of channel 0, -6.75~6.75
+-6.75 # yaw start angle of channel 1, -6.75~6.75
+6.75 # yaw end angle of channel 1, -6.75~6.75
+-6.75 # yaw start angle of channel 2, -6.75~6.75
+6.75 # yaw end angle of channel 2, -6.75~6.75
+-6.75 # yaw start angle of channel 3, -6.75~6.75
+6.75 # yaw end angle of channel 3, -6.75~6.75
+-6.75 # yaw start angle of channel 4, -6.75~6.75
+6.75 # yaw end angle of channel 4, -6.75~6.75
+-6.75 # yaw start angle of channel 5, -6.75~6.75
+6.75 # yaw end angle of channel 5, -6.75~6.75

src/driver/driver_rs_m1/driver_rs_m1.pro

@@ -0,0 +1,42 @@
+QT -= gui
+
+QT += network
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+SOURCES += \
+        main.cpp \
+        rsdriver.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    rawdata.h \
+    rsdriver.h
+
+
+INCLUDEPATH += /usr/include/boost/
+LIBS += -lboost_system -lpthread -lm
+
+LIBS += -L/usr/lib/x86_64-linux-gnu/  -lpcap
+
+INCLUDEPATH += /usr/include/pcl-1.7
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += /usr/include/eigen3
+
+INCLUDEPATH += $$PWD/../../../include/
+LIBS += -L$$PWD/../../../bin/ -lxmlparam -lmodulecomm -livlog -livfault -livexit

src/driver/driver_rs_m1/limit.csv

@@ -0,0 +1,2 @@
+0
+20000

src/driver/driver_rs_m1/main.cpp

@@ -0,0 +1,41 @@
+#include <QCoreApplication>
+#include "rsdriver.h"
+#include <yaml-cpp/yaml.h>
+
+
+using namespace lidar;
+using namespace std;
+char gstr_memname[256];
+
+
+//void loadparam(const char * stryamlpath)
+//{
+//    YAML::Node config;
+//    try
+//    {
+//        config = YAML::LoadFile(stryamlpath);
+//    }
+//    catch(YAML::BadFile e)
+//    {
+//        qDebug("load error.");
+//        return;
+//    }
+
+
+
+
+//}
+
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+    snprintf(gstr_hostip,255,"0.0.0.0");
+    snprintf(gstr_port,255,"6699");//默认端口号
+    snprintf(gstr_memname,255,"lidar_pc");
+    StartLidar_rs_m1();
+
+    return a.exec();
+
+}

src/driver/driver_rs_m1/param.yaml

@@ -0,0 +1,40 @@
+range_min: 0
+range_max: 20000
+distance_correction:
+   -channel_0: 0
+   -channel_1: 0
+   -channel_2: 0
+   -channel_3: 0
+   -channel_4: 0
+   -channel_5: 0
+pitch_rate: 1
+yaw_rate: 1
+pitch_offset:
+   -channel_0: 0
+   -channel_1: 0
+   -channel_2: 0
+   -channel_3: 0
+   -channel_4: 0
+   -channel_5: 0
+yaw_offset:
+   -channel_0: 0
+   -channel_1: 0
+   -channel_2: 0
+   -channel_3: 0
+   -channel_4: 0
+   -channel_5: 0
+yaw_start:
+   -channel_0: -6.75
+   -channel_1: -6.75
+   -channel_2: -6.75
+   -channel_3: -6.75
+   -channel_4: -6.75
+   -channel_5: -6.75
+yaw_end:
+   -channel_0: 6.75
+   -channel_1: 6.75
+   -channel_2: 6.75
+   -channel_3: 6.75
+   -channel_4: 6.75
+   -channel_5: 6.75
+	

src/driver/driver_rs_m1/rawdata.h

@@ -0,0 +1,9 @@
+#pragma once
+#define BK32_DATA_BUFSIZE 20000000
+
+class lidar_rsM1_raw
+{
+public:
+    unsigned int mnLen;
+    char mstrdata[BK32_DATA_BUFSIZE];
+};

src/driver/driver_rs_m1/rsdriver.cpp

@@ -0,0 +1,332 @@
+#include "rsdriver.h"
+
+#define RS_Grabber_toRadians(x) ((x)*M_PI / 180.0)
+extern char gstr_memname[256];
+
+namespace lidar {
+char gstr_hostip[256];
+char gstr_port[256];
+bool g_rs_m1_run =false;
+bool g_rs_m1_running = false;
+bool g_brs_m1_Proc_running = false;
+int index;
+uint32_t point_index;
+
+rsM1_Buf *rs_m1_buf;
+char * g_RawData_Buf;
+int gnRawPos = 0;
+int g_seq = 0;
+int g_pcl =0;
+float pitch_rate;
+int pitch_max;
+int skip_block;
+
+float yaw_rate;
+float distance_max_thd;
+float distance_min_thd;
+
+std::vector<float> tan_lookup_table;
+
+std::thread * g_prsm1Thread;
+std::thread * g_prsm1ProcThread;
+
+ QTime gTime;
+ void *g_lidar_pc;
+
+ static const int MEMS_BLOCKS_PER_PACKET =25;
+ static const int MEMS_SCANS_PER_FIRING = 6;
+ static const int POINT_COUNT_PER_VIEWFIELD = 15750;
+ static const float ANGLE_RESOLUTION = 0.01f;
+ static const float DISTANCE_RESOLUTION = 0.01f;
+ int last_bag_index = -1;
+ int last_pitch_index = 0;
+
+ static float channel_cor[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};  // distance correction offset
+ static float channel_pitch_offset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};   // pitch offset
+ static float channel_yaw_offset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};     //yaw offset
+
+ Eigen::Matrix<float, 3, 5> input_ref_;
+ Eigen::Matrix<float, 3, 3> rotate_gal_;
+
+ void loadParam(){
+     input_ref_<< 0.748956, 0.414693, -0.414693, -0.748956, 0,
+             0.33131,  0.454981,  0.454981,  0.33131,  0.5,
+            -0.573846,-0.78805,  -0.78805,  -0.573846,-0.866025;
+
+     rotate_gal_<< 1, 0, 0, 0, 0.965926, -0.258819, 0, 0.258819, 0.965926;
+     pitch_rate = 1.0f;
+     yaw_rate = 1.0f;
+     distance_max_thd = 200.0;
+     distance_min_thd = 0.0;
+     pitch_max = 25000;
+     skip_block = 0;
+     point_index = 0;
+     tan_lookup_table.resize(2000);
+     for(int i = -1000; i<1000; i++)
+     {
+         double rad =RS_Grabber_toRadians(i/100.0f);
+         tan_lookup_table[i+1000] = std::tan(rad);
+     }
+ }
+
+ float pixlToDistance(int distance, int dsr){
+     float result;
+     int cor = channel_cor[dsr];
+     if(distance <= cor)
+     {
+         result = 0.0;
+     }else{
+         result = distance -cor;
+     }
+     return result;
+ }
+
+ float yawToDeg(int deg){
+     float result_f;
+     float deg_f =deg;
+     result_f = (deg_f * (1350.0f / 65534.0f) - 675.0f);
+     return result_f;
+ }
+
+ float pitchToDeg(int deg){
+     float result_f;
+     float deg_f = deg;
+     result_f = (deg_f * (1250.0f / pitch_max ) - 625.0f);
+     return result_f;
+ }
+
+ void processrs_M1_Data(QByteArray ba)
+ {
+     unsigned char * pdata;
+     float fa,fb;
+     pdata = reinterpret_cast<unsigned char*>(ba.data());
+     ++index;
+     if(ba.length() != 1400 && pdata[0] != 0x55 && pdata[0] != 0xa5)
+     {
+         std::cout<<"No right packets!"<<std::endl;
+     }else {
+         if(!((int)(pdata[4]*256 +pdata[5]) < (POINT_COUNT_PER_VIEWFIELD/MEMS_BLOCKS_PER_PACKET)))
+         {
+             memcpy(g_RawData_Buf+gnRawPos,pdata,1400);
+             gnRawPos= gnRawPos+1400;
+             rs_m1_buf->WriteData(g_RawData_Buf,gnRawPos);
+             gnRawPos = 0;
+             index = 0;
+         }else {
+             if((gnRawPos+1400)<= BK32_DATA_BUFSIZE)
+             {
+                 memcpy(g_RawData_Buf+gnRawPos,pdata,1400);
+                 gnRawPos= gnRawPos+1400;
+             }
+             else
+             {
+                 std::cout<<"lidar_rsM1 processrs_M1_Data data is very big gnRawPos = "<<gnRawPos<<std::endl;
+             }
+        }
+     }
+ }
+
+void getPacket(int n){
+    gTime.start();
+    QUdpSocket * udpSocket = new QUdpSocket( );
+    bool bbind = udpSocket->bind(QHostAddress(gstr_hostip), atoi(gstr_port));
+    int i = 0;
+   while(g_rs_m1_run)
+   {
+       if(udpSocket->hasPendingDatagrams())
+       {
+           QNetworkDatagram datagram = udpSocket->receiveDatagram();
+           processrs_M1_Data(datagram.data());
+           datagram.clear();
+       }
+       else
+       {
+           std::this_thread::sleep_for(std::chrono::milliseconds(1));
+       }
+   }
+   udpSocket->close();
+   delete udpSocket;
+   g_rs_m1_running = false;
+}
+
+
+void process_rs_M1_obs(char * strdata,int nLen)
+{
+    loadParam();
+    float pitch;
+    float yaw;
+    int bag = 0;
+    int block = 0;
+    int channel = 0;
+    int buf1len = nLen/1400;
+//    std::cout<<"   len    :"<<nLen<<"ooooooooooooooo"<<buf1len<<std::endl;
+    QDateTime dt = QDateTime::currentDateTime();
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+    pcl::PointXYZI point;
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 5;
+    point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
+    point_cloud->width = 15750;
+//    point_cloud->is_dense = false;
+    point_cloud->resize(point_cloud->height*point_cloud->width);
+    unsigned char * pstr = (unsigned char *)strdata;
+
+//    std::cout<<"len is "<<buf1len<<std::endl;
+
+    for ( bag = 0; bag < buf1len; bag++)
+    {
+        int bag_index = (pstr[bag*1400 +4]*256 + pstr[bag*1400 + 5]);
+        if(bag_index - last_bag_index > 1 && (bag_index - last_bag_index) < POINT_COUNT_PER_VIEWFIELD / MEMS_BLOCKS_PER_PACKET
+                && last_bag_index != -1)
+        {
+            int lose_bag_count = bag_index - last_bag_index - 1;
+            while(lose_bag_count --)
+            {
+                for(block = 0; block < MEMS_BLOCKS_PER_PACKET; block++)
+                {
+                    for (channel = 1; channel <MEMS_SCANS_PER_FIRING; channel++){
+                        point.x = NAN;
+                        point.y = NAN;
+                        point.z = NAN;
+                        point.intensity = 0;
+                        point_cloud->at(point_index, channel - 1) = point;
+//                        point_cloud->push_back(point);
+//                        ++point_cloud->width;
+                    }
+                    point_index++;
+                }
+            }
+        }
+        for (block = skip_block; block < MEMS_BLOCKS_PER_PACKET; block++)
+        {
+            int pitch_t = ((pstr[bag*1400+20  + block * 52]   *256) + pstr[bag*1400 + 21 + block * 52]) ;
+
+            if(last_pitch_index > pitch_t){
+                 if(last_bag_index = POINT_COUNT_PER_VIEWFIELD / MEMS_BLOCKS_PER_PACKET){
+                  pitch_max = last_pitch_index;
+             }
+                   point_index= 0;
+ //                 point_cloud->width = 0;
+                  skip_block = block;
+                  last_pitch_index = pitch_t;
+                  break;
+             }
+             last_pitch_index = pitch_t;
+             pitch = pitchToDeg(pitch_t) * ANGLE_RESOLUTION;
+             int yaw_t = ((pstr[bag*1400 +22 + block * 52] *256) + pstr[bag*1400 + 23 + block * 52]) ;
+             yaw = yawToDeg(yaw_t) * ANGLE_RESOLUTION;
+             Eigen::Matrix<float, 3, 1> n_gal;
+             Eigen::Matrix<float, 3, 1> i_out;
+             for (channel = 0; channel <(MEMS_SCANS_PER_FIRING-1); channel++){
+                 int ax, ay;
+                 float tanax, tanay;
+                 ax = (int)(100 * pitch_rate * (pitch + channel_pitch_offset[channel+1]));
+                 ay = (int)(100 * yaw_rate * (yaw + channel_yaw_offset[channel+1]));
+                 tanax = tan_lookup_table[ax + 1000];
+                 tanay = tan_lookup_table[ay + 1000];
+                 tanax = -tanax;
+                 tanay = -tanay;
+                 n_gal << tanay, -tanax, 1;
+                 n_gal.normalize();
+                 n_gal = rotate_gal_ * n_gal;
+                 i_out = input_ref_.col(channel ) - 2 * n_gal * n_gal.transpose() * input_ref_.col(channel);
+                 int Range_t = ((pstr[bag*1400 + block * 52 + 34 + 8 * channel] << 8) + pstr[bag*1400+block * 52 + 35 + 8 * channel]);
+                 float Range=pixlToDistance(Range_t,(channel+1)) *DISTANCE_RESOLUTION ;
+                 unsigned char intensity = ((pstr[bag*1400 + block * 52 + 32 + 8 * channel] << 8)
+                            + pstr[bag*1400 + block* 52 + 33 + 8 * channel]);
+                 if(Range > distance_max_thd || Range < distance_min_thd)
+                 {
+                     point.x = NAN;
+                     point.y = NAN;
+                     point.z = NAN;
+                     point.intensity = 0;
+//                     point_cloud->push_back(point);
+//                     ++point_cloud->width;
+                 }else {
+
+                     point.x = i_out(2) * Range;
+                     point.y = i_out(0) * Range;
+                     point.z = i_out(1) * Range;
+                     point.intensity = intensity;
+//                     point_cloud->push_back(point);
+//                     ++point_cloud->width;
+//                     std::cout<<" x   y   z    "<<point.x<<"   ,   "<<point.y<<"   ,  "<<point.z<< "   ,  "<<Range<<std::endl;
+                  }
+                 if(point_index < POINT_COUNT_PER_VIEWFIELD){
+                     point_cloud->at(point_index, channel) = point;
+//                     point_cloud->push_back(point);
+//                     ++point_cloud->width;
+                 }
+             }
+                point_index++;
+                skip_block = 0;
+            }
+        last_bag_index = bag_index;
+    }
+
+//    std::cout<<"point size is "<<point_cloud->width<<std::endl;
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->size()* sizeof(pcl::PointXYZI)];
+    int * pHeadSize = (int *)strOut;
+    *pHeadSize = 4 + point_cloud->header.frame_id.size()+4+8;
+    memcpy(strOut+4,point_cloud->header.frame_id.c_str(),point_cloud->header.frame_id.size());
+    pcl::uint32_t * pu32 = (pcl::uint32_t *)(strOut+4+sizeof(point_cloud->header.frame_id.size()));
+    *pu32 = point_cloud->header.seq;
+    memcpy(strOut+4+sizeof(point_cloud->header.frame_id.size()) + 4,&point_cloud->header.stamp,8);
+    pcl::PointXYZI * p;
+    p = (pcl::PointXYZI *)(strOut +4+sizeof(point_cloud->header.frame_id.size()) + 4+8 );
+    memcpy(p,point_cloud->points.data(),point_cloud->size() * sizeof(pcl::PointXYZI));
+    iv::modulecomm::ModuleSendMsg(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->size() * sizeof(pcl::PointXYZI));
+    delete strOut;
+}
+
+void  rsm1_Proc_Func(int n)
+{
+    std::cout<<"Enter rsM1_Proc_Func"<<std::endl;
+    char * strdata = new char[BK32_DATA_BUFSIZE];
+    int nIndex;
+    int nRead;
+    while(g_rs_m1_run)
+    {
+        if((nRead = rs_m1_buf->ReadData(strdata,BK32_DATA_BUFSIZE,&nIndex))>0)
+        {
+            process_rs_M1_obs(strdata,nRead);
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+    g_rs_m1_running = false;
+    std::cout<<"Exit rsm1_Proc_Func"<<std::endl;
+}
+
+
+int StartLidar_rs_m1()
+{
+    std::cout<<"Now Start rsm1 Listen."<<std::endl;
+    g_RawData_Buf = new char[BK32_DATA_BUFSIZE];
+    rs_m1_buf = new rsM1_Buf();
+    g_rs_m1_run = true;
+    g_rs_m1_running = true;
+    g_brs_m1_Proc_running = true;
+    g_lidar_pc = iv::modulecomm::RegisterSend(gstr_memname,20000000,1);
+    g_prsm1Thread = new std::thread(getPacket,0);
+    g_prsm1ProcThread = new std::thread(rsm1_Proc_Func,0);
+    return 0;
+}
+
+void StopLidar_rs16()
+{
+    std::cout<<"Now Close rsm1. "<<std::endl;
+    g_rs_m1_run = false;
+    g_prsm1Thread->join();
+    g_prsm1ProcThread->join();
+
+    delete g_prsm1ProcThread;
+    delete g_prsm1Thread;
+
+    delete rs_m1_buf;
+    delete g_RawData_Buf;
+}
+}

src/driver/driver_rs_m1/rsdriver.h

@@ -0,0 +1,82 @@
+#ifndef RSDRIVER_H
+#define RSDRIVER_H
+#include <thread>
+#include <iostream>
+#include <QUdpSocket>
+#include <QNetworkDatagram>
+#include <iostream>
+#include <QMutex>
+#include <QDateTime>
+#include "rawdata.h"
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include "modulecomm.h"
+
+namespace  lidar{
+
+extern char gstr_hostip[256];
+extern char gstr_port[256];
+
+
+    struct {
+      std::string frame_id; ///< tf frame ID
+      std::string model;    ///< device model name
+      int npackets;         ///< number of packets to collect
+      double rpm;           ///< device rotation rate (RPMs)
+      double time_offset;   ///< time in seconds added to each  time stamp
+      bool start_from_edge;
+    } config_;
+
+    void getPacket(int n);
+
+    int StartLidar_rs_m1();
+    void StopLidar_rs_m1();
+
+    class rsM1_Buf
+    {
+    private:
+        char * mstrdata;
+        int mnSize;  //Data SizeUse
+        QMutex mMutex;
+        int mIndex;
+    public:
+        rsM1_Buf()
+        {
+            mstrdata = new char[BK32_DATA_BUFSIZE];
+            mIndex = 0;
+            mnSize = 0;
+        }
+        ~rsM1_Buf()
+        {
+            delete mstrdata;
+        }
+        void WriteData(const char * strdata,const int nSize)
+        {
+            mMutex.lock();
+            memcpy(mstrdata,strdata,nSize);
+            mnSize = nSize;
+            mIndex++;
+            mMutex.unlock();
+        }
+        int ReadData(char * strdata,const int nRead,int * pnIndex)
+        {
+            int nRtn = 0;
+            if(mnSize <= 0)return 0;
+            mMutex.lock();
+            nRtn = mnSize;
+            if(nRtn >nRead)
+            {
+                nRtn = nRead;
+                std::cout<<"lidar_rsM1 rsM1_Buf ReadData data nRead = "<<nRead<<" is small"<<std::endl;
+            }
+            memcpy(strdata,mstrdata,nRtn);
+            mnSize = 0;
+            if(pnIndex != 0)*pnIndex = mIndex;
+            mMutex.unlock();
+            return nRtn;
+        }
+    };
+}
+
+
+#endif // RSDRIVER_H