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

一 基本格式:
   文件头 数据包头 数据报数据包头数据报......
二、文件头:
    velodyne:激光雷达pcap文件格式及写入、数据解析_第1张图片
   文件头结构体
 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] view plain copy
  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  
  9. #include  
  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;  
  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. }  
  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, "%05d.txt", 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.                 }  
  167.   
  168.                 UsefulData.PASSEGEWAY_[i][j] = passway;  
  169.   
  170.                 UsefulData.TimeOffset[i][j] = Timeoffsetvec[i * 32 + j] + header->ts.tv_usec;  //时间戳  
  171.                 UsefulData.SIN_[i][j] = lasersinvec[i * 32 + j];  
  172.                 UsefulData.COS_[i][j] = lasercosvec[i * 32 + j];  
  173.                 //  printf("%f ", UsefulData.PASSEGEWAY_[i][j]);  
  174.   
  175.   
  176.             }  
  177.   
  178.         }  
  179.         //经度赋值  
  180.         for (int i1 = 0; i1 < 12; i1++)  
  181.         {  
  182.             for (int k = 0; k < 32; k++)  
  183.             {  
  184.                 if (k < 16)  
  185.                 {  
  186.                     UsefulData.Azimuth[i1][k] = UsefulData.JIAODU2_[i1];// +LASER_vert_correction[k];  
  187.                 }  
  188.                 else if (k >= 16)  
  189.                 {  
  190.                     if (i1 < 11)//前11帧  
  191.                     {  
  192.                         if (UsefulData.JIAODU2_[i1 + 1] < UsefulData.JIAODU2_[i1])  
  193.                         {  
  194.                             UsefulData.JIAODU2_[i1 + 1] += 360.0;  
  195.                             float azimuth2 = UsefulData.JIAODU2_[i1] + (UsefulData.JIAODU2_[i1 + 1] - UsefulData.JIAODU2_[i1]) / 2.0;  
  196.                             if (azimuth2 > 360)// 角度变化了  
  197.                             {  
  198.                                 azimuth2 -= 360;  
  199.                                 UsefulData.Azimuth[i1][k] = azimuth2;// +LASER_vert_correction[k - 16];  
  200.                             }  
  201.                             else  
  202.                             {  
  203.                                 UsefulData.Azimuth[i1][k] = azimuth2;// +LASER_vert_correction[k - 16];  
  204.                             }  
  205.                         }  
  206.                         else  
  207.                         {  
  208.                             float azimuth4 = UsefulData.JIAODU2_[i1] + (UsefulData.JIAODU2_[i1 + 1] - UsefulData.JIAODU2_[i1]) / 2.0;  
  209.                             UsefulData.Azimuth[i1][k] = azimuth4;// +LASER_vert_correction[k - 16  
  210.                         }  
  211.                     }  
  212.                     else if (i1 == 11)//最后一帧  
  213.                     {  
  214.                         float azimuth3 = UsefulData.JIAODU2_[i1] + 0.2;  
  215.                         if (azimuth3 > 360)  
  216.                         {  
  217.                             azimuth3 -= 360;  
  218.                         }  
  219.                         else  
  220.                         {  
  221.                         }  
  222.                         UsefulData.Azimuth[i1][k] = azimuth3;//;+LASER_vert_correction[k - 16];  
  223.                     }  
  224.                 }  
  225.                 point.x = UsefulData.JULI_[i1][k] * UsefulData.COS_[i1][k] * sin(UsefulData.Azimuth[i1][k] / 180.0*3.1415926);  
  226.                 point.y = UsefulData.JULI_[i1][k] * UsefulData.COS_[i1][k] * cos(UsefulData.Azimuth[i1][k] / 180.0*3.1415926);  
  227.                 point.z = UsefulData.JULI_[i1][k] * UsefulData.SIN_[i1][k];  
  228.                 point.r = UsefulData.PASSEGEWAY_[i1][k];  
  229.                 point.tus = UsefulData.TimeOffset[i1][k];  
  230.                 point.tsec = UsefulData.Timesec;  
  231.                 if ((point.x == 0) && (point.y == 0) && (point.z == 0) || (UsefulData.JULI_[i1][k] <= 0.05))  
  232.                 {  
  233.                 }  
  234.                 else  
  235.                 {  
  236.                     //X  Y  Z  Azimuth Distance Laser_ID   
  237.                     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);  
  238.                     fprintf(fp, "%f %f %ld %ld\n ", UsefulData.PASSEGEWAY_[i1][k], point.tus,  header->ts.tv_sec,ustime);  
  239.                 }  
  240.             }  
  241.   
  242.         }  
  243.         fclose(fp);  
  244.           
  245.     }  
  246.   
  247.     printf("\n\n");  
  248. }  
  249.   
  250. void UDPtoXYZfunALL(Usefulmessage data)  
  251. {  
  252.     PointXYZ point[384];  
  253.     FILE *fp;  
  254.     if ((fp = fopen("all.txt""a")) == NULL)  
  255.     {  
  256.         printf("Create File failure");  
  257.         //getch();  
  258.         exit(1);  
  259.     }  
  260.     for (int i = 0; i < 12; i++)  
  261.     {  
  262.         for (int j = 0; j < 32; j++)  
  263.         {  
  264.             point[i * 32 + j].x = data.JULI_[i][j] * data.COS_[i][j] * sin(data.Azimuth[i][j] / 180.0*3.1415926);  
  265.             point[i * 32 + j].y = data.JULI_[i][j] * data.COS_[i][j] * cos(data.Azimuth[i][j] / 180.0*3.1415926);  
  266.             point[i * 32 + j].z = data.JULI_[i][j] * data.SIN_[i][j];  
  267.             point[i * 32 + j].r = data.PASSEGEWAY_[i][j];  
  268.             point[i * 32 + j].tus = data.TimeOffset[i][j];  
  269.             point[i * 32 + j].tsec = data.Timesec;  
  270.             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))  
  271.             {  
  272.             }  
  273.             else  
  274.             {  
  275.                 //X  Y  Z  Azimuth Distance Laser_ID   
  276.                 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);  
  277.                 fprintf(fp, "%f %f %f %f \n ", data.PASSEGEWAY_[i][j], point[i * 32 + j].tus, data.TimeStamp, data.Timesec);  
  278.   
  279.                 //fprintf(fp, "%f %f %f \n", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);  
  280.   
  281.             }  
  282.         }  
  283.     }  
  284.     fclose(fp);  
  285. }  
  286.   
  287.   
  288. void UDPtoXYZfun(Usefulmessage data)  
  289. {  
  290.     PointXYZ point[384];  
  291.     FILE *fp;  
  292.     if ((fp = fopen("my.txt""a")) == NULL)  
  293.     {  
  294.         printf("Create File failure");  
  295.         //getch();  
  296.         exit(1);  
  297.     }     
  298.     for (int i = 0; i < 12; i++)  
  299.     {  
  300.         for (int j = 0; j < 32; j++)  
  301.         {  
  302.             point[i * 32 + j].x = data.JULI_[i][j]*data.COS_[i][j]*sin(data.Azimuth[i][j]/180.0*3.1415926);  
  303.             point[i * 32 + j].y = data.JULI_[i][j]*data.COS_[i][j]*cos(data.Azimuth[i][j]/180.0*3.1415926);  
  304.             point[i * 32 + j].z = data.JULI_[i][j] * data.SIN_[i][j];  
  305.             point[i * 32 + j].r = data.PASSEGEWAY_[i][j];  
  306.             point[i * 32 + j].tus = data.TimeOffset[i][j];  
  307.             point[i * 32 + j].tsec = data.Timesec;  
  308.             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))  
  309.             {  
  310.             }  
  311.             else  
  312.             {  
  313.   
  314.                 //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);  
  315.                 fprintf(fp, "%f %f %f \n", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);  
  316.   
  317.             }  
  318.         }  
  319.     }  
  320.     fclose(fp);  

你可能感兴趣的:(激光雷达)