LiDAR-Velodyne激光雷达pcap文件解析

最新推荐文章于 2024-07-29 18:59:01 发布

指尖热度

最新推荐文章于 2024-07-29 18:59:01 发布

阅读量1.2w

点赞数 2

分类专栏： PCL编程文章标签： velodyne pcap 点云转换

本文链接：https://blog.csdn.net/sunboyiris/article/details/75172025

版权

这篇博客主要介绍了如何解析Velodyne LiDAR的pcap文件，并将其转化为XYZ格式的点云数据。作者分享了一段轻量级的代码实现，简化了这个过程。

摘要由CSDN通过智能技术生成

在使用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, dispatc