c# 解析gprmc数据-凯发ag旗舰厅登录网址下载

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

kitware/veloview​github.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/velodyne​github.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 #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的小伙伴们，一起加油吧！！！

总结

以上是凯发ag旗舰厅登录网址下载为你收集整理的c# 解析gprmc数据_windows下vlp16激光雷达数据解析的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得凯发ag旗舰厅登录网址下载网站内容还不错，欢迎将凯发ag旗舰厅登录网址下载推荐给好友。