轉載自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）然后就看到了最終的結果
一 基本格式：
文件頭 數據包頭 數據報數據包頭數據報......
二、文件頭：
文件頭結構體
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格式的點雲數據，寫完之后發現其實代碼也不多，更輕量級了，代碼如下：
// 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);
}