modularization/src/driver/driver_lidar_merge/main.cpp

#include <QCoreApplication>
#include <QDateTime>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>

#include <iostream>
#include <fstream>
#include <yaml-cpp/yaml.h>
#include <QMutex>
#include <vector>
#include <thread>

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


QMutex gMutex;

static qint64 glasttime = 0;
static std::vector<iv::lidar_data> gvectorlidar;
static char gstroutmemname[255];
static void * gpaout;

void dec_yaml(const char * stryamlpath)
{

    YAML::Node config;
    try
    {
        config = YAML::LoadFile(stryamlpath);
    }
    catch(YAML::BadFile e)
    {
        qDebug("load error.");
        return;
    }

    int i;
    int nlidarsize;
    std::vector<std::string> veclidarname;

    if(config["lidar"])
    {
        qDebug("have lidar size is %d",config["lidar"].size());
        nlidarsize = config["lidar"].size();
        for(i=0;i<nlidarsize;i++)
        {
            std::string strname = config["lidar"][i].as<std::string>();
            veclidarname.push_back(strname);
        }
    }
    else
    {
        return;
    }

    if(nlidarsize <1)return;

    std::string strmemname;
    std::string stroffset_x;
    std::string stroffset_y;
    std::string stroffset_z;

    for(i=0;i<nlidarsize;i++)
    {
        if(config[veclidarname[i].data()])
        {
            if((config[veclidarname[i].data()]["memname"])&&(config[veclidarname[i].data()]["offset"]["x"])&&(config[veclidarname[i].data()]["offset"]["y"])&&(config[veclidarname[i].data()]["offset"]["z"]))
            {
                strmemname = config[veclidarname[i].data()]["memname"].as<std::string>();
                stroffset_x = config[veclidarname[i].data()]["offset"]["x"].as<std::string>();
                stroffset_y = config[veclidarname[i].data()]["offset"]["y"].as<std::string>();
                stroffset_z = config[veclidarname[i].data()]["offset"]["z"].as<std::string>();

                iv::lidar_data xlidardata;
                xlidardata.foff_x = atof(stroffset_x.data());
                xlidardata.foff_y = atof(stroffset_y.data());
                xlidardata.foff_z = atof(stroffset_z.data());
                strncpy(xlidardata.strmemname,strmemname.data(),255);
                gvectorlidar.push_back(xlidardata);
                qDebug("name is %s ",strmemname.data());
            }
        }
    }

    if(config["output"])
    {
       strncpy(gstroutmemname,config["output"].as<std::string>().data(),255);
    }
    else
    {
        strncpy(gstroutmemname,"lidar_pc",255);
    }
    return;

}

void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    if(nSize <=16)return;
    unsigned int * pHeadSize = (unsigned int *)strdata;
    if(*pHeadSize > nSize)
    {
        std::cout<<"ListenPointCloud data is small headsize ="<<*pHeadSize<<"  data size is"<<nSize<<std::endl;
    }

    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());
    int nNameSize;
    nNameSize = *pHeadSize - 4-4-8;
    char * strName = new char[nNameSize+1];strName[nNameSize] = 0;
    memcpy(strName,(char *)((char *)strdata +4),nNameSize);
    point_cloud->header.frame_id = strName;
    memcpy(&point_cloud->header.seq,(char *)strdata+4+nNameSize,4);
    memcpy(&point_cloud->header.stamp,(char *)strdata+4+nNameSize+4,8);
    int nPCount = (nSize - *pHeadSize)/sizeof(pcl::PointXYZI);
    int i;
    pcl::PointXYZI * p;
    p = (pcl::PointXYZI *)((char *)strdata + *pHeadSize);
    for(i=0;i<nPCount;i++)
    {
        pcl::PointXYZI xp;
        xp.x = p->x;
        xp.y = p->y;
        xp.z = p->z;
        xp.intensity = p->intensity;
        point_cloud->push_back(xp);
        p++;

  //      std::cout<<" x is "<<xp.x<<" y is "<<xp.y<<std::endl;
    }


    for(i=0;i<gvectorlidar.size();i++)
    {
        if(strncmp(strmemname,gvectorlidar[i].strmemname,255) == 0)
        {
            gMutex.lock();
            gvectorlidar[i].mpoint_cloud = point_cloud;
            gvectorlidar[i].bUpdate = true;
            gMutex.unlock();
        }
    }
}

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

void merge()
{
    int i;
    std::vector<iv::lidar_data> xvectorlidar;
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
                new pcl::PointCloud<pcl::PointXYZI>());



    gMutex.lock();
    xvectorlidar = gvectorlidar;
    for(i=0;i<gvectorlidar.size();i++)
    {
        gvectorlidar[i].bUpdate = false;
    }
    gMutex.unlock();
    point_cloud = mergefunc(xvectorlidar);
    sharepointcloud(point_cloud,gpaout);
}

void mergethread()
{
    int i;
    while(1)
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
        bool bNOtAllOK = false;
        for(i=0;i<gvectorlidar.size();i++)
        {
            if(!gvectorlidar[i].bUpdate)
            {
                bNOtAllOK = true;
                break;
            }
        }
        if(((QDateTime::currentMSecsSinceEpoch() - glasttime)>150) ||(bNOtAllOK == false))
        {
            merge();
            qDebug("time is %ld",glasttime);
            glasttime = QDateTime::currentMSecsSinceEpoch();

        }

    }
}


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

    dec_yaml("driver_lidar_merge.yaml");

    void * pa;
    int i;
    for(i=0;i<gvectorlidar.size();i++)
    {
        iv::modulecomm::RegisterRecv(gvectorlidar[i].strmemname,ListenPointCloud);
    }
    gpaout = iv::modulecomm::RegisterSend(gstroutmemname,20000000,1);

    std::thread xthread(mergethread);

    return a.exec();
}