#include #include "modulecomm.h" #include "input.h" #include "rsdriver.h" #include "convert.h" #include #include #include volatile sig_atomic_t flag = 1; std::shared_ptr grawscan; QMutex gMutexScan; bool gbUpdate = false; QMutex gMutexWait; QWaitCondition gWait; rslidar_pointcloud::Convert * gpConvert; void * g_lidar_pc; std::thread * gthreadpoll,*gthreadconvert; void PublishPointCloud(std::shared_ptr rawscan) { pcl::PointCloud::Ptr point_cloud; gpConvert->processScan(rawscan,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(g_lidar_pc,strOut,4+sizeof(point_cloud->header.frame_id.size()) + 4+8+point_cloud->width * sizeof(pcl::PointXYZI)); delete strOut; } void pollthread() { rs_driver::rslidarDriver lidarm1; while(true) { std::shared_ptr rawscan; if(true == lidarm1.poll(rawscan)) { gMutexScan.lock(); grawscan = rawscan; gbUpdate = true; gMutexScan.unlock(); gWait.wakeAll(); } } } void convertthread() { while(true) { gMutexWait.lock(); gWait.wait(&gMutexWait,300); gMutexWait.unlock(); if(gbUpdate) { gMutexScan.lock(); std::shared_ptr rawscan; rawscan = grawscan; gbUpdate = false; gMutexScan.unlock(); PublishPointCloud(rawscan); } } } int main(int argc, char *argv[]) { QCoreApplication a(argc, argv); g_lidar_pc = iv::modulecomm::RegisterSend("lidar_pc",20000000,1); gpConvert = new rslidar_pointcloud::Convert("ChannelNum.csv","limit.csv"); gthreadpoll = new std::thread(pollthread); gthreadconvert = new std::thread(convertthread); return a.exec(); }