最近，實驗室有一個對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 <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的小伙伴們，一起加油吧！！！

總結

以上是生活随笔為你收集整理的c# 解析gprmc数据_Windows下VLP16激光雷达数据解析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。