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格式的点云数据,写完之后发现其实代码也不多,更轻量级了,代码如下:
-
-
- #define _CRT_SECURE_NO_DEPRECATE
- #define _CRT_SECURE_NO_WARNINGS
- #include "pcap.h"
- #include "stdio.h"
- #include "math.h"
- #include
- #include
- #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 *);
-
-
-
- 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;
- }
-
-
- 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);
-
- 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);
-
- if ((fp = fopen(fn, "a")) == NULL)
- {
- printf("Create File failure 1");
- fclose(fp);
-
- }
-
-
- 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;
-
- 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;
-
- }
- 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];
-
-
-
- }
-
- }
-
- for (int i1 = 0; i1 < 12; i1++)
- {
- for (int k = 0; k < 32; k++)
- {
- if (k < 16)
- {
- UsefulData.Azimuth[i1][k] = UsefulData.JIAODU2_[i1];
- }
- else if (k >= 16)
- {
- if (i1 < 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;
- }
- else
- {
- UsefulData.Azimuth[i1][k] = azimuth2;
- }
- }
- else
- {
- float azimuth4 = UsefulData.JIAODU2_[i1] + (UsefulData.JIAODU2_[i1 + 1] - UsefulData.JIAODU2_[i1]) / 2.0;
- UsefulData.Azimuth[i1][k] = azimuth4;
- }
- }
- else if (i1 == 11)
- {
- float azimuth3 = UsefulData.JIAODU2_[i1] + 0.2;
- if (azimuth3 > 360)
- {
- azimuth3 -= 360;
- }
- else
- {
- }
- UsefulData.Azimuth[i1][k] = azimuth3;
- }
- }
- 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
- {
-
- 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");
-
- 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 ", 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);
-
-
-
- }
- }
- }
- fclose(fp);
- }
-
-
- void UDPtoXYZfun(Usefulmessage data)
- {
- PointXYZ point[384];
- FILE *fp;
- if ((fp = fopen("my.txt", "a")) == NULL)
- {
- printf("Create File failure");
-
- 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 \n", point[i * 32 + j].x, point[i * 32 + j].y, point[i * 32 + j].z);
-
- }
- }
- }
- fclose(fp);
- }