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)然后就看到了最终的结果

一 基本格式:  

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

二、文件头:
   

 


   文件头结构体

1
2
3
4
5
6
7
8
9
10
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格式的点云数据,写完之后发现其实代码也不多,更轻量级了,代码如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
// 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)\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" )) !=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\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);
}

 

posted @   Radiumlrb  阅读(4153)  评论(1编辑  收藏  举报
编辑推荐:
· DeepSeek 解答了困扰我五年的技术问题
· 为什么说在企业级应用开发中,后端往往是效率杀手?
· 用 C# 插值字符串处理器写一个 sscanf
· Java 中堆内存和栈内存上的数据分布和特点
· 开发中对象命名的一点思考
阅读排行:
· DeepSeek 解答了困扰我五年的技术问题。时代确实变了!
· PPT革命!DeepSeek+Kimi=N小时工作5分钟完成?
· What?废柴, 还在本地部署DeepSeek吗?Are you kidding?
· DeepSeek企业级部署实战指南:从服务器选型到Dify私有化落地
· 程序员转型AI:行业分析
历史上的今天:
2016-11-07 洛谷 P1038 神经网络 Label:拓扑排序 && 坑 60分待查
2016-11-07 洛谷 P1033 自由落体 Label:模拟&&非学习区警告
点击右上角即可分享
微信分享提示