c# 解析gprmc数据_Windows下VLP16激光雷达数据解析

最近，实验室有一个对VLP16数据解析的需求，要求在Windows系统下单独把VLP16的数据解析整理出来，作为后期多传感器融合的一个必要基础准备。无奈从ROS转战Windows，网上查了查Windows系统下velodyne激光雷达的驱动，只找到了一个VeloView，很复杂，VeloView依赖winpcap、paraview、qt、python......单独摘出数据解析模块很麻烦。

Kitware/VeloViewgithub.com

本来在Windows下在重新写一个VLP16的驱动就属于重复造轮子，而且还不能保证轮子是圆的。。。所以又转回ROS下，去撸了一遍velodyne驱动的源码，ROS下的驱动源码还是很清楚的，所以决定把ROS下velodyne驱动的数据解析模块抠出来，移植到Windows下，期间还是有些坑要踩的。

一、VLP16原始数据格式

用过VLP16的人应该都清楚它的原始数据格式，雷达的技术文档里有详细的叙述，网上也有很多帖子解释VLP16的数据格式，这里我就不详细的说了，贴出几个我觉得写的还可以的帖子，如果对VLP16数据不熟悉的建议先把介绍看一下。

Velodyne VLP-16激光雷达数据格式解析blog.csdn.net

这里我要强调的是，网上这些帖子是介绍VLP16的pcap格式原始数据的，pcap是一种通用的网络传输文件格式（本人对这方面不是很了解，叙述可能不准确。。。），它不是由驱动接收到的原始数据。例如下面是我用wireshark软件接收到的VLP16数据：

可以看到数据包的端口是2368，长度是1248字节，但这当中由pcap的42字节的数据头，因此，实际的原始数据是1206字节，从图中3框的位置开始，“ff ee”是两字节的数据开始标志。所以，我们要做的就是从网口接收VLP16的原始数据，以packet为单位，解析出一个packet的时间戳、方位角、和384个点坐标。

我的主要参考就是ROS下的velodyne驱动：

https://github.com/ros-drivers/velodynegithub.com

二、程序设计

VLP16播发的是UDP数据包，可以通过socket接收，其中包括两种类型：端口为2368的数据包和端口为8308的位置包。我们主要解析数据包即可，因为它包含了点云数据、时间戳、反射率和方位角信息，够用了。如果对VLP16使用PPS + GPRMC 进行了时间同步，则在位置包中还可以解析出经纬度坐标、UTC时间戳、GPS标志等非常有用的信息。

1、socket编程

使用套接字接收原始数据，其主要代码如下：

    //创建套接字SOCKET sock = socket(PF_INET, SOCK_DGRAM, 0);//服务器地址信息sockaddr_in servAddr;memset(&servAddr, 0, sizeof(servAddr));  //每个字节都用0填充servAddr.sin_family = PF_INET;servAddr.sin_addr.s_addr = INADDR_ANY;servAddr.sin_port = htons(2308);bind(sock, (SOCKADDR*)&servAddr, sizeof(SOCKADDR));//接收SOCKADDR clntAddr;  //地址信息int nSize = sizeof(SOCKADDR);char buffer[BUF_SIZE];  //缓冲区int strLen = recvfrom(sock, buffer, BUF_SIZE, 0, &clntAddr, &nSize);

依赖这两个：

#include <winsock2.h>
#pragma comment (lib, "ws2_32.lib")

配置好IPV4网址后，启动程序，可以看到buffer中已经有了内容，正好是1206大小。

2、解析VLP16原始数据

这里要说一下，接收到的1206字节的原始数据是char类型的，其范围为-128 ~ 127，查看技术手册可以知道，pcap类型的原始数据存储的是十六进制的数据，这里正好对应了，可以通过强制转换将char类型的原始数据转为uint8_t类型的数据，这样就可以使十六进制的原始数据与uint8_t类型的数据对应起来了。下面贴出解析VLP16数据的源代码。

主要思想其实很简单，就是首先解析出极坐标的距离值，然后根据配置参数，加入各项改正后，在根据激光雷达所采用模型的不同，最后把坐标进行想应的转换即可。原驱动中数据解析考虑的情况非常多，也做了很多的改正，例如距离改正量、方位角改正量和模型假设等。这些不是按照技术文档里说的那样，把解析出的距离和方位角、垂直角一转换就行的。以下是我根据ROS下的驱动，结合我们组的实际需求进行修改后的代码，有一定的参考意义吧。

void VelodyneDriver::unpack_vlp16(uint8_t *pkt, VelodynePacket *packet)
{float azimuth;float azimuth_diff;int raw_azimuth_diff;float last_azimuth_diff = 0;float azimuth_corrected_f;int azimuth_corrected;float x, y, z;float intensity;const raw_packet *raw = (const raw_packet *)&pkt[0];packet->time_stamp = raw->time_stamp.uint;for (int block = 0; block < BLOCKS_PER_PACKET; block++) {if (UPPER_BANK != raw->blocks[block].header) {std::cout << "skipping invalid VLP-16 packet: block "<< block << " header value is "<< raw->blocks[block].header << std::endl;return;}azimuth = (float)(raw->blocks[block].rotation);packet->azimuth[block] = raw->blocks[block].rotation;if (block < (BLOCKS_PER_PACKET - 1)) {raw_azimuth_diff = raw->blocks[block + 1].rotation - raw->blocks[block].rotation;azimuth_diff = (float)((36000 + raw_azimuth_diff) % 36000);if (raw_azimuth_diff < 0){std::cout << "Packet containing angle overflow, first angle: " << raw->blocks[block].rotation << " second angle: " << raw->blocks[block + 1].rotation << std::endl;if (last_azimuth_diff > 0) {azimuth_diff = last_azimuth_diff;}else {continue;}}last_azimuth_diff = azimuth_diff;}else {azimuth_diff = last_azimuth_diff;}for (int firing = 0, k = 0; firing < VLP16_FIRINGS_PER_BLOCK; firing++) {for (int dsr = 0; dsr < VLP16_SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) {LaserCorrection &corrections = laser_corrections_[dsr];union two_bytes tmp;tmp.bytes[0] = raw->blocks[block].data[k];tmp.bytes[1] = raw->blocks[block].data[k + 1];azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION);azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;if ((azimuth_corrected >= config_.min_angle && azimuth_corrected <= config_.max_angle  && config_.min_angle < config_.max_angle) || (config_.min_angle > config_.max_angle && (azimuth_corrected <= config_.max_angle || azimuth_corrected >= config_.min_angle))) {float distance = tmp.uint * distance_resolution_m_;distance += corrections.dist_correction;float cos_vert_angle = corrections.cos_vert_correction;float sin_vert_angle = corrections.sin_vert_correction;float cos_rot_correction = corrections.cos_rot_correction;float sin_rot_correction = corrections.sin_rot_correction;float cos_rot_angle =cos_rot_table_[azimuth_corrected] * cos_rot_correction +sin_rot_table_[azimuth_corrected] * sin_rot_correction;float sin_rot_angle =sin_rot_table_[azimuth_corrected] * cos_rot_correction -cos_rot_table_[azimuth_corrected] * sin_rot_correction;float horiz_offset = corrections.horiz_offset_correction;float vert_offset = corrections.vert_offset_correction;float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;if (xx < 0) xx = -xx;if (yy < 0) yy = -yy;float distance_corr_x = 0;float distance_corr_y = 0;if (corrections.two_pt_correction_available) {distance_corr_x = (corrections.dist_correction - corrections.dist_correction_x)* (xx - 2.4) / (25.04 - 2.4) + corrections.dist_correction_x;distance_corr_x -= corrections.dist_correction;distance_corr_y = (corrections.dist_correction - corrections.dist_correction_y)* (yy - 1.93) / (25.04 - 1.93) + corrections.dist_correction_y;distance_corr_y -= corrections.dist_correction;}float distance_x = distance + distance_corr_x;xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle;x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;float distance_y = distance + distance_corr_y;xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle;y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;z = distance_y * sin_vert_angle + vert_offset*cos_vert_angle;float x_coord = y;float y_coord = -x;float z_coord = z;float min_intensity = corrections.min_intensity;float max_intensity = corrections.max_intensity;intensity = raw->blocks[block].data[k + 2];float focal_offset = 256 * SQR(1 - corrections.focal_distance / 13100);float focal_slope = corrections.focal_slope;intensity += focal_slope * (std::abs(focal_offset - 256 *SQR(1 - tmp.uint / 65535)));intensity = (intensity < min_intensity) ? min_intensity : intensity;intensity = (intensity > max_intensity) ? max_intensity : intensity;//if (x_coord < -300 || y_coord < -300 || z_coord < -300 || x_coord > 300 || y_coord > 300 || z_coord > 300)//   continue;addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, intensity, packet->data[block * 32 + firing * 16 + dsr]);}}}}
}

三、写在最后

代码的移植还是比较简单的，虽然也踩过了一些坑，尤其是进制转换那里，源代码可是秀了我一脸，建议大家还是自己看一下源码的想应片段，很快就能看完，对于自己的理解很有帮助。

最后把驱动封装成一个类，定义符合我们要求的数据结构，留下调用的接口，就完事了，下一步就是多传感器融合了。

做SLAM的小伙伴们，一起加油吧！！！