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

2024-02-14 23:38
文章标签 解析 激光雷达 pcap lidar velodyne

本文主要是介绍LiDAR-Velodyne激光雷达pcap文件解析，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

       在使用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 16int 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)\n", 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" )) !=0if ((fp = fopen(fn, "a")) == NULL){printf("Create File failure 1");fclose(fp);//exit(1);}//read databyte 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\n", 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\n ", UsefulData.PASSEGEWAY_[i1][k], point.tus,  header->ts.tv_sec,ustime);}}}fclose(fp);}printf("\n\n");
}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 \n ", data.PASSEGEWAY_[i][j], point[i * 32 + j].tus, data.TimeStamp, data.Timesec);//fprintf(fp, "%f %f %f \n", 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\n", 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 \n", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);}}}fclose(fp);
}


这篇关于LiDAR-Velodyne激光雷达pcap文件解析的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/709895。

相关文章

网页解析 lxml 库--实战

网页解析 lxml 库--实战

lxml库使用流程 lxml 是 Python 的第三方解析库，完全使用 Python 语言编写，它对 XPath表达式提供了良好的支 持，因此能够了高效地解析 HTML/XML 文档。本节讲解如何通过 lxml 库解析 HTML 文档。 pip install lxml lxm| 库提供了一个 etree 模块，该模块专门用来解析 HTML/XML 文档，下面来介绍一下 lxml 库
阅读更多...
【C++】_list常用方法解析及模拟实现

【C++】_list常用方法解析及模拟实现

相信自己的力量，只要对自己始终保持信心，尽自己最大努力去完成任何事，就算事情最终结果是失败了，努力了也不留遗憾。💓💓💓 目录   ✨说在前面 🍋知识点一：什么是list？ •🌰1.list的定义 •🌰2.list的基本特性 •🌰3.常用接口介绍 🍋知识点二：list常用接口 •🌰1.默认成员函数 🔥构造函数（⭐） 🔥析构函数 •🌰2.list对象
阅读更多...
基于UE5和ROS2的激光雷达+深度RGBD相机小车的仿真指南(五)：Blender锥桶建模

基于UE5和ROS2的激光雷达+深度RGBD相机小车的仿真指南(五)：Blender锥桶建模

前言 本系列教程旨在使用UE5配置一个具备激光雷达+深度摄像机的仿真小车，并使用通过跨平台的方式进行ROS2和UE5仿真的通讯，达到小车自主导航的目的。本教程默认有ROS2导航及其gazebo仿真相关方面基础，Nav2相关的学习教程可以参考本人的其他博客Nav2代价地图实现和原理–Nav2源码解读之CostMap2D(上)-CSDN博客往期教程： 第一期：基于UE5和ROS2的激光雷达+深度RG
阅读更多...
OWASP十大安全漏洞解析

OWASP十大安全漏洞解析

OWASP（开放式Web应用程序安全项目）发布的“十大安全漏洞”列表是Web应用程序安全领域的权威指南，它总结了Web应用程序中最常见、最危险的安全隐患。以下是对OWASP十大安全漏洞的详细解析： 1. 注入漏洞（Injection） 描述：攻击者通过在应用程序的输入数据中插入恶意代码，从而控制应用程序的行为。常见的注入类型包括SQL注入、OS命令注入、LDAP注入等。 影响：可能导致数据泄
阅读更多...
从状态管理到性能优化：全面解析 Android Compose

从状态管理到性能优化：全面解析 Android Compose

文章目录 引言一、Android Compose基本概念1.1 什么是Android Compose?1.2 Compose的优势1.3 如何在项目中使用Compose 二、Compose中的状态管理2.1 状态管理的重要性2.2 Compose中的状态和数据流2.3 使用State和MutableState处理状态2.4 通过ViewModel进行状态管理 三、Compose中的列表和滚动
阅读更多...
Spring 源码解读：自定义实现Bean定义的注册与解析

Spring 源码解读：自定义实现Bean定义的注册与解析

引言 在Spring框架中，Bean的注册与解析是整个依赖注入流程的核心步骤。通过Bean定义，Spring容器知道如何创建、配置和管理每个Bean实例。本篇文章将通过实现一个简化版的Bean定义注册与解析机制，帮助你理解Spring框架背后的设计逻辑。我们还将对比Spring中的BeanDefinition和BeanDefinitionRegistry，以全面掌握Bean注册和解析的核心原理。
阅读更多...
CSP 2023 提高级第一轮 CSP-S 2023初试题 完善程序第二题解析 未完

CSP 2023 提高级第一轮 CSP-S 2023初试题 完善程序第二题解析 未完

一、题目阅读 （最大值之和）给定整数序列 a0,⋯,an−1，求该序列所有非空连续子序列的最大值之和。上述参数满足 1≤n≤105 和 1≤ai≤108。 一个序列的非空连续子序列可以用两个下标 ll 和 rr（其中0≤l≤r<n0≤l≤r<n）表示，对应的序列为 al,al+1,⋯,ar​。两个非空连续子序列不同，当且仅当下标不同。 例如，当原序列为 [1,2,1,2] 时，要计算子序列 [
阅读更多...
多线程解析报表

多线程解析报表

假如有这样一个需求，当我们需要解析一个Excel里多个sheet的数据时，可以考虑使用多线程，每个线程解析一个sheet里的数据，等到所有的sheet都解析完之后，程序需要提示解析完成。 Way1 join import java.time.LocalTime;public class Main {public static void main(String[] args) thro
阅读更多...
ZooKeeper 中的 Curator 框架解析

ZooKeeper 中的 Curator 框架解析

Apache ZooKeeper 是一个为分布式应用提供一致性服务的软件。它提供了诸如配置管理、分布式同步、组服务等功能。在使用 ZooKeeper 时，Curator 是一个非常流行的客户端库，它简化了 ZooKeeper 的使用，提供了高级的抽象和丰富的工具。本文将详细介绍 Curator 框架，包括它的设计哲学、核心组件以及如何使用 Curator 来简化 ZooKeeper 的操作。 1
阅读更多...
Unity3D自带Mouse Look鼠标视角代码解析。

Unity3D自带Mouse Look鼠标视角代码解析。

Unity3D自带Mouse Look鼠标视角代码解析。 代码块 代码块语法遵循标准markdown代码，例如： using UnityEngine;using System.Collections;/// MouseLook rotates the transform based on the mouse delta./// Minimum and Maximum values can
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2