change src1 add new driver_lidar_rs16.

src1/common/modulecomm/modulecomm.h

@@ -21,12 +21,7 @@
 
 
 
-enum ModuleComm_TYPE
-{
-    ModuleComm_SHAREMEM = 0,
-    ModuleComm_INTERIOR = 1,
-    ModuleComm_FASTRTPS = 2
-};
+
 
 //#include <iostream>
 //#include <thread>
@@ -42,6 +37,15 @@ typedef void (* SMCallBack)(const char * strdata,const unsigned int nSize,const
 
 namespace iv {
 namespace modulecomm {
+
+enum ModuleComm_TYPE
+{
+    ModuleComm_SHAREMEM = 0,
+    ModuleComm_INTERIOR = 1,
+    ModuleComm_FASTRTPS = 2
+};
+
+
 void * MODULECOMMSHARED_EXPORT RegisterSend(const char * strcommname,const unsigned int nBufSize,const unsigned int nMsgBufCount
                                             ,ModuleComm_TYPE xmctype = ModuleComm_SHAREMEM);
 void * MODULECOMMSHARED_EXPORT RegisterRecv(const char * strcommname,SMCallBack pCall,

src1/common/modulecomm/modulecomm.pro

@@ -22,6 +22,8 @@ SOURCES += \
 HEADERS += \
     modulecomm.h
 
+CONFIG += plugin
+
 # Default rules for deployment.
 unix {
     target.path = /usr/lib

src1/common/modulecomm_fastrtps/modulecomm_fastrtps.pro

@@ -9,7 +9,7 @@ QT       -= gui
 
 #DEFINES += dds_use_shm
 
-TARGET = modulecomm
+TARGET = modulecomm_fastrtps
 TEMPLATE = lib
 
 DEFINES += MODULECOMM_FASTRTPS_LIBRARY

src1/common/modulecomm_inter/intercomm.cpp

@@ -61,7 +61,7 @@ intercomm::intercomm(const char * strsmname,const unsigned int nBufSize,const un
         if(p == 0)
         {
             interunit * pnewinter = new interunit;
-            strncpy(p->strintername,strsmname,256);
+            strncpy(pnewinter->strintername,strsmname,256);
             pnewinter->strdatabuf = new char[sizeof(procinter_info)+nMaxPacCount*sizeof(procinter_head) + nBufSize];
             pnewinter->nPacCount = nMaxPacCount;
             pnewinter->nbufsize = nBufSize;

src1/driver/driver_lidar_rs16/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src1/driver/driver_lidar_rs16/driver_lidar_rs16.pro

@@ -0,0 +1,61 @@
+QT -= gui
+
+QT += network
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has 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 it uses 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 += \
+        ../../interface/ivmodule.cpp \
+        ../interface/ivdriver.cpp \
+        ../interface/ivdriver_lidar.cpp \
+        ../interface/lidarmain.cpp \
+        ivdriver_lidar_rs16.cpp \
+        main.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 += \
+    ../../interface/ivmodule.h \
+    ../interface/ivdriver.h \
+    ../interface/ivdriver_lidar.h \
+    ../interface/lidarmain.h \
+    ivdriver_lidar_rs16.h
+
+!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!" )
+}
+
+
+INCLUDEPATH += $$PWD/../interface
+INCLUDEPATH += $$PWD/../../interface
+
+
+INCLUDEPATH += $$PWD/../../common/modulecomm
+
+INCLUDEPATH += $$PWD/../../common/modulecomm_shm
+INCLUDEPATH += $$PWD/../../common/modulecomm_fastrtps
+INCLUDEPATH += $$PWD/../../common/modulecomm_inter
+
+LIBS += -L$$PWD/../../common/build-modulecomm-Debug -lmodulecomm
+LIBS += -L$$PWD/../../common/build-modulecomm_fastrtps-Debug -lmodulecomm_fastrtps
+LIBS += -L$$PWD/../../common/build-modulecomm_shm-Debug -lmodulecomm_shm
+LIBS += -L$$PWD/../../common/build-modulecomm_inter-Debug -lmodulecomm_inter

src1/driver/driver_lidar_rs16/ivdriver_lidar_rs16.cpp

@@ -0,0 +1,139 @@
+#include "ivdriver_lidar_rs16.h"
+
+namespace iv {
+
+
+ivdriver_lidar_rs16::ivdriver_lidar_rs16()
+{
+
+}
+
+int ivdriver_lidar_rs16::processudpData(QByteArray ba, pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud)
+{
+    static double fAngle_Total = 0;
+    static double fAngle_Last = 0;
+
+    int nrtn = 0;
+
+    float Ang = 0;
+    float Range = 0;
+    int Group = 0;
+    int pointi = 0;
+    float wt = 0;
+    int wt1 = 0;
+
+    float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,15,13,11,9,7,5,3,1};
+
+    char * pstr = ba.data();
+
+    wt1 = ((pstr[2 + Group * 100] *256) + pstr[ 3 + Group * 100]) ;
+    wt = wt1/ 100.0;
+
+    double fAngX = wt;
+    if(fabs(fAngX-fAngle_Last)>300)
+    {
+        fAngle_Total = fAngle_Total + fabs(fabs(fAngX-fAngle_Last)-360);
+    }
+    else
+    {
+        fAngle_Total = fAngle_Total + fabs(fabs(fAngX-fAngle_Last));
+    }
+
+    fAngle_Last = fAngX;
+    if(fAngle_Total > 360)
+    {
+        nrtn = 1;
+        fAngle_Total = 0;
+    }
+
+
+    for (Group = 0; Group <= 11; Group++)
+    {
+        wt1 = ((pstr[2 + Group * 100] *256) + pstr[ 3 + Group * 100]) ;
+        wt = wt1/ 100.0;
+
+
+        for (pointi = 0; pointi <16; pointi++)
+        {
+//                        Ang = (0 - wt - w * T[pointi] - H_BETA[pointi]+213) / 180.0 * Lidar_Pi;
+            Ang = (0 - wt) / 180.0 * M_PI;
+            Range = ((pstr[ Group * 100 + 4 + 3 * pointi] << 8) + pstr[Group * 100 + 5 + 3 * pointi]);
+            unsigned char intensity = pstr[ Group * 100 + 6 + 3 * pointi];
+            Range=Range* 5.0/1000.0;
+
+            if(Range<150)
+            {
+                pcl::PointXYZI point;
+                point.x  = Range*cos(V_theta[pointi] / 180 * M_PI)*cos(Ang + mfrollang);
+                point.y  = Range*cos(V_theta[pointi] / 180 * M_PI)*sin(Ang + mfrollang);
+                point.z  = Range*sin(V_theta[pointi] / 180 * M_PI);
+                if(mbinclix)
+                {
+                    double y,z;
+                    y = point.y;
+                    z = point.z;
+                    point.y = y*mcos_finclinationang_xaxis +z*msin_finclinationang_xaxis;
+                    point.z = z*mcos_finclinationang_xaxis - y*msin_finclinationang_xaxis;
+                }
+                if(mbincliy)
+                {
+                    double z,x;
+                    z = point.z;
+                    x = point.x;
+                    point.z = z*mcos_finclinationang_yaxis + x*msin_finclinationang_yaxis;
+                    point.x = x*mcos_finclinationang_yaxis - z*msin_finclinationang_yaxis;
+                }
+                point.intensity = intensity;
+                point_cloud->points.push_back(point);
+
+
+                ++point_cloud->width;
+            }
+        }
+
+
+        wt = wt + 0.18;
+
+
+
+        for (pointi = 0; pointi < 16; pointi++)
+        {
+            Ang = (0 - wt) / 180.0 * M_PI;
+   //         Ang = Ang+angdiff;
+            Range = ((pstr[ Group * 100 + 52 + 3 * pointi] << 8) + pstr[Group * 100 + 53 + 3 * pointi]);
+            unsigned char intensity = pstr[ Group * 100 + 54 + 3 * pointi];
+            Range=Range* 5.0/1000.0;
+
+            if(Range<150)
+            {
+                pcl::PointXYZI point;
+                point.x  = Range*cos(V_theta[pointi] / 180 * M_PI)*cos(Ang + mfrollang);
+                point.y  = Range*cos(V_theta[pointi] / 180 * M_PI)*sin(Ang + mfrollang);
+                point.z  = Range*sin(V_theta[pointi] / 180 * M_PI);
+                if(mbinclix)
+                {
+                    double y,z;
+                    y = point.y;z = point.z;
+                    point.y = y*mcos_finclinationang_xaxis +z*msin_finclinationang_xaxis;
+                    point.z = z*mcos_finclinationang_xaxis - y*msin_finclinationang_xaxis;
+                }
+                if(mbincliy)
+                {
+                    double z,x;
+                    z = point.z;x = point.x;
+                    point.z = z*mcos_finclinationang_yaxis + x*msin_finclinationang_yaxis;
+                    point.x = x*mcos_finclinationang_yaxis - z*msin_finclinationang_yaxis;
+                }
+                point.intensity = intensity;
+                point_cloud->points.push_back(point);
+
+
+                ++point_cloud->width;
+            }
+        }
+    }
+    return nrtn;
+
+}
+
+}

src1/driver/driver_lidar_rs16/ivdriver_lidar_rs16.h

@@ -0,0 +1,20 @@
+#ifndef IVDRIVER_LIDAR_RS16_H
+#define IVDRIVER_LIDAR_RS16_H
+
+#include "ivdriver_lidar.h"
+
+namespace iv {
+
+
+class ivdriver_lidar_rs16 : public ivdriver_lidar
+{
+public:
+    ivdriver_lidar_rs16();
+    virtual int processudpData(QByteArray ba,pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud);
+
+
+};
+
+}
+
+#endif // IVDRIVER_LIDAR_RS16_H

src1/driver/driver_lidar_rs16/main.cpp

@@ -0,0 +1,19 @@
+#include <QCoreApplication>
+
+#include "lidarmain.h"
+
+#include "ivdriver_lidar_rs16.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    iv::ivdriver_lidar * pivm = new iv::ivdriver_lidar_rs16();
+    int nrtn = lidarmain(pivm,argc,argv,&a,"driver_lidar_rs16");
+
+    if(nrtn == 0)
+    {
+        return 0;
+    }
+    return a.exec();
+}

src1/driver/interface/ivdriver.cpp

@@ -0,0 +1,10 @@
+#include "ivdriver.h"
+
+namespace iv {
+
+ivdriver::ivdriver()
+{
+
+}
+
+}

src1/driver/interface/ivdriver.h

@@ -0,0 +1,16 @@
+#ifndef IVDRIVER_H
+#define IVDRIVER_H
+
+#include "ivmodule.h"
+
+namespace  iv {
+
+class ivdriver : public ivmodule
+{
+public:
+    ivdriver();
+};
+
+}
+
+#endif // IVDRIVER_H

src1/driver/interface/ivdriver_lidar.cpp

@@ -0,0 +1,141 @@
+#include "ivdriver_lidar.h"
+
+#include <iostream>
+
+namespace  iv {
+ivdriver_lidar::ivdriver_lidar()
+{
+    strncpy(mstr_hostip,"0.0.0.0",256);
+    strncpy(mstr_port,"6699",256);
+    strncpy(mstr_memname,"lidar_pc",256);
+}
+
+void ivdriver_lidar::RegisterPointMsg(const char * strmemname,iv::modulecomm::ModuleComm_TYPE xtype)
+{
+    mpa = iv::modulecomm::RegisterSend(strmemname,10000000,1,xtype);
+}
+
+
+
+void ivdriver_lidar::SharePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud)
+{
+    char * strOut = new char[4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * 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->width * sizeof(pcl::PointXYZI));
+
+    iv::modulecomm::ModuleSendMsg(mpa,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
+    delete strOut;
+}
+
+
+void ivdriver_lidar::modulerun()
+{
+
+    if(mfinclinationang_xaxis != 0.0)mbinclix = true;
+    if(mfinclinationang_yaxis != 0.0)mbincliy = true;
+    mcos_finclinationang_xaxis= cos(mfinclinationang_xaxis);
+    msin_finclinationang_xaxis = sin(mfinclinationang_xaxis);
+    mcos_finclinationang_yaxis = cos(mfinclinationang_yaxis);
+    msin_finclinationang_yaxis = sin(mfinclinationang_yaxis);
+
+    RegisterPointMsg(mstr_memname,getdefefaultcommtype());
+
+    mbrunudpthread = true;
+    std::thread * xudpthread = new std::thread(&ivdriver_lidar::threadudp,this);
+
+    QDateTime dt = QDateTime::currentDateTime();
+    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 = dt.currentMSecsSinceEpoch();
+    point_cloud->width = 0;
+    point_cloud->header.seq =m_seq;
+    m_seq++;
+
+    while(mbrun)
+    {
+        if(mvectorba.size()>0)
+        {
+            mMutexba.lock();
+            QByteArray ba = mvectorba[0];
+            mvectorba.erase(mvectorba.begin());
+            mMutexba.unlock();
+            if(processudpData(ba,point_cloud) == 1)
+            {
+                SharePointCloud(point_cloud);
+                point_cloud->clear();
+                point_cloud->height = 1;
+                point_cloud->header.stamp = dt.currentMSecsSinceEpoch();
+                point_cloud->width = 0;
+                point_cloud->header.seq =m_seq;
+                m_seq++;
+            }
+        }
+        else
+        {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+    }
+
+    mbrunudpthread = false;
+    xudpthread->join();
+
+
+}
+
+void ivdriver_lidar::threadudp()
+{
+    QUdpSocket * udpSocket = new QUdpSocket( );
+    bool bbind =     udpSocket->bind(QHostAddress(mstr_hostip), atoi(mstr_port));
+    if(bbind == false)
+    {
+        qDebug("bind socket %s:%s error.",mstr_hostip,mstr_port);
+        return;
+    }
+    int nnotrecv = 0;
+    while(mbrunudpthread)
+    {
+        if(udpSocket->hasPendingDatagrams())
+        {
+            if(nnotrecv > 10000)
+            {
+                std::cout<<" Recv Data."<<std::endl;
+            }
+            QNetworkDatagram datagram = udpSocket->receiveDatagram();
+            mMutexba.lock();
+            if(mvectorba.size()<1000)
+            {
+                mvectorba.push_back(datagram.data());
+            }
+            else
+            {
+                std::cout<<" proc buffer is full."<<std::endl;
+            }
+            mMutexba.unlock();
+            datagram.clear();
+            nnotrecv = 0;
+        }
+        else
+        {
+            nnotrecv++;
+//            std::cout<<"running."<<std::endl;
+            std::this_thread::sleep_for(std::chrono::microseconds(100));
+            if(nnotrecv == 10000)
+            {
+                std::cout<<"not recv datagram."<<std::endl;
+            }
+        }
+    }
+}
+
+}

src1/driver/interface/ivdriver_lidar.h

@@ -0,0 +1,64 @@
+#ifndef IVDRIVER_LIDAR_H
+#define IVDRIVER_LIDAR_H
+
+#include <QUdpSocket>
+#include <QNetworkDatagram>
+#include <vector>
+#include <QMutex>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include "ivdriver.h"
+
+#include "modulecomm.h"
+
+namespace  iv {
+
+
+class ivdriver_lidar : public ivdriver
+{
+public:
+    ivdriver_lidar();
+
+    void RegisterPointMsg(const char * strmemname,iv::modulecomm::ModuleComm_TYPE xtype);
+    void SharePointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud);
+    virtual void modulerun();
+    virtual int processudpData(QByteArray ba,pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud) = 0;
+
+private:
+    void * mpa;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    qint64 m_seq = 0;
+public:
+    char mstr_memname[256];
+    double mfrollang = 0;
+    double  mfinclinationang_yaxis = 0;  //from y axis
+    double mfinclinationang_xaxis = 0;  //from x axis
+    char mstr_hostip[256];
+    char mstr_port[256];
+    double mfoffset_x = 0;
+    double mfoffset_y = 0;
+    double mfoffset_z = 0;
+
+    bool mbinclix = false;
+    bool mbincliy = false;
+ //   if(finclinationang_xaxis != 0.0)binclix = true;
+ //   if(finclinationang_yaxis != 0.0)bincliy = true;
+
+    double mcos_finclinationang_xaxis;// = cos(mfinclinationang_xaxis);
+    double msin_finclinationang_xaxis;// = sin(mfinclinationang_xaxis);
+    double mcos_finclinationang_yaxis;// = cos(mfinclinationang_yaxis);
+    double msin_finclinationang_yaxis;// = sin(mfinclinationang_yaxis);
+
+private:
+    void threadudp();
+    bool mbrunudpthread = false;
+    std::vector<QByteArray> mvectorba;
+    QMutex mMutexba;
+
+};
+
+}
+
+#endif // IVDRIVER_LIDAR_H

src1/driver/interface/lidarmain.cpp

@@ -0,0 +1,220 @@
+#include <QCoreApplication>
+
+
+#include "ivdriver_lidar_rs16.h"
+
+#include <signal.h>
+
+#include <getopt.h>
+#include <iostream>
+
+#include <iostream>
+#include <fstream>
+#include <yaml-cpp/yaml.h>
+
+iv::ivmodule * gpivmodule;
+QCoreApplication * gpapp;
+
+void sigint_handler(int sig){
+    if(sig == SIGINT){
+        // ctrl+c退出时执行的代码
+        delete gpivmodule;
+        gpapp->exit(0);
+    }
+}
+char gstr_memname[256];
+char gstr_rollang[256];
+char gstr_inclinationang_yaxis[256];  //from y axis
+char gstr_inclinationang_xaxis[256];  //from x axis
+char gstr_hostip[256];
+char gstr_port[256];
+char gstr_yaml[256];
+
+
+void print_useage()
+{
+    std::cout<<" -m --memname $memname : share memory name. eq.  -m lidar_pc"<<std::endl;
+    std::cout<<" -r --rollang $rollang : roll angle. eq.  -r 10.0"<<std::endl;
+    std::cout<<" -x --inclinationang_xaxis $inclinationang_xaxis : inclination angle from x axis. eq.  -x 0.0"<<std::endl;
+    std::cout<<" -y --inclinationang_yaxis $inclinationang_yaxis : inclination angle from y axis. eq.  -y 0.0"<<std::endl;
+    std::cout<<" -o --hostip $hostip : host ip. eq.  -o 192.168.1.111"<<std::endl;
+    std::cout<<" -p --port $port : port . eq.  -p 2368"<<std::endl;
+    std::cout<<" -s --setyaml $yaml : port . eq.  -s rs1.yaml"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "m:r:x:y:o:p:s:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"memname", required_argument, NULL, 'm'},
+        {"rollang", required_argument, NULL, 'r'},
+        {"inclinationang_xaxis", required_argument, NULL, 'x'},
+        {"inclinationang_yaxis", required_argument, NULL, 'y'},
+        {"hostip", required_argument, NULL, 'o'},
+        {"port", required_argument, NULL, 'p'},
+        {"setyaml", required_argument, NULL, 's'},
+        {"help",  no_argument,       NULL, 'h'},
+ //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                               argv,
+                               optstring,
+                               long_options,
+                               &option_index)) != -1) {
+//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+//        printf("optarg = %s\n", optarg); // 参数内容
+//        printf("optind = %d\n", optind); // 下一个被处理的下标值
+//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+//        printf("\n");
+        switch(opt)
+        {
+        case 'm':
+            strncpy(gstr_memname,optarg,255);
+            break;
+        case 'r':
+            strncpy(gstr_rollang,optarg,255);
+            break;
+        case 'x':
+            strncpy(gstr_inclinationang_xaxis,optarg,255);
+            break;
+        case 'y':
+            strncpy(gstr_inclinationang_yaxis,optarg,255);
+            break;
+        case 'o':
+            strncpy(gstr_hostip,optarg,255);
+            break;
+        case 'p':
+            strncpy(gstr_port,optarg,255);
+            break;
+        case 's':
+            strncpy(gstr_yaml,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
+void decodeyaml(const char * stryaml)
+{
+    YAML::Node config;
+    try
+    {
+        config = YAML::LoadFile(stryaml);
+    }
+    catch(YAML::BadFile e)
+    {
+        qDebug("load yaml error.");
+        return;
+    }
+
+
+    if(config["memname"])
+    {
+        strncpy(gstr_memname,config["memname"].as<std::string>().data(),255);
+    }
+    if(config["rollang"])
+    {
+        strncpy(gstr_rollang,config["rollang"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_xaxis"])
+    {
+        strncpy(gstr_inclinationang_xaxis,config["inclinationang_xaxis"].as<std::string>().data(),255);
+    }
+    if(config["inclinationang_yaxis"])
+    {
+        strncpy(gstr_inclinationang_yaxis,config["inclinationang_yaxis"].as<std::string>().data(),255);
+    }
+    if(config["hostip"])
+    {
+        strncpy(gstr_hostip,config["hostip"].as<std::string>().data(),255);
+    }
+    if(config["port"])
+    {
+        strncpy(gstr_port,config["port"].as<std::string>().data(),255);
+    }
+
+
+
+//    std::cout<<gstr_memname<<std::endl;
+//    std::cout<<gstr_rollang<<std::endl;
+//    std::cout<<gstr_inclinationang_xaxis<<std::endl;
+//    std::cout<<gstr_inclinationang_yaxis<<std::endl;
+//    std::cout<<gstr_hostip<<std::endl;
+//    std::cout<<gstr_port<<std::endl;
+}
+
+
+
+int lidarmain(iv::ivdriver_lidar * pivm,int argc, char *argv[],QCoreApplication * pa,const char * strmodulename)
+{
+
+    snprintf(gstr_memname,255,"lidar_pc");
+    snprintf(gstr_rollang,255,"-9.0");
+    snprintf(gstr_inclinationang_xaxis,255,"0.0");
+    snprintf(gstr_inclinationang_yaxis,255,"0");
+    snprintf(gstr_hostip,255,"0.0.0.0");
+    snprintf(gstr_port,255,"6699");//默认端口号
+    snprintf(gstr_yaml,255,"");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstr_yaml,255)>0)
+    {
+        decodeyaml(gstr_yaml);
+    }
+
+    strncpy(pivm->mstr_memname,gstr_memname,256);
+    strncpy(pivm->mstr_hostip,gstr_hostip,256);
+    strncpy(pivm->mstr_port,gstr_port,256);
+    pivm->mfrollang = atof(gstr_rollang)*M_PI/180.0;
+    pivm->mfinclinationang_xaxis = atof(gstr_inclinationang_xaxis)*M_PI/180.0;
+    pivm->mfinclinationang_yaxis = atof(gstr_inclinationang_yaxis)*M_PI/180.0;
+
+    iv::ivmodule * pivmodule = pivm;
+    signal(SIGINT, sigint_handler);
+    gpivmodule = pivmodule;
+    pivmodule->start();
+    gpapp = pa;
+
+    return 1;
+
+
+}

src1/driver/interface/lidarmain.h

@@ -0,0 +1,8 @@
+#ifndef LIDARMAIN_H
+#define LIDARMAIN_H
+
+#include "ivmodule.h"
+#include "ivdriver_lidar.h"
+
+int lidarmain(iv::ivdriver_lidar * pivm,int argc, char *argv[],QCoreApplication * pa,const char * strmodulename);
+#endif // LIDARMAIN_H

src1/interface/ivmodule.cpp

@@ -0,0 +1,45 @@
+#include "ivmodule.h"
+
+#include <string.h>
+
+namespace iv {
+
+
+ivmodule::ivmodule()
+{
+
+}
+
+ivmodule::~ivmodule()
+{
+    mbrun = false;
+    mpthread->join();
+}
+
+void ivmodule::start()
+{
+    mbrun = true;
+    mpthread = new std::thread(&ivmodule::modulerun,this);
+}
+
+void ivmodule::modulerun()
+{
+
+}
+
+void ivmodule::setmodulename(const char *strmodulename)
+{
+    strncpy(mstrmodulename,strmodulename,256);
+}
+
+void ivmodule::setdefaultcommtype(iv::modulecomm::ModuleComm_TYPE xtype)
+{
+    mdefcommtype = xtype;
+}
+
+iv::modulecomm::ModuleComm_TYPE ivmodule::getdefefaultcommtype()
+{
+    return mdefcommtype;
+}
+
+}

src1/interface/ivmodule.h

@@ -0,0 +1,31 @@
+#ifndef IVMODULE_H
+#define IVMODULE_H
+
+#include <thread>
+
+#include "modulecomm.h"
+
+namespace  iv {
+
+class ivmodule
+{
+public:
+    ivmodule();
+    ~ivmodule();
+
+public:
+    void start();
+    virtual void modulerun();
+    void setmodulename(const char * strmodulename);
+    void setdefaultcommtype(iv::modulecomm::ModuleComm_TYPE xtype);
+    iv::modulecomm::ModuleComm_TYPE getdefefaultcommtype();
+public:
+    bool mbrun = false;
+private:
+    std::thread * mpthread;
+    iv::modulecomm::ModuleComm_TYPE mdefcommtype = iv::modulecomm::ModuleComm_SHAREMEM;
+    char mstrmodulename[256];
+};
+}
+
+#endif // IVMODULE_H

src1/test/testmodulecomm_inter/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src1/test/testmodulecomm_inter/main.cpp

@@ -0,0 +1,42 @@
+#include <QCoreApplication>
+
+#include <thread>
+
+#include "modulecomm.h"
+
+void * gpa;
+
+std::chrono::time_point<std::chrono::steady_clock, std::chrono::duration<double,std::nano>> t1,t2;
+
+void testcall(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    qint64 sendtime;
+    t2 = std::chrono::steady_clock::now();
+    double dr_ns = std::chrono::duration<double,std::nano>(t2-t1).count();
+    memcpy(&sendtime,strdata,8);
+    qDebug("lat is %d ns is %f ",QDateTime::currentMSecsSinceEpoch() - sendtime,dr_ns);
+}
+
+void threadsend()
+{
+    char * strdata = new char[3000000];
+    while(1)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+        qint64 ntime = QDateTime::currentMSecsSinceEpoch();
+        memcpy(strdata,&ntime,8);
+        iv::modulecomm::ModuleSendMsg(gpa,strdata,2000000);
+        t1 =  std::chrono::steady_clock::now();
+        qDebug("send msg.");
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    gpa = iv::modulecomm::RegisterSend("test1",3000000,1,iv::modulecomm::ModuleComm_INTERIOR);
+    iv::modulecomm::RegisterRecv("test1",testcall,iv::modulecomm::ModuleComm_INTERIOR);
+    std::thread * xthread = new std::thread(threadsend);
+    return a.exec();
+}

src1/test/testmodulecomm_inter/testmodulecomm_inter.pro

@@ -0,0 +1,35 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has 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 it uses 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
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+INCLUDEPATH += $$PWD/../../common/modulecomm
+
+INCLUDEPATH += $$PWD/../../common/modulecomm_shm
+INCLUDEPATH += $$PWD/../../common/modulecomm_fastrtps
+INCLUDEPATH += $$PWD/../../common/modulecomm_inter
+
+LIBS += -L$$PWD/../../common/build-modulecomm-Debug -lmodulecomm
+LIBS += -L$$PWD/../../common/build-modulecomm_fastrtps-Debug -lmodulecomm_fastrtps
+LIBS += -L$$PWD/../../common/build-modulecomm_shm-Debug -lmodulecomm_shm
+LIBS += -L$$PWD/../../common/build-modulecomm_inter-Debug -lmodulecomm_inter
+