#include "lidar_16z.h" static float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,1,3,5,7,9,11,13,15}; static float H_Beta[16] = {-2.5,-2.5 ,-2.5 ,-2.5,-2.5,-2.5 ,-2.5 ,-2.5, 2.5,-2.5 ,-2.5 ,-2.5, -2.5,-2.5 ,-2.5 ,-2.5}; lidar_16z::lidar_16z() { pcl::PointCloud::Ptr point_cloud( new pcl::PointCloud()); point_cloud->header.frame_id = "velodyne"; point_cloud->height = 1; point_cloud->header.stamp = static_cast(std::chrono::system_clock::now().time_since_epoch().count()/1000); point_cloud->width = 0; point_cloud->header.seq =0; mpoint_cloud_temp = point_cloud; mpdriver = new lidar_bkdriver(); mpTheadDecode = new std::thread(&lidar_16z::ThreadDecode,this); } lidar_16z::~lidar_16z(){ mbRun = false; mpTheadDecode->join(); delete mpdriver; } void lidar_16z::ThreadDecode() { static double azutotal = 0.0; static int g_seq = 0; while(mbRun) { std::shared_ptr pdata_ptr; int64_t nrecvtime; int ndatasize = 0; int nread = mpdriver->GetRecvData(pdata_ptr,nrecvtime,ndatasize); if(nread == 1) { std::cout<<"read "<(*pang_start) * 0.01; for(i=0;i<12;i++) { int j; for(j=0;j<16;j++) { unsigned char * pecho = (unsigned char *)(p +nheadlen + i*96 + j*6); unsigned short * prange; unsigned char * pchch; double frange; unsigned char nch; unsigned char intensity; pchch = pecho; prange = (unsigned short *)(pecho +3); intensity = *(pecho + 5); nch = * pchch; frange = static_cast(*prange); frange = frange * 0.004; if(nch<=16){ double fAng = (-fAzuAng - H_Beta[nch] - mfRollAng) * M_PI/180.0; double vtheta = V_theta[nch] * M_PI/180.0; double x = frange * cos(fAng)* cos(vtheta); double y = frange * sin(fAng)* cos(vtheta); double z = frange * sin(vtheta); pcl::PointXYZI point; point.x = x; point.y = y; point.z = z; point.intensity = intensity; mpoint_cloud_temp->points.push_back(point); ++mpoint_cloud_temp->width; } else { std::cout<<" channel error."<=360.0) { //share pointcloud mmutexpc.lock(); mpoint_cloud = mpoint_cloud_temp; mbpcupdate = true; mmutexpc.unlock(); mcv.notify_all(); pcl::PointCloud::Ptr point_cloud( new pcl::PointCloud()); point_cloud->header.frame_id = "velodyne"; point_cloud->height = 1; point_cloud->header.stamp = static_cast(std::chrono::system_clock::now().time_since_epoch().count()/1000); point_cloud->width = 0; point_cloud->header.seq =g_seq; g_seq++; mpoint_cloud_temp = point_cloud; azutotal = 0; } } } } } } int lidar_16z::GetPointCloud(pcl::PointCloud::Ptr & point_cloud) { return 1; }