velodyne：激光雷达pcap⽂件格式及写⼊、数据解析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格式的点云数据，写完之后发现其实代码也不多，更轻量级了，代码如下：
[cpp]
1. // readpcap.cpp : 定义控制台应⽤程序的⼊⼝点。
2. //
3. #define _CRT_SECURE_NO_DEPRECATE
4. #define _CRT_SECURE_NO_WARNINGS
5. #include "pcap.h"
6. #include "stdio.h"
7. #include "math.h"
8. #include<stdlib.h>
9. #include<conio.h>
10. #include "velodyneptk.h"
11. #define LINE_LEN 16
12.
13. int Azimuth_[12]; //原始值
14. float Azimuth_Value_[12];
15. int Distance_[12][32]; //原始值
16. float Distance_Value_[12][32];
17. int Atten_[12][32]; //原始值
18. Usefulmessage UsefulData;
19. int framecount;fprintf格式
20. int frameint;
21. //计算时间戳函数
22. float Timeoffsetvec[384];
23. float lasersinvec[384];
24. float lasercosvec[384];
25. void Timeoffsetfun()
26. {
27. for (int i = 0; i < 24; i++)
28. {
29. for (int j = 0; j < 16; j++)
30. {
31. Timeoffsetvec[i * 16 + j] = i*55.296 + j*2.304;
32. lasersinvec[i * 16 + j] = LASER_SIN[j];
33. lasercosvec[i * 16 + j] = LASER_COS[j];
34. }
35. }
36. }
36. }
37.
38. void dispatcher_handler(u_char *, const struct pcap_pkthdr *, const u_char *);
39.
40.
41. //byte转int Azimuth
42. int bytes2ToInt(byte* bytes)
43. {
44. int addr = bytes[0] & 0xFF;
45. addr |= (bytes[1]<<8 & 0xFF00);
46. return addr;
47. }
48.
49. int bytes1ToInt(byte* bytes)
50. {
51. int addr = bytes[0] & 0xFF;
52. return addr;
53. }
54.
55. //byte转int Azimuth
56. long int bytes4ToInt(byte* bytes)
57. {
58. long int addr = bytes[0] & 0xFF;
59. addr |= (bytes[1] << 8 & 0xFF00);
60. addr |= ((bytes[2] << 16) & 0xFF0000);
61. addr |= ((bytes[3] << 24) & 0xFF000000);
62. return addr;
63. }
64.
65. float stamptimecount = 0;
66. void UDPtoXYZfun(Usefulmessage data);
67. void UDPtoXYZfunALL(Usefulmessage data);
68. errno_t err;
69.
70. int _tmain(int argc, _TCHAR* argv[])
71. {
72. pcap_t *fp;
73. char errbuf[PCAP_ERRBUF_SIZE];
74. Usefulmessage UsefulData;
75. Timeoffsetfun();
76. framecount = 0;
77.
78. fp = pcap_open_offline("1.pcap",errbuf);
79. pcap_loop(fp, 0, dispatcher_handler, NULL);
80. pcap_close(fp);
81. return 0;
82. }
83.
84. void dispatcher_handler(u_char *temp1,
85. const struct pcap_pkthdr *header,
86. const u_char *pkt_data)
87. {
88. u_int it = 0;
89.
90. (VOID*)temp1;
91.
92. //保存数据
93. char fn[20];
94. PointXYZ point;
95. FILE *fp;
96.
97.
98. long int ustime = header->ts.tv_usec;
99. printf("%ld:%ld (%ld)\n", header->ts.tv_sec, header->ts.tv_usec, header->len); 100. if (header->len==1248)
101. {
102. byte timestampl[4];
103. byte factoryl[2];
104.
105. byte lo[1248];
106. memcpy(&lo, pkt_data, 1248);
107. memcpy(×tampl, pkt_data + 42 + 12 * 100, 4);
108. memcpy(&factoryl, pkt_data + 42 + 12 * 100 + 4, 2);
109. //float fValuet = *((float*)×tampl); //系统时间
110. int fValue1 = *((float*)&factoryl[0]);
111. long int fValue = bytes4ToInt(timestampl);
112. if (stamptimecount == 0)
113. {
114. stamptimecount = fValue;
115. }
116. if ((fValue - stamptimecount) >= 100000)
117. {
118. stamptimecount = fValue;
119. frameint++;
120. }
121. /保存数据
122. sprintf_s(fn, "%", frameint);
123. //err = fopen_s( &stream, "crt_fopen_s.c", "r" )) !=0
124. if ((fp = fopen(fn, "a")) == NULL)
125. {
126. printf("Create File failure 1");
127. fclose(fp);
128. //exit(1);
129. }
130.
131. //read data
132. byte datal[12][100];
133. int packet_size = 100;
134. int azimuth;
135. float distance;
136. int passway;
137. for (int i = 0; i < 12; i++)
138. {
139. memcpy(&datal[i], pkt_data + 42 + i * 100, packet_size); 140. BYTE b[2];
141. BYTE b1[1];
142. memcpy(&b, pkt_data + 42 + i * 100 + 2, 2);
143. azimuth = bytes2ToInt(b);
144. UsefulData.JIAODU_[i] = azimuth;
145. UsefulData.JIAODU2_[i] = azimuth*0.01;
146. UsefulData.Timesec = header->ts.tv_sec;
147. //printf("%f\n", UsefulData.JIAODU2_[i]);
148. UsefulData.TimeStamp = fValue;
149. for (int j = 0; j < 32; j++)
150. {
151. memcpy(&b, pkt_data + 42 + i * 100 + 4 + j * 3, 2); 152. memcpy(&b1, pkt_data + 42 + i * 100 + 4 + j * 3 + 2, 1); 153. distance = float(bytes2ToInt(b))*0.002f;
154.
155. passway = bytes1ToInt(b1);
156. if (distance<0.05)
157. {
158. UsefulData.JULI_[i][j] = 0;
159. UsefulData.PointPos[i][j] = i * 32 + j;
160. //printf("%d ", UsefulData.PointPos[i][j]);
161. }
162. else
163. {
164. UsefulData.JULI_[i][j] = distance;
165. UsefulData.PointPos[i][j] = i * 32 + j;
166. }
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论