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); } |
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 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:模拟&&非学习区警告