Velodyne线性激光雷达pcap文件格式及写入、数据解析 Label:激光雷达

　转载自https://blog.csdn.net/qq_25241325/article/details/80766305

roslaunch loam_velodyne loam_velodyne.launch

rosbag record -o out /velodyne_points

rosbag play nsh_indoor_outdoor.bag

最后会记录一个名为out_(记录时间)的.bag包

然后将.bag包转化为 .pcd文件

rosrun pcl_ros bag_to_pcd  out_.bag(上面记录的那个包) /cloud  pcd

此时转化为一个名为cloud的pcd集，进入pcd那个文件夹

用pcl_viewer last.pcd（最后一帧pcd）然后就看到了最终的结果

一 基本格式：  

 文件头 数据包头 数据报数据包头数据报......

二、文件头：
   

   文件头结构体

 sturct pcap_file_header
 {
      DWORD           magic;
      DWORD           version_major;
      DWORD           version_minor;
      DWORD           thiszone;
      DWORD           sigfigs;
      DWORD           snaplen;
      DWORD           linktype;
 }

说明：

1、标识位：32位的，这个标识位的值是16进制的 0xa1b2c3d4。
a 32-bit magic number ,The magic number has the value hex a1b2c3d4.
2、主版本号：16位， 默认值为0x2。
a 16-bit major version number,The major version number should have the value 2.
3、副版本号：16位，默认值为0x04。
a 16-bit minor version number,The minor version number should have the value 4.
4、区域时间：32位，实际上该值并未使用，因此可以将该位设置为0。
a 32-bit time zone offset field that actually not used, so you can (and probably should) just make it 0;
5、精确时间戳：32位，实际上该值并未使用，因此可以将该值设置为0。
a 32-bit time stamp accuracy field tha not actually used,so you can (and probably should) just make it 0;
6、数据包最大长度：32位，该值设置所抓获的数据包的最大长度，如果所有数据包都要抓获，将该值设置为65535；例如：想获取数据包的前64字节，可将该值设置为64。
a 32-bit snapshot length" field;The snapshot length field should be the maximum number of bytes perpacket that will be captured. If the entire packet is captured, make it 65535; if you only capture, for example, the first 64 bytes of the packet, make it 64.
7、链路层类型：32位， 数据包的链路层包头决定了链路层的类型。
a 32-bit link layer type field.The link-layer type depends on the type of link-layer header that the
packets in the capture file have:

以下是数据值与链路层类型的对应表
0 BSD loopback devices, except for later OpenBSD
1 Ethernet, and Linux loopback devices 以太网类型，大多数的数据包为这种类型。
6 802.5 Token Ring
7 ARCnet
8 SLIP
9 PPP
10

在使用velodyne的时候，PCAP数据解析比较麻烦，为此写了一点代码来专门解析PCAP文件，将PCAP格式数据转为XYZ格式的点云数据，写完之后发现其实代码也不多，更轻量级了，代码如下：

// readpcap.cpp : 定义控制台应用程序的入口点。
//
#define _CRT_SECURE_NO_DEPRECATE
#define _CRT_SECURE_NO_WARNINGS
#include "pcap.h"
#include "stdio.h"
#include "math.h"
#include<stdlib.h>
#include<conio.h>
#include "velodyneptk.h"
#define LINE_LEN 16
 
int Azimuth_[12];				//原始值
float Azimuth_Value_[12];
int Distance_[12][32];			//原始值
float Distance_Value_[12][32];
int Atten_[12][32];		//原始值
Usefulmessage UsefulData;
int framecount;
int frameint;
//计算时间戳函数
float Timeoffsetvec[384];
float lasersinvec[384];
float lasercosvec[384];
void Timeoffsetfun()
{
	for (int i = 0; i < 24; i++)
	{
		for (int j = 0; j < 16; j++)
		{
			Timeoffsetvec[i * 16 + j] = i*55.296 + j*2.304;
			lasersinvec[i * 16 + j] = LASER_SIN[j];
			lasercosvec[i * 16 + j] = LASER_COS[j];
		}
	}
}
 
void dispatcher_handler(u_char *, const struct pcap_pkthdr *, const u_char *);
 
 
//byte转int  Azimuth
int bytes2ToInt(byte* bytes)
{
	int addr = bytes[0] & 0xFF;
	addr |= (bytes[1]<<8 & 0xFF00);
	return addr;
}
 
int bytes1ToInt(byte* bytes)
{
	int addr = bytes[0] & 0xFF;
	return addr;
}
 
//byte转int  Azimuth
long int bytes4ToInt(byte* bytes)
{
	long int addr = bytes[0] & 0xFF;
	addr |= (bytes[1] << 8 & 0xFF00);
	addr |= ((bytes[2] << 16) & 0xFF0000);
	addr |= ((bytes[3] << 24) & 0xFF000000);
	return addr;
}
 
float stamptimecount = 0;
void UDPtoXYZfun(Usefulmessage data);
void UDPtoXYZfunALL(Usefulmessage data);
errno_t err;
 
int _tmain(int argc, _TCHAR* argv[])
{
	pcap_t *fp;
	char errbuf[PCAP_ERRBUF_SIZE];
	Usefulmessage UsefulData;
	Timeoffsetfun();
	framecount = 0;
 
	fp = pcap_open_offline("1.pcap",errbuf);
	pcap_loop(fp, 0, dispatcher_handler, NULL);
	pcap_close(fp);
	return 0;
}
 
void dispatcher_handler(u_char *temp1,
	const struct pcap_pkthdr *header,
	const u_char *pkt_data)
{
	u_int it = 0;
 
	(VOID*)temp1;
 
	//保存数据
	char fn[20];
	PointXYZ point;
	FILE *fp;
 
 
	long int ustime = header->ts.tv_usec;
	printf("%ld:%ld (%ld)
", header->ts.tv_sec, header->ts.tv_usec, header->len);
	if (header->len==1248)
	{
		byte timestampl[4];
		byte factoryl[2];
 
		byte lo[1248];
		memcpy(&lo, pkt_data, 1248);
		memcpy(×tampl, pkt_data + 42 + 12 * 100, 4);
		memcpy(&factoryl, pkt_data + 42 + 12 * 100 + 4, 2);
		//float fValuet = *((float*)×tampl); //系统时间
		int fValue1 = *((float*)&factoryl[0]);
		long int fValue = bytes4ToInt(timestampl);
		if (stamptimecount == 0)
		{
			stamptimecount = fValue;
		}
		if ((fValue - stamptimecount) >= 100000)
		{
			stamptimecount = fValue;
			frameint++;
		}
		/////保存数据
		sprintf_s(fn, "%05d.txt", frameint);
		//err  = fopen_s( &stream, "crt_fopen_s.c", "r" )) !=0
		if ((fp = fopen(fn, "a")) == NULL)
		{
			printf("Create File failure 1");
			fclose(fp);
			//exit(1);
		}
 
		//read data
		byte datal[12][100];
		int packet_size = 100;
		int azimuth;
		float distance;
		int passway;
		for (int i = 0; i < 12; i++)
		{
			memcpy(&datal[i], pkt_data + 42 + i * 100, packet_size);
			BYTE b[2];
			BYTE b1[1];
			memcpy(&b, pkt_data + 42 + i * 100 + 2, 2);
			azimuth = bytes2ToInt(b);
			UsefulData.JIAODU_[i] = azimuth;
			UsefulData.JIAODU2_[i] = azimuth*0.01;
			UsefulData.Timesec = header->ts.tv_sec;
			//printf("%f
", UsefulData.JIAODU2_[i]);
			UsefulData.TimeStamp = fValue;
			for (int j = 0; j < 32; j++)
			{
				memcpy(&b, pkt_data + 42 + i * 100 + 4 + j * 3, 2);
				memcpy(&b1, pkt_data + 42 + i * 100 + 4 + j * 3 + 2, 1);
				distance = float(bytes2ToInt(b))*0.002f;
 
				passway = bytes1ToInt(b1);
				if (distance<0.05)
				{
					UsefulData.JULI_[i][j] = 0;
					UsefulData.PointPos[i][j] = i * 32 + j;
					//printf("%d  ", UsefulData.PointPos[i][j]);
				}
				else
				{
					UsefulData.JULI_[i][j] = distance;
					UsefulData.PointPos[i][j] = i * 32 + j;
				}
 
				UsefulData.PASSEGEWAY_[i][j] = passway;
 
				UsefulData.TimeOffset[i][j] = Timeoffsetvec[i * 32 + j] + header->ts.tv_usec;  //时间戳
				UsefulData.SIN_[i][j] = lasersinvec[i * 32 + j];
				UsefulData.COS_[i][j] = lasercosvec[i * 32 + j];
				//	printf("%f ", UsefulData.PASSEGEWAY_[i][j]);
 
 
			}
 
		}
		//经度赋值
		for (int i1 = 0; i1 < 12; i1++)
		{
			for (int k = 0; k < 32; k++)
			{
				if (k < 16)
				{
					UsefulData.Azimuth[i1][k] = UsefulData.JIAODU2_[i1];// +LASER_vert_correction[k];
				}
				else if (k >= 16)
				{
					if (i1 < 11)//前11帧
					{
						if (UsefulData.JIAODU2_[i1 + 1] < UsefulData.JIAODU2_[i1])
						{
							UsefulData.JIAODU2_[i1 + 1] += 360.0;
							float azimuth2 = UsefulData.JIAODU2_[i1] + (UsefulData.JIAODU2_[i1 + 1] - UsefulData.JIAODU2_[i1]) / 2.0;
							if (azimuth2 > 360)// 角度变化了
							{
								azimuth2 -= 360;
								UsefulData.Azimuth[i1][k] = azimuth2;// +LASER_vert_correction[k - 16];
							}
							else
							{
								UsefulData.Azimuth[i1][k] = azimuth2;// +LASER_vert_correction[k - 16];
							}
						}
						else
						{
							float azimuth4 = UsefulData.JIAODU2_[i1] + (UsefulData.JIAODU2_[i1 + 1] - UsefulData.JIAODU2_[i1]) / 2.0;
							UsefulData.Azimuth[i1][k] = azimuth4;// +LASER_vert_correction[k - 16
						}
					}
					else if (i1 == 11)//最后一帧
					{
						float azimuth3 = UsefulData.JIAODU2_[i1] + 0.2;
						if (azimuth3 > 360)
						{
							azimuth3 -= 360;
						}
						else
						{
						}
						UsefulData.Azimuth[i1][k] = azimuth3;//;+LASER_vert_correction[k - 16];
					}
				}
				point.x = UsefulData.JULI_[i1][k] * UsefulData.COS_[i1][k] * sin(UsefulData.Azimuth[i1][k] / 180.0*3.1415926);
				point.y = UsefulData.JULI_[i1][k] * UsefulData.COS_[i1][k] * cos(UsefulData.Azimuth[i1][k] / 180.0*3.1415926);
				point.z = UsefulData.JULI_[i1][k] * UsefulData.SIN_[i1][k];
				point.r = UsefulData.PASSEGEWAY_[i1][k];
				point.tus = UsefulData.TimeOffset[i1][k];
				point.tsec = UsefulData.Timesec;
				if ((point.x == 0) && (point.y == 0) && (point.z == 0) || (UsefulData.JULI_[i1][k] <= 0.05))
				{
				}
				else
				{
					//X  Y  Z  Azimuth Distance Laser_ID 
					fprintf(fp, "%f %f %f %f %f %d ", point.x, point.y, point.z, UsefulData.Azimuth[i1][k], UsefulData.JULI_[i1][k], UsefulData.PointPos[i1][k] % 16);
					fprintf(fp, "%f %f %ld %ld
 ", UsefulData.PASSEGEWAY_[i1][k], point.tus,  header->ts.tv_sec,ustime);
				}
			}
 
		}
		fclose(fp);
		
	}
 
	printf("

");
}
 
void UDPtoXYZfunALL(Usefulmessage data)
{
	PointXYZ point[384];
	FILE *fp;
	if ((fp = fopen("all.txt", "a")) == NULL)
	{
		printf("Create File failure");
		//getch();
		exit(1);
	}
	for (int i = 0; i < 12; i++)
	{
		for (int j = 0; j < 32; j++)
		{
			point[i * 32 + j].x = data.JULI_[i][j] * data.COS_[i][j] * sin(data.Azimuth[i][j] / 180.0*3.1415926);
			point[i * 32 + j].y = data.JULI_[i][j] * data.COS_[i][j] * cos(data.Azimuth[i][j] / 180.0*3.1415926);
			point[i * 32 + j].z = data.JULI_[i][j] * data.SIN_[i][j];
			point[i * 32 + j].r = data.PASSEGEWAY_[i][j];
			point[i * 32 + j].tus = data.TimeOffset[i][j];
			point[i * 32 + j].tsec = data.Timesec;
			if ((point[i * 32 + j].x == 0) && (point[i * 32 + j].y == 0) && (point[i * 32 + j].z == 0) || (data.JULI_[i][j] <= 0.05))
			{
			}
			else
			{
				//X  Y  Z  Azimuth Distance Laser_ID 
				fprintf(fp, "%f %f %f %f %f %d ", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z, data.Azimuth[i][j], data.JULI_[i][j], data.PointPos[i][j] % 16);
				fprintf(fp, "%f %f %f %f 
 ", data.PASSEGEWAY_[i][j], point[i * 32 + j].tus, data.TimeStamp, data.Timesec);
 
				//fprintf(fp, "%f %f %f 
", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);
 
			}
		}
	}
	fclose(fp);
}
 
 
void UDPtoXYZfun(Usefulmessage data)
{
	PointXYZ point[384];
	FILE *fp;
	if ((fp = fopen("my.txt", "a")) == NULL)
	{
		printf("Create File failure");
		//getch();
		exit(1);
	}	
	for (int i = 0; i < 12; i++)
	{
		for (int j = 0; j < 32; j++)
		{
			point[i * 32 + j].x = data.JULI_[i][j]*data.COS_[i][j]*sin(data.Azimuth[i][j]/180.0*3.1415926);
			point[i * 32 + j].y = data.JULI_[i][j]*data.COS_[i][j]*cos(data.Azimuth[i][j]/180.0*3.1415926);
			point[i * 32 + j].z = data.JULI_[i][j] * data.SIN_[i][j];
			point[i * 32 + j].r = data.PASSEGEWAY_[i][j];
			point[i * 32 + j].tus = data.TimeOffset[i][j];
			point[i * 32 + j].tsec = data.Timesec;
			if ((point[i * 32 + j].x == 0) && (point[i * 32 + j].y == 0) && (point[i * 32 + j].z == 0)||(data.JULI_[i][j]<=0.05))
			{
			}
			else
			{
 
				//fprintf(fp, "%f %f %f %f %f %d
", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z,data.Azimuth[i][j],data.JULI_[i][j],data.PointPos[i][j]%16);
				fprintf(fp, "%f %f %f 
", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);
 
			}
		}
	}
	fclose(fp);
}