日常工作系列之：解析雷达

任务思考：

总的流程：一边接收udp包，一边处理数据并发送出去
使用ROS框架
明确数据结构：RangeView，纵轴为雷达线数，横轴为转360度的点数
解决问题的逻辑：
（1）总的逻辑：生存者，消费者，一个接收包，一个处理包，所以需要加锁。
（2）线程一：读取UDP的包，把接收到的包都存在一个 data[BUFLEN]; 中，接收到的数据总长度为 data_len，data_len是包长的整数倍。
（3）线程二：将data数据保存到 decode数组中，记录下长度decode_len，清空data，准备处理数据。

伪代码：

总的类似于：生存者，消费者，一个接收包，一个处理包，所以需要加锁。

1. 线程一：读取UDP的包，把接收到的包都存在一个unsigned char data[BUFLEN]; 中，总的长度为 data_len，data_len是包长的整数倍。

2. 线程二：将data数据保存到 decode数组中，记录下长度decode_len，清空data数组，处理数据。

if (**处理并存储完一帧RangeView**)
        {
            数据转存为sensor::PointCloud
            发布出去。
        }

处理数据的伪代码

循环decode数组中每一帧udp：
	每一帧的数据解析出来，（距离d，水平角h，反射率）
	将数据转换为我们想要的数据，（x,y,z,r,d）
	数据存储到RangeView数据结构中，如果填满一帧，就处理并存储完了。

细节伪代码：

每一帧udp数据如何解析：
	根据说明书：
	一帧大小1248，分为（42 + 1200 + 6），数据部分在1200部分
	1200 = 12 * 100           :(12个data block)
	100 = （2 + 2） + 32*（2+1):(标志位 + 角度 + 32*（距离+反射率） )
	所以：

数据转换：
d = 高两位 << 8 | 底两位；
d = d*分辨率；

azimuth100 = linedata[2] << 8 | linedata[3];
azimuth100 = azimuth100 / (double)100;

x = d cos(w)sin(h)
y = d cos(w)cos(h)
z = d sin(w)



for (size_t frames =0; frames < frames_len; frames += PACKTLEN)
{
	unsigned char* data_frames = (unsigned char*)&decode[frames+42]; 
	// data_frames : 1200长度数组
	for (size_t i = 0; i < 12; ++i)
	{
		// data_lines : 100长度数组
		unsigned char* data_lines = &data_frames[i * 100];  // 100 = 2 + 2 + 32*3
		int azimuth100 = data_lines[2] << 8 | data_lines[3] ;
		double azimuth = azimuth100 / (double)100;
		//获得水平角，判断当前是否已经一圈  TODO		
		if (pre_azimuth > 350 && azimuth < 10 )// 说明转了一圈了
		{
			get_whole_circle = true;
			circlelen = circlePt;  // 用来记录这一圈有多少circlelen，因为不是每次都可以有1800列的。
			circlePt = 0;
		}
		pre_azimuth = azimuth;

		// lidar_data : 96 = 32*3
		unsigned char *lidar_data = &data_lines[4]; //line_data  = 32 * 3

		for (int line = 0; line < 32; ++line)
		{
			int distance =   lidar_data[0 + line*3] << 8 | lidar_data[1 + line*3)]	;
			distance = distance * 0.25 * 0.01;
			
			if (distance > 0)
			{    // 求出 x,y,z
			 		pt->x = distance * 角度
			 		pt->y = distance * 角度
			 		pt->z = distance * 角度
			 		pt->r = lidar_data[2 + line*3];
			 		pt->d = distance;
					
				  //坐标转换 TODO  坐标转换(pt);
			}
			else 
			{
				 pt->x = 0,pt->y = 0,pt->z = 0;
			}
       if (line == 0)
       {	
       	 circlePt++; // rangeView 为长方形，横轴为激光水平移动，纵轴为不同线数雷达
       } 
		}
}

判断什么时候存储完一帧rangview

if (pre_azimuth > 350 && azimuth <10) 
	转了一圈了
	

Tip:

申请了一个 LINE * CIRCLELEN 大小的数组， 
pointX* pt = new pointX [LINE*CIRCLELEN];

用另一个二维数组去表示这块区域
pointX **rangeview = new pointX *[LINE];
for (int l = 0; l < LINE; l++)
{
	rangeview[l] = &pt[l * CIRCLELEN]; 
} 

最后在存储值的时候，用一个point指针来控制
pointX *p = &rangeview[line]][circlePt]; 
//来存储一个个点云数据，方便之后坐标转换