镭神32线激光雷达pcap数据离线解析 支持A C型 矫正激光阵列有序化点云

#include "liadr32Decoder.h"
#define LIDAR_VERSION 2				//



static LidarDatas LidarDataFrist;	//fisrt echo
static LidarDatas LidarDataSecond;	//second echo

static std::ofstream outputFile;	//output the parsed data
static uint64_t GPStimestamp = 0;	//GPS timestamp
static pcap_t *pfile;

double Angle1 = 0.0, Angle2 = 0.0;	//水平补偿角度
double device_type = 0.0;
int frame_num = 1;


double theta_3[32] = { -16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4, -3, -2, -1,
	0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 };

//double theta_2[32] = { -16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, -8,
//						8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15 };
 
 
double theta_2[32] = { -16, -15, -14, -13, -12, -11, -10, -9, -8, -7, -6, -5, -4, -3, -2, -1,
	0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 };  // for a 

double theta_C[32] = { -18, -15, -12, -10, -8, -7, -6, -5, -4, -3.33, -2.66,-3, -2.33, -2, -1.33, -1.66, -1, -0.66,
 	0,-0.33, 0.33, 0.66, 1.33, 1, 1.66, 2, 3, 4, 6, 8, 11, 14}; // for c  angval 重写theta_2

static int exchange[32] = { 0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30,1, 3, 5, 7, 9, 11, 13, 15, 17, 19, 21, 23, 25, 27, 29, 31 };
double vertialAngles[32] = {};
double const PI = 3.1415926;
double cosTheta[32] = { 0 };
double sinTheta[32] = { 0 };

static const float DistanePrecision = 2.5;  //mm
 
std::vector<Cld_point> temp_p_all;
int col_num = 0;
std::vector<Cld_point> channel; // 32
std::vector<std::vector<Cld_point>> thread;
std::vector<std::vector<Cld_point>> thread_rebuild;
double angle_bias;
double angle_resolution = 0.18;
int thread_bias = 0;
void HowToUse()
{
	createSaveFile();
	resetDataBuff();
	openPcapFile("E:/code/LSC32解析/test.pcap");
	//C:/Users/leishen/Desktop/iScan-Pcd-1 - 副本.pcap
	readPcapFile();   // lidarDataDecoder
	resetDataBuff();
}

void createSaveFile()
{
	time_t nowtime;
	struct tm *p;
	time(&nowtime);
	p = localtime(&nowtime);
	char filename[50] = "";
	sprintf(filename, "./output/%06d-%02d%02d%02d-%02d%02d.txt", frame_num-1 ,p->tm_year + 1900 , p->tm_mon + 1, p->tm_mday, p->tm_hour, p->tm_min);
	const char *file = filename;
	outputFile.close();
	outputFile.open(file, std::ios::app);
}

void createSaveFile_frame(int frame)
{
	time_t nowtime;
	struct tm* p;
	time(&nowtime);
	p = localtime(&nowtime);
	char filename[60] = "";
	sprintf(filename, "./output/%06d-%02d%02d%02d-%02d%02d.txt", frame, p->tm_year + 1900, p->tm_mon + 1, p->tm_mday, p->tm_hour, p->tm_min);
	const char* file = filename;
	outputFile.close();
	outputFile.open(file, std::ios::app);
}

void openPcapFile(const char *fileName)
{
	char errbuf[100];
	pfile = pcap_open_offline(fileName, errbuf);
	if (NULL == pfile) {
		std::cout << errbuf;
	}
}

void readPcapFile()  // lidarDataDecoder
{
	pcap_pkthdr *pkthdr = 0;
	const u_char *pktdata = 0;
	bool isFindDevPacket = false;
	unsigned char databuf[1206];
	while (pcap_next_ex(pfile, &pkthdr, &pktdata) > 0) 
	{
		if (pkthdr->caplen == 1248 && (pktdata[42] == 0xa5 && pktdata[43] == 0xff && pktdata[44] == 0x00 && pktdata[45] == 0x5a)) 
		{
			pktdata = pktdata + 42;
			
			int	timeStampsFlag = 0;
			if (LIDAR_VERSION == 2)
				timeStampsFlag = 36;
			else
				timeStampsFlag = 52;
			struct tm cur_time;
			cur_time.tm_year = pktdata[timeStampsFlag++] + 2000 - 1900;
			cur_time.tm_mon = pktdata[timeStampsFlag++] - 1;
			cur_time.tm_mday = pktdata[timeStampsFlag++];
			cur_time.tm_hour = pktdata[timeStampsFlag++]/* + 8*/;
			cur_time.tm_min = pktdata[timeStampsFlag++];
			cur_time.tm_sec = pktdata[timeStampsFlag++];
			GPStimestamp = static_cast<uint64_t>(mktime(&cur_time));

			static bool flag = true;
			if (flag) {
				flag = false;

				if (LIDAR_VERSION == 2)
				{
					Angle1 = (pktdata[34] * 256 + pktdata[35]) / 100.f;
					Angle2 = (pktdata[42] * 256 + pktdata[43]) / 100.f;
					 
					std::cout << "A1:" <<Angle1 << std::endl;
					std::cout << "A2:" <<Angle2 << std::endl;
					 
					//device_type = pktdata[0:8];
					//垂直角度
					if (Angle2 != 0)
					{
						for (int i = 0; i < 32; i++)
						{
							uint8_t data1 = pktdata[882 + 2 * i];
							uint8_t data2 = pktdata[882 + 2 * i + 1];
							int angleVal = data1 * 256 + data2;
							if (angleVal > 32767) {
								angleVal = angleVal - 65535;
							}
							theta_2[i] = (double)angleVal / 100.0;
						}
					}
					
					for (int i = 0; i < 32; i++)
					{
						cosTheta[i] = cos(theta_2[i] * PI / 180);
						//std::cout << theta_2[i] << std::endl;
						sinTheta[i] = sin(theta_2[i] * PI / 180);
						//std::cout << sinTheta[i] << std::endl;
					}
				}
				else
				{
					Angle1 = (pktdata[186] * 256 + pktdata[187]) / 100.f;
					Angle2 = (pktdata[190] * 256 + pktdata[191]) / 100.f;
					

					//垂直角度
					for (int i = 0; i < 32; i++) {
						uint8_t data1 = pktdata[245 + 2 * i];
						uint8_t data2 = pktdata[245 + 2 * i + 1];
						int angleVal = data1 * 256 + data2;
						if (angleVal > 32767) {
							angleVal = angleVal - 65535;
						}
						//将角度的旧排序转换为新排序
						vertialAngles[exchange[i]] = (double)angleVal / 100.0;
					}
					//float theta1[32] = { -16, 0, -15, 1, -14, 2, -13, 3, -12, 4, -11, 5, -10, 6, -9, 7, -8, 8, -7, 9, -6, 10, -5, 11, -4, 12, -3, 13, -2, 14, -1, 15 };
					for (int i = 0; i < 32; i++)
					{
						theta_3[i] = vertialAngles[i];
						cosTheta[i] = cos(theta_3[i] * PI / 180);
						sinTheta[i] = sin(theta_3[i] * PI / 180);
					}
				}

			}
			isFindDevPacket = true;
		}
		//data
		if (isFindDevPacket && pkthdr->caplen == 1248 && (pktdata[42] == 0xff && pktdata[43] == 0xee)) {
			pktdata = pktdata + 42;
			memcpy(databuf, pktdata, 1206);
			lidarDataDecoder(databuf);
		}
	}
	pcap_close(pfile);
}

void lidarDataDecoder(const unsigned char *data)
{
	uint64_t mtimestamps = 16777216 * data[1203] + 65536 * data[1202] + 256 * data[1201] + data[1200] + (GPStimestamp) * 10e5;
	for (int i = 0; i < 12; i++) {
		if ((data[3 + 100 * i] * 256 + data[2 + 100 * i]) / 100.f >= 360.0) {
			LidarDataFrist.angle.push_back(0);
		}
		else {
			LidarDataFrist.angle.push_back((data[3 + 100 * i] * 256 + data[2 + 100 * i]) / 100.f);   //提取出一圈中的方位角度值
		}
		for (int j = 0; j < 32; j++) {
			LidarDataFrist.distance[j].push_back((data[5 + 3 * j + 100 * i] * 256 + data[4 + 3 * j + 100 * i]) / DistanePrecision);
			LidarDataFrist.intensity[j].push_back(data[6 + 3 * j + 100 * i]);
			LidarDataFrist.mtimestamp[j].push_back(mtimestamps + (32 * i + j) * 1.536 - 588.288);
		}
		if (LidarDataFrist.angle.size() >= 2) 
		{
			for (int j = 0; j < 32; j++) {
				float mdiff = (LidarDataFrist.angle.back() - LidarDataFrist.angle[LidarDataFrist.angle.size() - 2]);
				mdiff = mdiff >= 0 ? mdiff : mdiff + 360;
				if ((LidarDataFrist.angle[LidarDataFrist.angle.size() - 2] + (mdiff / 32) * j) < 360.0)
					LidarDataFrist.azimuth[j].push_back(LidarDataFrist.angle[LidarDataFrist.angle.size() - 2] + (mdiff / 32) * j);
				else
					LidarDataFrist.azimuth[j].push_back(LidarDataFrist.angle[LidarDataFrist.angle.size() - 2] + (mdiff / 32) * j - 360.0);
			}
		}
		//a circle
		if (LidarDataFrist.angle.size() >= 2 && abs(LidarDataFrist.angle.back() - LidarDataFrist.angle[LidarDataFrist.angle.size() - 2]) > 180) {
			float mdiff = (LidarDataFrist.angle.back() - LidarDataFrist.angle[LidarDataFrist.angle.size() - 2]);
			mdiff = mdiff >= 0 ? mdiff : mdiff + 360;
			for (size_t j = 0; j < 32; j++)
			{
				if ((LidarDataFrist.angle.back() + (mdiff / 32) * j) < 360)
					LidarDataFrist.azimuth[j].push_back(LidarDataFrist.angle[LidarDataFrist.angle.size() - 2] + (mdiff / 32) * j);
				else
					LidarDataFrist.azimuth[j].push_back(LidarDataFrist.angle[LidarDataFrist.angle.size() - 2] + (mdiff / 32) * j - 360);
			}


			//print first echo coordinate
			coordinateParsing(LidarDataFrist);

			//clear buffer
			resetDataBuff();
		}
	}
}
void coordinateParsing(LidarDatas LidarData)
{
	for (int i = 0; i < 32; i++)
	{
		for (int j = 0; j < (int)LidarData.azimuth[i].size(); j++)
		{
			double x, y, z;
			if (LIDAR_VERSION == 2)
			{
				if (i % 2 == 0) {
					x = (LidarData.distance[i][j] * cos((LidarData.azimuth[i][j] + Angle1) * PI / 180) * cosTheta[i] +
						4.94 * cos((LidarData.azimuth[i][j] + Angle2 - 12.98) * PI / 180)) / 100.f;
					y = -(LidarData.distance[i][j] * sin((LidarData.azimuth[i][j] + Angle1) * PI / 180) * cosTheta[i] +
						4.94 * sin((LidarData.azimuth[i][j] + Angle2 - 12.98) * PI / 180)) / 100.f;
					z = LidarData.distance[i][j] * sinTheta[i] / 100.f;

				}
				else {
					x = (LidarData.distance[i][j] * cos((LidarData.azimuth[i][j] - Angle1) * PI / 180) * cosTheta[i] +
						4.94 * cos((LidarData.azimuth[i][j] + Angle1 - 12.98) * PI / 180)) / 100.f;
					y = -(LidarData.distance[i][j] * sin((LidarData.azimuth[i][j] - Angle1) * PI / 180) * cosTheta[i] +
						4.94 * sin((LidarData.azimuth[i][j] + Angle1 - 12.98) * PI / 180)) / 100.f;
					z = LidarData.distance[i][j] * sinTheta[i] / 100.f;
					//std::cout << sinTheta[i] << std::endl;

				}
			}

			else
			{
				if (i % 4 <= 1) {
					x = (LidarData.distance[i][j] * cos((LidarData.azimuth[i][j] + Angle2) * PI / 180) * cosTheta[i] +
						4.94 * cos((LidarData.azimuth[i][j] + Angle2 - 12.98) * PI / 180)) / 100.f;
					y = -(LidarData.distance[i][j] * sin((LidarData.azimuth[i][j] + Angle2) * PI / 180) * cosTheta[i] +
						4.94 * sin((LidarData.azimuth[i][j] + Angle2 - 12.98) * PI / 180)) / 100.f;
					z = LidarData.distance[i][j] * sinTheta[i] / 100.f;
				}
				else {
					x = (LidarData.distance[i][j] * cos((LidarData.azimuth[i][j] + Angle1) * PI / 180) * cosTheta[i] +
						4.94 * cos((LidarData.azimuth[i][j] + Angle1 - 12.98) * PI / 180)) / 100.f;
					y = -(LidarData.distance[i][j] * sin((LidarData.azimuth[i][j] + Angle1) * PI / 180) * cosTheta[i] +
						4.94 * sin((LidarData.azimuth[i][j] + Angle1 - 12.98) * PI / 180)) / 100.f;
					z = LidarData.distance[i][j] * sinTheta[i] / 100.f;
				}
			}
			Cld_point temp;
			temp.x = x; temp.y = y; temp.z = z;
			temp.azimuth = LidarData.azimuth[i][j];
			temp.intensity = LidarData.intensity[i][j];
			temp.mtimestamp = LidarData.mtimestamp[i][j];
			temp_p_all.push_back(temp);

			//outputFile << x << " " << y << " " << z << " "
			//	<< LidarData.intensity[i][j] << " "
			//	<< LidarData.azimuth[i][j] << " "
			//	//<< LidarData.mtimestamp[i][j] <<" "
			//	<< std::endl;
		}
	}
	std::cout << "1" << std::endl;

	col_num = temp_p_all.size() / 32;

	for (int i = 0; i < col_num; i++)
	{
		channel.clear();
		for (int j = 0; j < 32; j++)
			channel.push_back(temp_p_all[j * col_num + i]);

		thread.push_back(channel);
	}
	if (Angle2 != 0) //c
	{
		angle_bias = Angle1 + Angle2;
		angle_bias = angle_bias / 2.0;
		 
	}
	else
	{
		angle_bias = 2.0 * Angle1;
		
	}
	thread_bias = ceil(angle_bias / angle_resolution);

	//thread_bias = ceil(angle_bias / angle_resolution);  
	//std::cout << "thread_bias" << std::endl;
	//std::cout << thread_bias << std::endl;
	 
	for (int i = 0; i < thread.size(); i++)
	{
		channel.clear();
		
		for (int j = 0; j < 32; j++)
		{
			if (j % 2 == 1) // 0 2 4为 右侧  1 3 5 为左laser
			{
				int temp_t = rtn_amend_thread_num(i, thread.size(), thread_bias);
				//std::cout << "i:" << i << std::endl;
				//std::cout << "temp_t:" << temp_t << std::endl;
				//std::cout << temp_t << std::endl;
				//std::cout << j << std::endl;
				int b = j - 1;
				channel.push_back(thread[temp_t][b]);
				channel.push_back(thread[i][j]);
				if (Angle2 != 0 && (j == 11 || j == 15 || j == 19 || j == 23)) // type c A1order
				{
					Cld_point temp1, temp2;
					temp1 = channel[channel.size() - 1];
					temp2 = channel[channel.size() - 2];
					channel.pop_back();
					channel.pop_back();
					channel.push_back(temp1);
					channel.push_back(temp2);

				}
				

			}
		}
		//std::cout << channel.size() << std::endl;
		quickSort(channel, 0, channel.size()-1);
		thread_rebuild.push_back(channel);
	}
	 
	for (int i = 0; i < thread_rebuild.size(); i++)
	{
		channel = thread_rebuild[i];
		for (int j = 0; j < 32; j++)
		{
			outputFile << channel[j].x << " " << channel[j].y << " " << channel[j].z << " "
				<< channel[j].intensity << " "
				//<< channel[j].azimuth << " "
				//<< channel[j].mtimestamp << " "
				<< channel.distance[i][j]
				<< std::endl;
		}

	}

	createSaveFile_frame(frame_num++);
	temp_p_all.clear();
	temp_p_all.shrink_to_fit();
	thread.clear();
	thread.shrink_to_fit();
	thread_rebuild.clear();
	thread_rebuild.shrink_to_fit();
}

int rtn_amend_thread_num(int t_in, int t_all, int t_bias) // for left laserseneor
{
	int t_amend = 0;
	t_amend = t_in - t_bias;
	if (t_amend < 0)
		t_amend = t_all-1 + t_amend;
	return t_amend;
}

 
void quickSort(std::vector<Cld_point> &array, int start, int end)
{
	if (start >= end) return;
	Cld_point temp = array[start];
	int L = start;          //L,R左右哨兵
	int R = end;

	while (L < R) {
		while (L < R && array[R].z >= temp.z)             
			R--;
		if (L < R)                                  
			array[L++] = array[R];
		while (L < R && array[L].z <= temp.z)
			L++;
		if (L < R)
			array[R--] = array[L];
	}
	array[L] = temp;                             
	quickSort(array, start, L - 1);
	quickSort(array, L + 1, end);
	
}

void resetDataBuff()
{
	LidarDataFrist.angle.clear();
	LidarDataFrist.azimuth.clear();
	LidarDataFrist.azimuth.resize(32);
	LidarDataFrist.distance.clear();
	LidarDataFrist.distance.resize(32);
	LidarDataFrist.intensity.clear();
	LidarDataFrist.intensity.resize(32);
	LidarDataFrist.mtimestamp.clear();
	LidarDataFrist.mtimestamp.resize(32);

	LidarDataSecond.angle.clear();
	LidarDataSecond.azimuth.clear();
	LidarDataSecond.azimuth.resize(32);
	LidarDataSecond.distance.clear();
	LidarDataSecond.distance.resize(32);
	LidarDataSecond.intensity.clear();
	LidarDataSecond.intensity.resize(32);
	LidarDataSecond.mtimestamp.clear();
	LidarDataSecond.mtimestamp.resize(32);


}