modularization/src/driver/driver_lidar_vtdpoint/main.cpp

#include <QCoreApplication>

#include <QDateTime>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include "modulecomm.h"
#include "ivversion.h"

void * gpvtdpoint, * g_lidar_pc;

static int g_seq = 0;

void Listenvtdpoint(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * pdt,const char * strmemname)
{

    if(nSize < 12)return;

    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 =g_seq;
    g_seq++;

    int nmark[3];
    memcpy(nmark,strdata,3*sizeof(int));

    if(nSize<(nmark[0]*nmark[1]*nmark[2]))
    {
        std::cout<<"Listenvtdpoint point is small."<<std::endl;
        return;
    }

    int npointcount = nmark[0]*nmark[1];

    int i;
    float * pvalue = (float *)(strdata + 3*sizeof(int));

    for(i=0;i<npointcount;i++)
    {
        float x,y,z;
        x = pvalue[3*i+0];
        y = pvalue[3*i+1];
        z = pvalue[3*i+2];

        pcl::PointXYZI point;
        point.x = x;
        point.y = y;
        point.z = z;
        point.intensity = 30;
        point_cloud->points.push_back(point);
        ++point_cloud->width;
    }


    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(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI));
    delete strOut;
}


int main(int argc, char *argv[])
{
    showversion("driver_lidar_vtdpoint");
    QCoreApplication a(argc, argv);

    gpvtdpoint = iv::modulecomm::RegisterRecv("vtdpoint",Listenvtdpoint);
    g_lidar_pc = iv::modulecomm::RegisterSend("lidar_pc",20000000,1);

    return a.exec();
}