vlp16 device pass test,

with auto-reconnection ability

CMakeLists.txt

@@ -5,14 +5,34 @@ cmake_minimum_required(VERSION 3.5)
 set (CMAKE_CXX_STANDARD 11)
 
 #set(PCL_DIR "/home/youchen/pcl-1.8/share/pcl-1.8")
+#nanomsg
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(nanomsg REQUIRED nanomsg)
+
+#yaml
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+find_path(YAML_CPP_INCLUDE_DIR
+          NAMES yaml_cpp.h
+          PATHS ${YAML_CPP_INCLUDE_DIRS})
+find_library(YAML_CPP_LIBRARY
+             NAMES YAML_CPP
+             PATHS ${YAML_CPP_LIBRARY_DIRS})
+link_directories(${YAML_CPP_LIBRARY_DIRS})
+if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
+  add_definitions(-DHAVE_NEW_YAMLCPP)
+endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
+
+# MESSAGE(WARNING ${YAML_CPP_VERSION}    ${YAML_CPP_INCLUDE_DIRS}     ${YAML_CPP_LIBRARY_DIRS}) 
+
+#other libs
 FIND_PACKAGE(Protobuf REQUIRED)
 FIND_PACKAGE(Glog REQUIRED)
 FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL REQUIRED)
 FIND_PACKAGE(Ceres REQUIRED)
 
+
+
 MESSAGE(WARN "pcl:: ${PCL_INCLUDE_DIRS} --- ${PCL_LIBRARIES}")
 include_directories(
         /usr/local/include
@@ -41,34 +61,51 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication COMMUNICATION_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/system SYSTEM_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY_SRC )
 
-add_executable(measure_wj
-        main.cpp
-        ${ERROR_SRC}
-        ${message_src}
-
-		${WANJI_LIDAR_SRC}
-        ${TASK_MANAGER_SRC}
-        ${TOOL_SRC}
-        ${COMMUNICATION_SRC}
-        ${SYSTEM_SRC}
-		${VERIFY_SRC}
-
-		)
-
-target_link_libraries(measure_wj
-        /usr/local/lib/libglog.a
-        /usr/local/lib/libgflags.a
-        /usr/local/lib/liblivox_sdk_static.a
-        /usr/local/apr/lib/libapr-1.a
-        nnxx
-        nanomsg
-
-        ${PROTOBUF_LIBRARIES}
-        ${OpenCV_LIBS}
-        ${GLOG_LIBRARIES}
-        ${PCL_LIBRARIES}
-		${CERES_LIBRARIES}
+# add_executable(measure_wj
+#         main.cpp
+#         ${ERROR_SRC}
+#         ${message_src}
 
-        -lpthread
-        )
+# 		${WANJI_LIDAR_SRC}
+#         ${TASK_MANAGER_SRC}
+#         ${TOOL_SRC}
+#         ${COMMUNICATION_SRC}
+#         ${SYSTEM_SRC}
+# 		${VERIFY_SRC}
 
+# 		)
+
+# target_link_libraries(measure_wj
+#         /usr/local/lib/libglog.a
+#         /usr/local/lib/libgflags.a
+#         /usr/local/lib/liblivox_sdk_static.a
+#         /usr/local/apr/lib/libapr-1.a
+#         nnxx
+#         nanomsg
+
+#         ${PROTOBUF_LIBRARIES}
+#         ${OpenCV_LIBS}
+#         ${GLOG_LIBRARIES}
+#         ${PCL_LIBRARIES}
+# 		${CERES_LIBRARIES}
+
+#         -lpthread
+#         )
+
+# vlp16 driver test
+add_executable(vlp16 tests/vlp16_driver_test.cpp 
+velodyne_lidar/velodyne_driver/input.cc 
+velodyne_lidar/velodyne_driver/rawdata.cc
+velodyne_lidar/velodyne_driver/calibration.cc
+velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp
+velodyne_lidar/velodyne_driver/velodyne_lidar_scan_task.cpp
+velodyne_lidar/velodyne_config.pb.cc
+${ERROR_SRC}
+${TOOL_SRC}
+${TASK_MANAGER_SRC}
+${WANJI_LIDAR_SRC}
+)
+target_link_libraries(vlp16 
+${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ${YAML_CPP_LIBRARIES} ${OpenCV_LIBS}
+        ${GLOG_LIBRARIES}
+		${CERES_LIBRARIES})

error_code/error_code.h

@@ -252,6 +252,12 @@ enum Error_code
 	WANJI_LIDAR_DEVICE_TASK_TYPE_ERROR,								//万集设备模块，任务类型错误
 	WANJI_LIDAR_DEVICE_TASK_OVER_TIME,								//万集设备模块，任务超时
 	WANJI_LIDAR_DEVICE_NO_CLOUD,									//万集设备模块，没有点云
+    VELODYNE_LIDAR_DEVICE_ERROR_BASE,								//velodyne设备模块，错误基类
+    VELODYNE_LIDAR_DEVICE_STATUS_BUSY,									//velodyne设备模块，状态正忙
+	VELODYNE_LIDAR_DEVICE_STATUS_ERROR,								//velodyne设备模块，状态错误
+	VELODYNE_LIDAR_DEVICE_TASK_TYPE_ERROR,								//velodyne设备模块，任务类型错误
+	VELODYNE_LIDAR_DEVICE_TASK_OVER_TIME,								//velodyne设备模块，任务超时
+	VELODYNE_LIDAR_DEVICE_NO_CLOUD,									//velodyne设备模块，没有点云
 
 
 //万集通信
@@ -264,6 +270,14 @@ enum Error_code
     WJ_LIDAR_READ_FAILED,											//万集通信,读取失败
     WJ_LIDAR_WRITE_FAILED,											//万集通信,写入失败
     WJ_LIDAR_GET_CLOUD_TIMEOUT,										//万集通信,获取点云超时
+    VELODYNE_LIDAR_COMMUNICATION_UNINITIALIZED,							//velodyne通信,未初始化
+	VELODYNE_LIDAR_COMMUNICATION_DISCONNECT,								//velodyne通信,断连
+	VELODYNE_LIDAR_COMMUNICATION_FAULT,									//velodyne通信,故障
+	VELODYNE_LIDAR_CONNECT_FAILED,										//velodyne通信,连接失败
+    VELODYNE_LIDAR_UNINITIALIZED,											//velodyne通信,未初始化
+    VELODYNE_LIDAR_READ_FAILED,											//velodyne通信,读取失败
+    VELODYNE_LIDAR_WRITE_FAILED,											//velodyne通信,写入失败
+    VELODYNE_LIDAR_GET_CLOUD_TIMEOUT,										//velodyne通信,获取点云超时
 
 //万集解析
     WJ_PROTOCOL_ERROR_BASE										=0x06020000,

proto.sh

@@ -8,3 +8,5 @@ protoc -I=./communication communication.proto --cpp_out=./communication
 
 protoc -I=./wanji_lidar wj_lidar_conf.proto --cpp_out=./wanji_lidar
 protoc -I=./verify hardware_limit.proto --cpp_out=./verify
+
+protoc -I=./velodyne_lidar velodyne_config.proto --cpp_out=./velodyne_lidar

setting/VLP16db.yaml

@@ -0,0 +1,51 @@
+lasers:
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 0, rot_correction: 0.0,
+  vert_correction: -0.2617993877991494, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 1, rot_correction: 0.0,
+  vert_correction: 0.017453292519943295, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 2, rot_correction: 0.0,
+  vert_correction: -0.22689280275926285, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 3, rot_correction: 0.0,
+  vert_correction: 0.05235987755982989, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 4, rot_correction: 0.0,
+  vert_correction: -0.19198621771937624, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 5, rot_correction: 0.0,
+  vert_correction: 0.08726646259971647, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 6, rot_correction: 0.0,
+  vert_correction: -0.15707963267948966, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 7, rot_correction: 0.0,
+  vert_correction: 0.12217304763960307, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 8, rot_correction: 0.0,
+  vert_correction: -0.12217304763960307, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 9, rot_correction: 0.0,
+  vert_correction: 0.15707963267948966, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 10, rot_correction: 0.0,
+  vert_correction: -0.08726646259971647, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 11, rot_correction: 0.0,
+  vert_correction: 0.19198621771937624, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 12, rot_correction: 0.0,
+  vert_correction: -0.05235987755982989, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 13, rot_correction: 0.0,
+  vert_correction: 0.22689280275926285, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 14, rot_correction: 0.0,
+  vert_correction: -0.017453292519943295, vert_offset_correction: 0.0}
+- {dist_correction: 0.0, dist_correction_x: 0.0, dist_correction_y: 0.0, focal_distance: 0.0,
+  focal_slope: 0.0, horiz_offset_correction: 0.0, laser_id: 15, rot_correction: 0.0,
+  vert_correction: 0.2617993877991494, vert_offset_correction: 0.0}
+num_lasers: 16
+distance_resolution: 0.002

system/system_executor.cpp

@@ -360,7 +360,7 @@ void System_executor::execute_for_measure(std::string command_info, int terminal
 	Error_manager t_result;
 	LOG(INFO) << " System_executor::execute_for_measure run "<< this;
 
-	//第二步, 按照时间生成中间文件的保存路径
+	//第1步, 按照时间生成中间文件的保存路径
 	time_t nowTime;
 	nowTime = time(NULL);
 	struct tm* sysTime = localtime(&nowTime);
@@ -369,14 +369,14 @@ void System_executor::execute_for_measure(std::string command_info, int terminal
 			sysTime->tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec);
 
 
-	//第7步, 创建万集管理模块的任务单, 并发送到 wanji_manager
+	//第2步, 创建万集管理模块的任务单, 并发送到 wanji_manager
 	Wanji_manager_task  t_wanji_manager_task ;
 	Common_data::Car_wheel_information	t_car_information_by_wanji;
-	t_wanji_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
+	t_wanji_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(3000),
 								   terminal_id, receive_time);
 	t_error = Task_command_manager::get_instance_references().execute_task(&t_wanji_manager_task);
 
-	//第8步, 等待任务单完成
+	//第3步, 等待任务单完成
 	if ( t_error != Error_code::SUCCESS )
 	{
 		LOG(INFO) << " Wanji_manager  execute_task   error "<< this;
@@ -417,8 +417,8 @@ void System_executor::execute_for_measure(std::string command_info, int terminal
 
 	//measure 测量模块的最终结果
 	Common_data::Car_measure_information	t_car_information_result;
-	//第9步, 生成反馈消息
 
+	//第4步, 生成反馈消息
 	if(t_car_information_by_wanji.correctness == true )
 	{
 		t_car_information_result.center_x = t_car_information_by_wanji.center_x;

task/task_base.h

@@ -26,6 +26,10 @@ enum Task_type
     WANJI_MANAGER_TASK,						//万集雷达管理任务
 	WANJI_LIDAR_SCAN,						//万集雷达扫描任务
 	WANJI_LIDAR_DETECT,						//万集雷达定位任务
+
+	VELODYNE_MANAGER_TASK,						//velodyne雷达管理任务
+	VELODYNE_LIDAR_SCAN,						//velodyne雷达扫描任务
+	VELODYNE_LIDAR_DETECT,						//velodyne雷达定位任务
     
 };
 //任务状态，如果任务故障，任务状态改为TASK_OVER，然后在m_task_error_manager 补充错误码。

tests/vlp16_driver_test.cpp

@@ -0,0 +1,115 @@
+/*
+ * @Description: vlp16驱动测试
+ * @Author: yct
+ * @Date: 2021-07-26 14:11:26
+ * @LastEditTime: 2021-07-28 10:14:20
+ * @LastEditors: yct
+ */
+
+#include <iostream>
+#include <fstream>
+
+#include "../velodyne_lidar/velodyne_driver/input.h"
+#include "../velodyne_lidar/velodyne_driver/rawdata.h"
+bool velo_driver_test()
+{
+    velodyne_driver::InputSocket sock;
+    if (!sock.init("", 2368))
+    {
+        std::cout << "sock init failed" << std::endl;
+        return false;
+    }
+
+    velodyne_rawdata::RawData data;
+    data.setup("VLP16", "/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/VLP16db.yaml", 1.2, 0.1, 0, 360);
+
+    int count = 0;
+    unsigned char pkt[1206];
+    std::vector<Lidar_point> t_point;
+    while (count++ < 50000)
+    {
+        int ret = sock.getPacket(pkt);
+        if (ret != 0)
+            std::cout << "get pack: " << ret << std::endl;
+        else
+        {
+            // t_point.clear();
+            // memset(&pkt[1205], 0, 1);
+            // printf("%s\n", pkt);
+            data.unpack(pkt, t_point);
+            std::cout << "point size: " << t_point.size() << std::endl;
+            if(t_point.size() > 30000)
+            {
+                std::ofstream out("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/point.txt");
+                if(out.is_open())
+                {
+                    for (size_t i = 0; i < t_point.size(); i++)
+                    {
+                        char buf[255] = {0};
+                        sprintf(buf, "%.3f %.3f %.3f %.6f\n", t_point[i].x, t_point[i].y, t_point[i].z, t_point[i].timestamp);
+                        out << buf;
+                    }
+                    out.close();
+                    return true;
+                }
+                else
+                {
+                    std::cout << "cannot open file to write cloud." << std::endl;
+                }
+            }
+        }
+        usleep(500);
+    }
+    sock.uninit();
+    return true;
+}
+
+// 初始化velodyne设备并获取点云
+#include "../velodyne_lidar/velodyne_driver/velodyne_lidar_device.h"
+void device_test()
+{
+    velodyne::velodyneLidarParams params;
+    params.set_ip("");
+    params.set_port(2368);
+    params.set_model("VLP16");
+    params.set_calibrationfile("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/VLP16db.yaml");
+
+    Velodyne_lidar_device t_device;
+    t_device.init(params);
+
+    usleep(1000 * 1000);
+
+    std::mutex p_mutex;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    Error_manager ec = t_device.get_new_cloud_and_wait(&p_mutex, p_cloud, std::chrono::steady_clock::now(), 6000);
+
+    if (ec == SUCCESS)
+    {
+        std::ofstream out("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/device_point.txt");
+        if (out.is_open())
+        {
+            for (size_t i = 0; i < p_cloud->size(); i++)
+            {
+                char buf[255] = {0};
+                sprintf(buf, "%.3f %.3f %.3f\n", p_cloud->points[i].x, p_cloud->points[i].y, p_cloud->points[i].z);
+                out << buf;
+            }
+            std::cout << "cloud written" << std::endl;
+            out.close();
+        }
+        else
+        {
+            std::cout << "cannot open file to write cloud." << std::endl;
+        }
+    }else{
+        std::cout << "get cloud failed" << std::endl;
+    }
+}
+
+int main()
+{
+    // velo_driver_test();
+    device_test();
+
+    return 0;
+}

velodyne_lidar/ground_region.cpp

@@ -0,0 +1,657 @@
+//
+// Created by zx on 2021/5/20.
+//
+
+#include "ground_region.h"
+
+#include <pcl/common/transforms.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <fcntl.h>
+#include "publisher.h"
+#include "point_tool.h"
+//欧式聚类*******************************************************
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
+{
+    std::vector<pcl::PointIndices> ece_inlier;
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
+    ece.setInputCloud(sor_cloud);
+    ece.setClusterTolerance(0.07);
+    ece.setMinClusterSize(20);
+    ece.setMaxClusterSize(20000);
+    ece.setSearchMethod(tree);
+    ece.extract(ece_inlier);
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>  segmentation_clouds;
+    for (int i = 0; i < ece_inlier.size(); i++)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
+        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
+        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy);//按照索引提取点云数据
+        segmentation_clouds.push_back(cloud_copy);
+    }
+    return segmentation_clouds;
+}
+
+double Ground_region::distance(cv::Point2f p1, cv::Point2f p2)
+{
+    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
+}
+
+
+bool Ground_region::isRect(std::vector<cv::Point2f>& points)
+{
+    if (points.size() == 4)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        cv::Point2f ps = points[0], pt = points[1];
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l)
+            {
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+
+        //直角边与坐标轴的夹角 <20°
+        float thresh=20.0*M_PI/180.0;
+        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
+        float angle=atan2(vct.y,vct.x);
+        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
+        {
+            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            /*char description[255]={0};
+            sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+        float width=std::min(l1,l2);
+        float length=std::max(l1,l2);
+        if(width<1.400 || width >1.900 || length >3.300 ||length < 2.200)
+        {
+            /*char description[255]={0};
+            sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+        double d = distance(pc, points[3]);
+        cv::Point2f center1 = (ps + pt) * 0.5;
+        cv::Point2f center2 = (pc + points[3]) * 0.5;
+        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
+            /*std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
+            char description[255]={0};
+            sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+        //std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
+
+
+        return true;
+    }
+    else if(points.size()==3)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        int max_index = 0;
+        cv::Point2f ps = points[0], pt = points[1];
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l) {
+                max_index = i;
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+
+        //直角边与坐标轴的夹角 <20°
+        float thresh=20.0*M_PI/180.0;
+        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
+        float angle=atan2(vct.y,vct.x);
+        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
+        {
+            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            /*char description[255]={0};
+            sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+        double l=std::max(l1,l2);
+        double w=std::min(l1,l2);
+        if(l>2.100 && l<3.300 && w>1.400 && w<2.100)
+        {
+            //生成第四个点
+            cv::Point2f vec1=ps-pc;
+            cv::Point2f vec2=pt-pc;
+            cv::Point2f point4=(vec1+vec2)+pc;
+            points.push_back(point4);
+
+            /*char description[255]={0};
+            sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
+            std::cout<<description<<std::endl;*/
+            return true;
+        }
+        else
+        {
+
+            /*char description[255]={0};
+            sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+    }
+    //std::cout<<" default false"<<std::endl;
+    return false;
+
+}
+bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double thresh_z,
+        detect_wheel_ceres3d::Detect_result& result)
+{
+    if(m_detector== nullptr)
+        return false;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    for(int i=0;i<cloud->size();++i)
+    {
+        pcl::PointXYZ pt=cloud->points[i];
+        if(pt.z< thresh_z)
+        {
+            cloud_filtered->push_back(pt);
+        }
+    }
+    //下采样
+    pcl::VoxelGrid<pcl::PointXYZ> vox;  //创建滤波对象
+    vox.setInputCloud(cloud_filtered);                 //设置需要过滤的点云给滤波对象
+    vox.setLeafSize(0.02f, 0.02f, 0.2f);     //设置滤波时创建的体素体积为1cm的立方体
+    vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
+    if(cloud_filtered->size()==0)
+    {
+        return false;
+    }
+
+    if(cloud_filtered->size()==0)
+        return false;
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;   //创建滤波器对象
+    sor.setInputCloud (cloud_filtered);                           //设置待滤波的点云
+    sor.setMeanK (5);                               //设置在进行统计时考虑的临近点个数
+    sor.setStddevMulThresh (3.0);                      //设置判断是否为离群点的阀值，用来倍乘标准差，也就是上面的std_mul
+    sor.filter (*cloud_filtered);                    //滤波结果存储到cloud_filtered
+
+    if(cloud_filtered->size()==0)
+    {
+        return false;
+    }
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
+    seg_clouds=segmentation(cloud_filtered);
+
+    if(!(seg_clouds.size()==4 || seg_clouds.size()==3))
+    {
+        return false;
+    }
+    std::vector<cv::Point2f> centers;
+    for(int i=0;i<seg_clouds.size();++i)
+    {
+        Eigen::Vector4f centroid;
+        pcl::compute3DCentroid(*seg_clouds[i], centroid);
+        centers.push_back(cv::Point2f(centroid[0],centroid[1]));
+
+    }
+    bool ret= isRect(centers);
+    if(ret)
+    {
+
+        std::string error_str;
+        if(m_detector->detect(seg_clouds,result,error_str))
+        {
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+    return ret;
+}
+
+
+Ground_region::~Ground_region()
+{
+    m_exit_condition.set_pass_ever(true);
+    // Close Capturte Thread
+    if( m_running_thread && m_running_thread->joinable() ){
+        m_running_thread->join();
+        m_running_thread->~thread();
+        delete m_running_thread;
+        m_running_thread = nullptr;
+    }
+
+    for(int i=0;i<m_lidars.size();++i)
+    {
+        if(m_lidars[i]!= nullptr)
+        {
+            m_lidars[i]->close();
+            delete m_lidars[i];
+        }
+    }
+    m_lidars.clear();
+    if(m_detector)
+    {
+        delete m_detector;
+        m_detector= nullptr;
+    }
+
+
+}
+Ground_region::Ground_region()
+{
+    m_detector= nullptr ;
+    m_running_thread=nullptr;
+}
+Error_manager Ground_region::init(std::string configure_file)
+{
+    if(read_proto_param(configure_file,m_configure)==false)
+    {
+        return Error_manager(FAILED,NORMAL,"read proto failed");
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr left_model=ReadTxtCloud("/media/youchen/block_ntfs/yct/catkin_ws/src/feature_extra/left_model.txt");
+    // std::cout << "left model:" << left_model->size() << std::endl;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr right_model = ReadTxtCloud("/media/youchen/block_ntfs/yct/catkin_ws/src/feature_extra/right_model.txt");
+    // std::cout << "right model:" << right_model->size() << std::endl;
+    m_detector=new detect_wheel_ceres3d(left_model,right_model);
+    // std::cout << "????" << std::endl;
+
+    return init(m_configure);
+
+}
+Error_manager Ground_region::init(ground_region::Configure configure)
+{
+    //绑定通讯ip
+    /*char connect_str[255]={0};
+    sprintf(connect_str,"tcp://%s:%d",configure.communication_cfg().ip().c_str(),
+            configure.communication_cfg().port());
+    Publisher::get_instance_pointer()->communication_bind(connect_str);
+    Publisher::get_instance_pointer()->communication_run();
+
+    //创建雷达
+    m_lidars.resize(configure.lidar_size());
+    for(int i=0;i<configure.lidar_size();++i)
+    {
+        const boost::asio::ip::address address = boost::asio::ip::address::from_string( configure.lidar(i).ip() );
+        const unsigned short port = configure.lidar(i).port();
+        m_lidars[i]=new velodyne::VLP16Capture(address,port);
+        if( !m_lidars[i]->isOpen() ){
+            char description[255]={0};
+            sprintf(description,"lidar [%d] open failed  ip:%s ,port:%d",i,configure.lidar(i).ip().c_str(),port);
+            return Error_manager(FAILED,NORMAL,description);
+        }
+    }
+*/
+    m_configure=configure;
+    m_running_thread=new std::thread(std::bind(thread_measure_func,this));
+    return SUCCESS;
+}
+
+bool Ground_region::read_proto_param(std::string file, ::google::protobuf::Message& parameter)
+{
+    int fd = open(file.c_str(), O_RDONLY);
+    if (fd == -1) return false;
+    FileInputStream* input = new FileInputStream(fd);
+    bool success = google::protobuf::TextFormat::Parse(input, &parameter);
+    delete input;
+    close(fd);
+    return success;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr Ground_region::scans2cloud(const velodyne::Frame& frame,
+        const ground_region::Calib_parameter& calib)
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    for(int i=0;i<frame.scans.size();++i)
+    {
+        pcl::PointXYZ pt;
+
+    }
+    // 变换
+    Eigen::Matrix3d rotation_matrix3 ;
+    rotation_matrix3= Eigen::AngleAxisd(calib.r(), Eigen::Vector3d::UnitZ()) *
+            Eigen::AngleAxisd(calib.p(), Eigen::Vector3d::UnitY()) *
+            Eigen::AngleAxisd(calib.y(), Eigen::Vector3d::UnitX());
+
+    Eigen::Affine3d transform_2 = Eigen::Affine3d::Identity();
+    transform_2.translation() << calib.cx(),calib.cy(),calib.cz();
+    transform_2.rotate (rotation_matrix3);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transpose(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::transformPointCloud (*cloud, *cloud_transpose, transform_2);
+
+    return cloud_transpose;
+}
+
+Error_manager Ground_region::sync_capture(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,double timeout_second)
+{
+    Tick timer;
+    bool sync=false;
+    clouds.clear();
+    std::vector<velodyne::Frame>  sync_frames;
+    while((false==m_exit_condition.wait_for_ex(std::chrono::microseconds(1))) && sync==false)
+    {
+        bool sync_once=true;
+        for(int i=0;i<m_lidars.size();++i)
+        {
+            velodyne::Frame frame;
+            m_lidars[i]->get_frame(frame);
+            if(frame.tic.tic()>0.2)
+            {
+                sync_frames.clear();
+                sync_once=false;
+                break;
+            }
+            else
+            {
+                sync_frames.push_back(frame);
+            }
+        }
+        if(timer.tic()>timeout_second)
+        {
+            return Error_manager(FAILED,NORMAL,"sync capture time out");
+        }
+        sync=sync_once;
+        usleep(1);
+    }
+    if(sync)
+    {
+        for(int i=0;i<sync_frames.size();++i)
+        {
+            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=(scans2cloud(sync_frames[i],m_configure.lidar(i).calib()));
+            clouds.push_back(cloud);
+        }
+        return SUCCESS;
+    }
+    return FAILED;
+
+}
+
+Error_manager Ground_region::prehandle_cloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,
+                                             message::Ground_measure_statu_msg& msg)
+{
+    if(msg.laser_statu_vector_size()!=m_lidars.size())
+        return ERROR;
+    //直通滤波
+    ground_region::Box box = m_configure.box();
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> filtered_cloud_vector;
+    for (int i = 0; i < clouds.size(); ++i)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+        for (int j = 0; j < clouds[i]->size(); ++j)
+        {
+            pcl::PointXYZ pt = clouds[i]->points[j];
+            if (pt.x > box.minx() && pt.x < box.maxx()
+                    && pt.y > box.miny() && pt.y < box.maxy()
+                    && pt.z > box.minz() && pt.z < box.maxz())
+            {
+                cloud_filtered->push_back(pt);
+            }
+        }
+        filtered_cloud_vector.push_back(cloud_filtered);
+    }
+    //根据点云判断雷达状态，若所有雷达滤波后都有点， 则正常,lidar_statu_bit 各个位代表雷达是否有点， 有点为0，无点为1
+    int lidar_statu_bit = 0;
+    for (int i = 0; i < filtered_cloud_vector.size(); ++i)
+    {
+        if (filtered_cloud_vector[i]->size() == 0)
+            lidar_statu_bit |= (0x01 << i);
+        else
+            msg.set_laser_statu_vector(i, message::LASER_READY);//雷达正常
+    }
+    if (lidar_statu_bit == 0)
+    {
+        clouds = filtered_cloud_vector;
+        return SUCCESS;
+    }
+
+    //判断是否有雷达故障
+    for (int i = 0; i < filtered_cloud_vector.size(); ++i)
+    {
+        if (filtered_cloud_vector[i]->size() == 0)
+        {
+            //滤波之前也没有点，雷达故障
+            if (clouds[i]->size() == 0)
+            {
+                clouds = filtered_cloud_vector;
+                msg.set_laser_statu_vector(i, message::LASER_DISCONNECT);
+                return Error_manager(DISCONNECT, NORMAL, "雷达故障");
+            }
+        }
+    }
+    //判断是不是所有雷达在该区域都没有点，表示该区域没有东西
+    int temp = 0;
+    if (~(((~temp) << (filtered_cloud_vector.size())) | lidar_statu_bit) == 0)
+    {
+        clouds = filtered_cloud_vector;
+        return Error_manager(POINT_EMPTY, NORMAL, "区域无点");
+    }
+    else
+    {
+        clouds = filtered_cloud_vector;
+        return Error_manager(CLOUD_INCOMPLETED, NORMAL, "点云不完整(雷达正常)");
+    }
+
+}
+
+
+bool computer_var(std::vector<double> data,double& var)
+{
+    if(data.size()==0)
+        return false;
+    Eigen::VectorXd  dis_vec(data.size());
+    for(int i=0;i<data.size();++i)
+    {
+        dis_vec[i]=data[i];
+    }
+    double mean=dis_vec.mean();
+    Eigen::VectorXd mean_vec(data.size());
+    Eigen::VectorXd mat=dis_vec-(mean_vec.setOnes()*mean);
+    Eigen::MatrixXd result=(mat.transpose())*mat;
+    var=sqrt(result(0)/double(data.size()));
+    return true;
+
+}
+
+
+Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                    const ground_region::Box& box,detect_wheel_ceres3d::Detect_result& last_result)
+{
+    if(cloud->size()==0)
+        return Error_manager(POINT_EMPTY,NORMAL,"no point");
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    for(int i=0;i<cloud->size();++i)
+    {
+        pcl::PointXYZ pt=cloud->points[i];
+        if(pt.x>box.minx()&&pt.x<box.maxx()
+        &&pt.y>box.miny()&&pt.y<box.maxy()
+        &&pt.z>box.minz()&&pt.z<box.maxz())
+        {
+            cloud_filtered->push_back(pt);
+        }
+    }
+    if(cloud_filtered->size()==0)
+        return Error_manager(POINT_EMPTY,NORMAL,"filtered no point");
+
+    float start_z=box.minz();
+    float max_z=0.2;
+    float center_z=(start_z+max_z)/2.0;
+    float last_center_z=start_z;
+    float last_succ_z=-1.0;
+    int count=0;
+    //二分法 找识别成功的 最高的z
+    std::vector<detect_wheel_ceres3d::Detect_result> results;
+    do
+    {
+        detect_wheel_ceres3d::Detect_result result;
+        bool ret = classify_ceres_detect(cloud_filtered, center_z,result);
+        // std::cout << "z: " << center_z <<", "<<start_z<<"," << max_z << std::endl;
+        if(ret)
+        {
+            results.push_back(result);
+            last_succ_z=center_z;
+            start_z=center_z;
+            last_center_z=center_z;
+
+        }
+        else
+        {
+            max_z=center_z;
+            last_center_z=center_z;
+
+        }
+        center_z=(start_z+max_z)/2.0;
+        count++;
+        
+    } while (fabs(center_z - last_center_z) > 0.01);
+
+    //
+    if(results.size()==0)
+    {
+        return Error_manager(FAILED,NORMAL,"nor car");
+    }
+    ///  to be
+    float min_mean_loss=1.0;
+    for(int i=0;i<results.size();++i)
+    {
+        detect_wheel_ceres3d::Detect_result result=results[i];
+        std::vector<double> loss;
+        loss.push_back(result.loss.lf_loss);
+        loss.push_back(result.loss.rf_loss);
+        loss.push_back(result.loss.lb_loss);
+        loss.push_back(result.loss.rb_loss);
+        double mean=(result.loss.lf_loss+result.loss.rf_loss+result.loss.lb_loss+result.loss.rb_loss)/4.0;
+        double var=-1.;
+        computer_var(loss,var);
+        if(mean<min_mean_loss)
+        {
+            last_result=result;
+            min_mean_loss=mean;
+        }
+
+    }
+    printf("z : %.3f  angle : %.3f front : %.3f wheel_base:%.3f,width:%.3f, mean:%.5f\n",
+           center_z,last_result.theta,last_result.front_theta,last_result.wheel_base,last_result.width,
+           min_mean_loss);
+    //m_detector->save_debug_data("/home/zx/zzw/catkin_ws/src/feature_extra/debug");
+    return SUCCESS;
+}
+
+#include "message/message_base.pb.h"
+void Ground_region::thread_measure_func(Ground_region* p)
+{
+    return ;
+    message::Ground_measure_statu_msg msg;
+    message::Base_info base_info;
+    base_info.set_msg_type(message::eGround_measure_statu_msg);
+    base_info.set_sender(message::eEmpty);
+    base_info.set_receiver(message::eEmpty);
+    msg.mutable_base_info()->CopyFrom(base_info);
+    msg.set_ground_statu(message::Nothing);
+    const float fps=10.;
+    //保持 10 fps
+    while(false==p->m_exit_condition.wait_for_millisecond(int(1000./fps)))
+    {
+        for(int i=0;i<p->m_lidars.size();++i)
+        {
+            msg.add_laser_statu_vector(
+                    p->m_lidars[i]->isRun()?message::LASER_READY:message::LASER_DISCONNECT);
+        }
+
+        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
+        Error_manager code=p->sync_capture(clouds,0.5);
+        if(code!=SUCCESS)
+        {
+            LOG(WARNING)<<code.get_error_description();
+            continue;
+        }
+
+        Tick tick;
+        code=p->prehandle_cloud(clouds,msg);
+        if(code!=SUCCESS)
+        {
+            LOG(WARNING)<<code.get_error_description();
+            continue;
+        }
+        detect_wheel_ceres3d::Detect_result result;
+        pcl::PointCloud<pcl::PointXYZ>::Ptr correct_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+        for(int i=0;i<clouds.size();++i)
+        {
+            *correct_cloud+=(*clouds[i]);
+        }
+        code=p->detect(correct_cloud,p->m_configure.box(),result);
+
+        if(tick.tic()>1./fps)
+        {
+            LOG(WARNING)<<" detect fps < capture fps";
+        }
+        if(code!=SUCCESS)
+        {
+            LOG_EVERY_N(INFO,20)<<code.get_error_description();
+            continue;
+        }
+
+        //  to be 发布结果
+        msg.set_ground_statu(message::Car_correct);
+        message::Locate_information locate_information;
+        locate_information.set_locate_x(result.cx);
+        locate_information.set_locate_y(result.cy);
+        locate_information.set_locate_angle(result.theta);
+        locate_information.set_locate_wheel_base(result.wheel_base);
+        locate_information.set_locate_length(result.wheel_base);
+        locate_information.set_locate_wheel_width(result.width);
+        locate_information.set_locate_width(result.body_width);
+        locate_information.set_locate_front_theta(result.front_theta);
+        locate_information.set_locate_correct(true);
+
+        Communication_message c_msg;
+        c_msg.reset(msg.base_info(),msg.SerializeAsString());
+        Publisher::get_instance_pointer()->publish_msg(&c_msg);
+
+
+    }
+}
+
+
+

velodyne_lidar/ground_region.h

@@ -0,0 +1,72 @@
+//
+// Created by zx on 2021/5/20.
+//
+
+#ifndef LIDARMEASURE__GROUD_REGION_HPP_
+#define LIDARMEASURE__GROUD_REGION_HPP_
+
+#include "error_code/error_code.h"
+#include <opencv2/opencv.hpp>
+#include "match3d/detect_wheel_ceres3d.h"
+#include "configure.pb.h"
+#include "vlp16.hpp"
+#include <glog/logging.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+#include <google/protobuf/message.h>
+#include "tool/thread_condition.h"
+using google::protobuf::Message;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include "message/ground_measure_msg.pb.h"
+
+class Ground_region
+{
+ public:
+    Ground_region();
+    ~Ground_region();
+    Error_manager init(std::string configure_file);
+    Error_manager init(ground_region::Configure configure);
+
+    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,const ground_region::Box& box,
+                         detect_wheel_ceres3d::Detect_result& result);
+
+ private:
+    bool read_proto_param(std::string file, ::google::protobuf::Message& parameter);
+    Error_manager sync_capture(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud,double timeout_second=1.0);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr scans2cloud(const velodyne::Frame& frame,const ground_region::Calib_parameter& calib);
+
+    //预处理点云， 并并判断雷达状态
+    Error_manager prehandle_cloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clouds,
+            message::Ground_measure_statu_msg& msg);
+    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double thresh_z,
+                               detect_wheel_ceres3d::Detect_result& result);
+
+    static void thread_measure_func(Ground_region* p);
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);
+    double distance(cv::Point2f p1, cv::Point2f p2);
+    bool isRect(std::vector<cv::Point2f>& points);
+ private:
+
+    std::vector<velodyne::VLP16Capture*>     m_lidars;
+ public:
+    ground_region::Configure                m_configure;
+    detect_wheel_ceres3d*                      m_detector;
+ private:
+    std::thread*                            m_running_thread;
+    Thread_condition                        m_exit_condition;
+
+
+
+};
+
+
+#endif //LIDARMEASURE__GROUD_REGION_HPP_

velodyne_lidar/match3d/common/math.h

@@ -0,0 +1,84 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_COMMON_MATH_H_
+#define CARTOGRAPHER_COMMON_MATH_H_
+
+#include <cmath>
+#include <vector>
+
+#include "Eigen/Core"
+#include "port.h"
+#include "ceres/ceres.h"
+
+namespace common {
+
+// Clamps 'value' to be in the range ['min', 'max'].
+template <typename T>
+T Clamp(const T value, const T min, const T max) {
+  if (value > max) {
+    return max;
+  }
+  if (value < min) {
+    return min;
+  }
+  return value;
+}
+
+// Calculates 'base'^'exponent'.
+template <typename T>
+constexpr T Power(T base, int exponent) {
+  return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
+}
+
+// Calculates a^2.
+template <typename T>
+constexpr T Pow2(T a) {
+  return Power(a, 2);
+}
+
+// Converts from degrees to radians.
+constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
+
+// Converts form radians to degrees.
+constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
+
+// Bring the 'difference' between two angles into [-pi; pi].
+template <typename T>
+T NormalizeAngleDifference(T difference) {
+  const T kPi = T(M_PI);
+  while (difference > kPi) difference -= 2. * kPi;
+  while (difference < -kPi) difference += 2. * kPi;
+  return difference;
+}
+
+template <typename T>
+T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
+  return ceres::atan2(vector.y(), vector.x());
+}
+
+template <typename T>
+inline void QuaternionProduct(const double* const z, const T* const w,
+                              T* const zw) {
+  zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
+  zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
+  zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
+  zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
+}
+
+}  // namespace common
+
+#endif  // CARTOGRAPHER_COMMON_MATH_H_

velodyne_lidar/match3d/common/port.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_COMMON_PORT_H_
+#define CARTOGRAPHER_COMMON_PORT_H_
+
+#include <boost/iostreams/device/back_inserter.hpp>
+#include <boost/iostreams/filter/gzip.hpp>
+#include <boost/iostreams/filtering_stream.hpp>
+#include <cinttypes>
+#include <cmath>
+#include <string>
+
+using int8 = int8_t;
+using int16 = int16_t;
+using int32 = int32_t;
+using int64 = int64_t;
+using uint8 = uint8_t;
+using uint16 = uint16_t;
+using uint32 = uint32_t;
+using uint64 = uint64_t;
+
+namespace common {
+
+inline int RoundToInt(const float x) { return std::lround(x); }
+
+inline int RoundToInt(const double x) { return std::lround(x); }
+
+inline int64 RoundToInt64(const float x) { return std::lround(x); }
+
+inline int64 RoundToInt64(const double x) { return std::lround(x); }
+
+inline void FastGzipString(const std::string& uncompressed,
+                           std::string* compressed) {
+  boost::iostreams::filtering_ostream out;
+  out.push(
+      boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));
+  out.push(boost::iostreams::back_inserter(*compressed));
+  boost::iostreams::write(out,
+                          reinterpret_cast<const char*>(uncompressed.data()),
+                          uncompressed.size());
+}
+
+inline void FastGunzipString(const std::string& compressed,
+                             std::string* decompressed) {
+  boost::iostreams::filtering_ostream out;
+  out.push(boost::iostreams::gzip_decompressor());
+  out.push(boost::iostreams::back_inserter(*decompressed));
+  boost::iostreams::write(out, reinterpret_cast<const char*>(compressed.data()),
+                          compressed.size());
+}
+
+}  // namespace common
+
+#endif  // CARTOGRAPHER_COMMON_PORT_H_

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -0,0 +1,660 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#include "detect_wheel_ceres3d.h"
+#include "interpolated_grid.hpp"
+#include <pcl/common/transforms.h>
+
+class CostFunctor3d
+{
+ private:
+    const InterpolatedProbabilityGrid m_left_interpolated_grid;
+    const InterpolatedProbabilityGrid m_right_interpolated_grid;
+    mutable double m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr;
+    pcl::PointCloud<pcl::PointXYZ> m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ> m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ> m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ> m_right_rear_cloud;     //右后点云
+
+
+ public:
+    CostFunctor3d(pcl::PointCloud<pcl::PointXYZ> left_front, pcl::PointCloud<pcl::PointXYZ> right_front,
+                pcl::PointCloud<pcl::PointXYZ> left_rear, pcl::PointCloud<pcl::PointXYZ> right_rear,
+                  const HybridGrid& left_interpolated_grid,
+                  const HybridGrid& right_interpolated_grid)
+                  :m_left_interpolated_grid(left_interpolated_grid),m_right_interpolated_grid(right_interpolated_grid)
+    {
+        m_left_front_cloud = left_front;
+        m_right_front_cloud = right_front;
+        m_left_rear_cloud = left_rear;
+        m_right_rear_cloud = right_rear;
+        m_costs_lf = 0.0;
+        m_costs_rf = 0.0;
+        m_costs_lr = 0.0;
+        m_costs_rr = 0.0;
+    }
+
+    template<typename T>
+    bool operator()(const T *const variable, T *residual) const
+    {
+        T cx = variable[0];
+        T cy = variable[1];
+        T theta = variable[2];
+        T length = variable[3];
+        T width = variable[4];
+        T theta_front = variable[5];
+
+        //整车旋转
+        Eigen::Rotation2D<T> rotation(theta);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //左前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+        //右前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+        //左后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+        //右后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+
+        //前轮旋转
+        Eigen::Rotation2D<T> rotation_front(theta_front);
+        Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+
+        //左前轮
+        int left_front_num = m_left_front_cloud.size();
+        for (int i = 0; i < m_left_front_cloud.size(); ++i)
+        {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud[i].x) - cx),
+                                               (T(m_left_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            residual[i] = T(1.0) - m_left_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
+                                T(m_left_front_cloud[i].z));
+
+        }
+        //右前轮
+        int right_front_num = m_right_front_cloud.size();
+        for (int i = 0; i < m_right_front_cloud.size(); ++i)
+        {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud[i].x) - cx),
+                                               (T(m_right_front_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+            //旋转
+            const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+
+            residual[left_front_num + i] = T(1.0) - m_right_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
+                                                                                 T(m_right_front_cloud[i].z));
+        }
+        //左后轮
+        int left_rear_num = m_left_rear_cloud.size();
+
+        for (int i = 0; i < m_left_rear_cloud.size(); ++i)
+        {
+            const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud[i].x) - cx),
+                                               (T(m_left_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+
+            residual[left_front_num + right_front_num + i] = T(1.0) -
+                    m_left_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
+                                                                                                  T(m_left_rear_cloud[i].z));
+        }
+
+        //右后轮
+        int right_rear_num = m_right_rear_cloud.size();
+        for (int i = 0; i < m_right_rear_cloud.size(); ++i)
+        {
+            const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud[i].x) - cx),
+                                               (T(m_right_rear_cloud[i].y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+
+            residual[left_front_num + right_front_num + left_rear_num+i] = T(1.0) -
+                    m_right_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
+                                                                  T(m_right_rear_cloud[i].z));
+        }
+
+
+        return true;
+    }
+
+};
+
+
+detect_wheel_ceres3d::detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr left_grid_cloud,
+                                           pcl::PointCloud<pcl::PointXYZ>::Ptr right_grid_cloud)
+{
+    float del_x=0.05;
+    float del_y=0.03;
+    float del_z=0.05;
+    Eigen::Matrix3f delta=Eigen::MatrixXf::Identity(3,3);
+    delta(0,0)=del_x*del_x;
+    delta(1,1)=del_y*del_y;
+    delta(2,2)=del_z*del_z;
+    Eigen::Matrix3f delta_inv=delta.inverse();
+
+
+    Eigen::Vector3f d(0,0,0.02);
+    Eigen::MatrixXf ce=-0.5*((d).transpose()*delta_inv*(d));
+    float cut_p=exp(ce(0,0));
+// std::cout << "1111" << std::endl;
+    float resolution=0.01;
+    mp_left_grid=new HybridGrid(resolution);
+    mp_right_grid=new HybridGrid(resolution);
+// std::cout << "2222" << std::endl;
+    for(int i=0;i<left_grid_cloud->size();++i)
+    {
+        pcl::PointXYZ pt=left_grid_cloud->points[i];
+        Eigen::Vector3f u(pt.x,pt.y,pt.z);
+
+        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_left_grid->resolution())
+        {
+            // std::cout << "aa" << std::endl;
+            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_left_grid->resolution())
+            {
+                // std::cout << "bb" << std::endl;
+                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_left_grid->resolution())
+                {
+                    // std::cout << "cc" << std::endl;
+                    Eigen::Vector3f p(x,y,z);
+                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
+                    float prob=exp(B(0,0));
+                    if(prob>cut_p)
+                        prob=cut_p;
+                    Eigen::Array3i index=mp_left_grid->GetCellIndex(p);
+                    // std::cout << "dd "<< index << std::endl;
+                    if(mp_left_grid->GetProbability(index)<prob){
+                        // std::cout << "e1 "<<index << std::endl;
+                        mp_left_grid->SetProbability(index,prob);
+                        // std::cout << "e2 "<<index << std::endl;
+                    }
+                    // std::cout << "ee "<<index << std::endl;
+
+                }
+            }
+        }
+
+    }
+std::cout << "3333" << std::endl;
+    for(int i=0;i<right_grid_cloud->size();++i)
+    {
+        pcl::PointXYZ pt=right_grid_cloud->points[i];
+        Eigen::Vector3f u(pt.x,pt.y,pt.z);
+
+        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_right_grid->resolution())
+        {
+            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_right_grid->resolution())
+            {
+                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_right_grid->resolution())
+                {
+                    Eigen::Vector3f p(x,y,z);
+                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
+                    float prob=exp(B(0,0));
+                    if(prob>cut_p)
+                        prob=cut_p;
+                    Eigen::Array3i index=mp_right_grid->GetCellIndex(p);
+                    if(mp_right_grid->GetProbability(index)<prob)
+                        mp_right_grid->SetProbability(index,prob);
+
+                }
+            }
+        }
+
+    }
+std::cout << "4444" << std::endl;
+    // save_model("/home/zx/zzw/catkin_ws/src/feature_extra");
+
+}
+
+#include "point_tool.h"
+void detect_wheel_ceres3d::save_model(std::string dir)
+{
+
+    InterpolatedProbabilityGrid inter_grid(*mp_left_grid);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_grid(new pcl::PointCloud<pcl::PointXYZRGB>);
+    for(int x=0;x<mp_left_grid->grid_size();++x)
+    {
+        for(int y=0;y<mp_left_grid->grid_size();++y)
+        {
+            for(int z=0;z<mp_left_grid->grid_size();++z)
+            {
+                Eigen::Array3i index(x-mp_left_grid->grid_size()/2,
+                        y-mp_left_grid->grid_size()/2,z-mp_left_grid->grid_size()/2);
+                Eigen::Vector3f pt=mp_left_grid->GetCenterOfCell(index);
+
+                float prob=inter_grid.GetInterpolatedValue(double(pt[0]),double(pt[1]),double(pt[2]));
+                if(prob>0.01)
+                {
+                    Eigen::Vector3f pt=mp_left_grid->GetCenterOfCell(index);
+                    int rgb=int((prob-0.01)*255);
+                    pcl::PointXYZRGB point;
+                    point.x=pt[0];
+                    point.y=pt[1];
+                    point.z=pt[2];
+                    point.r=rgb;
+                    point.g=0;
+                    point.b=255-rgb;
+                    cloud_grid->push_back(point);
+                }
+            }
+        }
+    }
+
+
+    InterpolatedProbabilityGrid right_grid(*mp_right_grid);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_right_grid(new pcl::PointCloud<pcl::PointXYZRGB>);
+    for(int x=0;x<mp_right_grid->grid_size();++x)
+    {
+        for(int y=0;y<mp_right_grid->grid_size();++y)
+        {
+            for(int z=0;z<mp_right_grid->grid_size();++z)
+            {
+                Eigen::Array3i index(x-mp_right_grid->grid_size()/2,
+                                     y-mp_right_grid->grid_size()/2,z-mp_right_grid->grid_size()/2);
+                Eigen::Vector3f pt=mp_right_grid->GetCenterOfCell(index);
+
+                float prob=right_grid.GetInterpolatedValue(double(pt[0]),double(pt[1]),double(pt[2]));
+                if(prob>0.01)
+                {
+                    Eigen::Vector3f pt=mp_right_grid->GetCenterOfCell(index);
+                    int rgb=int((prob-0.01)*255);
+                    pcl::PointXYZRGB point;
+                    point.x=pt[0];
+                    point.y=pt[1];
+                    point.z=pt[2];
+                    point.r=rgb;
+                    point.g=0;
+                    point.b=255-rgb;
+                    cloud_right_grid->push_back(point);
+                }
+            }
+        }
+    }
+
+    //std::cout<<"  save model :"<<dir<<std::endl;
+    //save_cloud_txt(cloud_grid,dir+"/left_model.txt");
+    //save_cloud_txt(cloud_right_grid,dir+"/right_model.txt");
+}
+
+detect_wheel_ceres3d::~detect_wheel_ceres3d(){
+    delete mp_left_grid;
+    delete mp_right_grid;
+    mp_left_grid= nullptr;
+    mp_right_grid= nullptr;
+}
+
+
+
+bool detect_wheel_ceres3d::detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_vec,
+        Detect_result &detect_result, std::string &error_info)
+{
+    error_info = "";
+    //清理点云
+    m_left_front_cloud.clear();
+    m_right_front_cloud.clear();
+    m_left_rear_cloud.clear();
+    m_right_rear_cloud.clear();
+    //重新计算点云,按方位分割
+    //第一步,计算整体中心,主轴方向
+    pcl::PointCloud<pcl::PointXYZ> cloud_all;
+    for (int i = 0; i < cloud_vec.size(); ++i)
+    {
+        cloud_all += (*cloud_vec[i]);
+    }
+
+    // std::cout<<"cloud size: "<<cloud_all.size()<<std::endl;
+
+    if (cloud_all.size() < 20)
+        return false;
+
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(cloud_all, centroid);
+    double center_x = centroid[0];
+    double center_y = centroid[1];
+    //计算外接旋转矩形
+    std::vector<cv::Point2f> points_cv;
+    for (int i = 0; i < cloud_all.size(); ++i)
+    {
+        points_cv.push_back(cv::Point2f(cloud_all[i].x, cloud_all[i].y));
+    }
+    cv::RotatedRect rotate_rect = cv::minAreaRect(points_cv);
+    //计算旋转矩形与X轴的夹角
+    cv::Point2f vec;
+    cv::Point2f vertice[4];
+    rotate_rect.points(vertice);
+
+    float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
+    float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
+    // 寻找长边，倾角为长边与x轴夹角
+    if (len1 > len2)
+    {
+        vec.x = vertice[0].x - vertice[1].x;
+        vec.y = vertice[0].y - vertice[1].y;
+    }
+    else
+    {
+        vec.x = vertice[1].x - vertice[2].x;
+        vec.y = vertice[1].y - vertice[2].y;
+    }
+    float angle_x = 180.0 / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+    // printf("rect theta: %.3f\n",angle_x);
+    //第二步, 将每份点云旋转回去,计算点云重心所在象限
+    for (int i = 0; i < cloud_vec.size(); ++i)
+    {
+        pcl::PointCloud<pcl::PointXYZ> cloud = (*cloud_vec[i]);
+        Eigen::Affine3f traslation = Eigen::Affine3f::Identity();//初始化变换矩阵为单位矩阵
+        // 平移
+        traslation.translation() << -center_x, -center_y, 0.0;
+        pcl::PointCloud<pcl::PointXYZ> translate_cloud;
+        pcl::transformPointCloud(cloud, translate_cloud, traslation);
+
+        // 旋转; Z 轴上旋转angle_x 弧度,Y轴上旋转0弧度
+        Eigen::Affine3f rotation = Eigen::Affine3f::Identity();
+        rotation.rotate(Eigen::AngleAxisf(-angle_x * M_PI / 180.0, Eigen::Vector3f::UnitZ()));
+
+        pcl::PointCloud<pcl::PointXYZ> transformed_cloud;
+        pcl::transformPointCloud(translate_cloud, transformed_cloud, rotation);
+
+        //计算重心
+        Eigen::Vector4f centroid;
+        pcl::compute3DCentroid(transformed_cloud, centroid);
+        double x = centroid[0];
+        double y = centroid[1];
+        //计算象限
+        if (x > 0 && y > 0)
+        {
+            //第一象限
+            m_left_front_cloud = cloud;
+        }
+        if (x > 0 && y < 0)
+        {
+            //第四象限
+            m_right_front_cloud = cloud;
+        }
+        if (x < 0 && y > 0)
+        {
+            //第二象限
+            m_left_rear_cloud = cloud;
+        }
+        if (x < 0 && y < 0)
+        {
+            //第三象限
+            m_right_rear_cloud = cloud;
+
+        }
+
+    }
+
+    // 多次优化，获取最佳优化结果
+    detect_result.cx = center_x;
+    detect_result.cy = center_y;
+    detect_result.wheel_base=std::max(len1,len2);
+    // 已计算次数，初始传入值正常，之后solve函数会修改初始值，需要恢复角度单位为弧度
+    int calc_count = 0;
+    // double final_theta = 0;
+    // 误差结构体，保存左前、右前、左后、右后、整体平均误差
+    Loss_result loss_result;
+    // 平均误差值，用于获取最小整体平均误差
+    double avg_loss = 100;
+    // 定义图像列数，控制图像大小
+    int map_cols = 800;
+    // 优化后图像行数，用于保存优化后结果图像
+    int optimized_map_rows = 200;
+    // 优化结果
+    bool solve_result = false;
+    double total_solve_time = 0;
+    bool stop_sign = false;
+
+    Detect_result t_detect_result = detect_result;
+    // 寻找最小loss值对应的初始旋转角
+
+    t_detect_result.theta = (-angle_x ) * M_PI / 180.0;
+    Loss_result t_loss_result;
+    t_loss_result.total_avg_loss = 1000;
+    //输出角度已变化，需恢复成弧度
+    if (calc_count > 0)
+    {
+        t_detect_result.front_theta *= (-M_PI) / 180.0;
+    }
+    std::string t_error_info;
+    bool current_result = Solve(t_detect_result, t_loss_result, t_error_info);
+    error_info = t_error_info;
+
+    // std::cout<<"current_result: "<<current_result<<std::endl;
+    if (current_result)
+    {
+            avg_loss = t_loss_result.total_avg_loss;
+            // final_theta = -input_vars[2] * M_PI / 180.0;
+            detect_result = t_detect_result;
+            detect_result.loss = t_loss_result;
+            solve_result = current_result;
+            loss_result = t_loss_result;
+
+    }
+
+    return solve_result;
+
+}
+
+void detect_wheel_ceres3d::get_costs(double* variable,double &lf, double &rf, double &lr, double &rr)
+{
+    double losses[4]={0};
+    double cx = variable[0];
+    double cy = variable[1];
+    double theta = variable[2];
+    double length = variable[3];
+    double width = variable[4];
+    double theta_front = variable[5];
+
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    //左前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+    //右前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+    //左后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+    //右后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+
+    //前轮旋转
+    Eigen::Rotation2D<double> rotation_front(theta_front);
+    Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+
+    InterpolatedProbabilityGrid left_interpolated_grid(*mp_left_grid);
+    InterpolatedProbabilityGrid right_interpolated_grid(*mp_right_grid);
+    //左前轮
+    int left_front_num = m_left_front_cloud.size();
+    m_left_front_transformed_cloud.clear();
+    for (int i = 0; i < m_left_front_cloud.size(); ++i)
+    {
+        const Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - cx),
+                                                (double(m_left_front_cloud[i].y) - cy));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+        //旋转
+        const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+        losses[0] += 1.0 - left_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
+                                                                         double(m_left_front_cloud[i].z));
+        m_left_front_transformed_cloud.push_back(pcl::PointXYZ(point_rotation[0],point_rotation[1],m_left_front_cloud[i].z));
+    }
+    //右前轮
+    int right_front_num = m_right_front_cloud.size();
+    m_right_front_transformed_cloud.clear();
+    for (int i = 0; i < m_right_front_cloud.size(); ++i)
+    {
+        const Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - cx),
+                                                (double(m_right_front_cloud[i].y) - cy));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+        //旋转
+        const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+
+        losses[1]+= 1.0 - right_interpolated_grid.GetInterpolatedValue(point_rotation[0],point_rotation[1],
+                                                                         double(m_right_front_cloud[i].z));
+        m_right_front_transformed_cloud.push_back(pcl::PointXYZ(point_rotation[0],point_rotation[1],
+                                                                double(m_right_front_cloud[i].z)));
+    }
+    //左后轮
+    int left_rear_num = m_left_rear_cloud.size();
+    m_left_rear_transformed_cloud.clear();
+    for (int i = 0; i < m_left_rear_cloud.size(); ++i)
+    {
+        const Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - cx),
+                                                (double(m_left_rear_cloud[i].y) - cy));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+
+        losses[2] += 1.0 -
+                left_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
+                                                              double(m_left_rear_cloud[i].z));
+        m_left_rear_transformed_cloud.push_back(pcl::PointXYZ(tanslate_point[0],tanslate_point[1],
+                                                              double(m_left_rear_cloud[i].z)));
+    }
+
+    //右后轮
+    int right_rear_num = m_right_rear_cloud.size();
+    m_right_rear_transformed_cloud.clear();
+    for (int i = 0; i < m_right_rear_cloud.size(); ++i)
+    {
+        const Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - cx),
+                                                (double(m_right_rear_cloud[i].y) - cy));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+
+        losses[3]+= 1.0 -
+                right_interpolated_grid.GetInterpolatedValue(tanslate_point[0],tanslate_point[1],
+                                                               double(m_right_rear_cloud[i].z));
+        m_right_rear_transformed_cloud.push_back(pcl::PointXYZ(tanslate_point[0],tanslate_point[1],
+                                                               double(m_right_rear_cloud[i].z)));
+    }
+
+    lf=losses[0];
+    rf=losses[1];
+    lr =losses[2];
+    rr =losses[3];
+}
+
+
+void detect_wheel_ceres3d::save_debug_data(std::string dir )
+{
+    save_cloud_txt(m_left_front_transformed_cloud.makeShared(),dir+"/left_front_tranformed.txt");
+    save_cloud_txt(m_right_front_transformed_cloud.makeShared(),dir+"/right_front_tranformed.txt");
+    save_cloud_txt(m_left_rear_transformed_cloud.makeShared(),dir+"/left_rear_tranformed.txt");
+    save_cloud_txt(m_right_rear_transformed_cloud.makeShared(),dir+"/right_rear_tranformed.txt");
+}
+
+bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info)
+{
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
+    double init_wheel_base=2.7;
+    if(detect_result.wheel_base>2.55 && detect_result.wheel_base<3.2)
+        init_wheel_base=detect_result.wheel_base;
+    double init_width=1.85;
+    double init_theta_front=0*M_PI/180.0;
+
+    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front};
+    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    CostFunctor3d *cost_func = new CostFunctor3d(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
+            *mp_left_grid,*mp_right_grid);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor3d, ceres::DYNAMIC, 6>(
+                    cost_func,
+                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+
+
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.use_nonmonotonic_steps=false;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    //options.logging_type = ceres::LoggingType::SILENT;
+    options.max_num_iterations=500;
+    options.num_threads=2;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    double loss=summary.final_cost/(m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size());
+
+    detect_result.cx=variable[0];
+    detect_result.cy=variable[1];
+    detect_result.theta=(-variable[2])*180.0/M_PI;
+    detect_result.wheel_base=variable[3];
+    detect_result.width=variable[4];
+    detect_result.front_theta=-(variable[5]*180.0/M_PI);
+
+    if(detect_result.theta>180.0)
+        detect_result.theta=detect_result.theta-180.0;
+    if(detect_result.theta<0)
+        detect_result.theta+=180.0;
+
+    if(summary.iterations.size()<=1)
+        return false;
+    if(summary.iterations[summary.iterations.size()-1].iteration<=1)
+        return false;
+    /*printf("it : %d ,summary loss: %.5f \n",
+           summary.iterations.size(),loss);*/
+
+    if (detect_result.width < 1.25 || detect_result.width > 2.000 || detect_result.wheel_base > 3.15 || detect_result.wheel_base < 2.200)
+    {
+        error_info.append(std::string("宽度(1.25, 2.00) 轴距(2.20, 3.15): ").append(
+                std::to_string(detect_result.width).append(", ").append(
+                        std::to_string(detect_result.wheel_base)).append("\t")));
+        return false;
+    }
+
+    if(detect_result.theta > 120 || detect_result.theta < 60)
+    {
+        error_info.append("车身角度错误 ").append(std::to_string(detect_result.theta)).append("\t");
+//        LOG(WARNING) <<"总角度错误 "<<detect_result.theta;
+        return false;
+    }
+    if(fabs(detect_result.front_theta)>35)
+    {
+        error_info.append("前轮角度错误 ").append(std::to_string(detect_result.front_theta)).append("\t");
+        return false;
+    }
+    // 将loss传出
+    if(cost_func!=nullptr)
+    {
+        double costs_lf, costs_rf, costs_lr, costs_rr;
+        get_costs(variable,costs_lf, costs_rf, costs_lr, costs_rr);
+
+        loss_result.lf_loss = costs_lf/float(m_left_front_cloud.size()+1e-6);
+        loss_result.rf_loss = costs_rf/float(m_right_front_cloud.size()+1e-6);
+        loss_result.lb_loss = costs_lr/float(m_left_rear_cloud.size()+1e-6);
+        loss_result.rb_loss = costs_rr/float(m_right_rear_cloud.size()+1e-6);
+        loss_result.total_avg_loss = loss;
+        detect_result.loss=loss_result;
+        if(loss_result.lf_loss>0.4 || loss_result.rf_loss>0.4
+        ||loss_result.lb_loss>0.4||loss_result.rb_loss>0.4)
+        {
+            return false;
+        }
+
+    }
+    else
+    {
+        return false;
+    }
+
+    return true;
+}

velodyne_lidar/match3d/detect_wheel_ceres3d.h

@@ -0,0 +1,95 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#ifndef CERES_TEST_DETECT_WHEEL_CERES_H
+#define CERES_TEST_DETECT_WHEEL_CERES_H
+
+#include <ceres/ceres.h>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/centroid.h>
+#include <opencv2/opencv.hpp>
+#include <iostream>
+#include <string>
+#include <chrono>
+#include "boost/format.hpp"
+#include "hybrid_grid.hpp"
+
+class detect_wheel_ceres3d {
+public:
+    struct Loss_result
+    {
+        public: 
+        double lf_loss;
+        double rf_loss;
+        double lb_loss;
+        double rb_loss;
+        double total_avg_loss;
+        Loss_result& operator=(const Loss_result& loss_result)
+        {
+            this->lf_loss = loss_result.lf_loss;
+            this->rf_loss = loss_result.rf_loss;
+            this->lb_loss = loss_result.lb_loss;
+            this->rb_loss = loss_result.rb_loss;
+            this->total_avg_loss = loss_result.total_avg_loss;
+            return *this;
+        }
+    };
+    struct Detect_result
+    {
+     public:
+        double cx;
+        double cy;
+        double theta;
+        double wheel_base;
+        double width;
+        double body_width;
+        double front_theta;
+        bool success;
+        Loss_result loss;
+        Detect_result& operator=(const Detect_result& detect_result)
+        {
+            this->cx = detect_result.cx;
+            this->cy = detect_result.cy;
+            this->theta = detect_result.theta;
+            this->wheel_base = detect_result.wheel_base;
+            this->width = detect_result.width;
+            this->front_theta = detect_result.front_theta;
+            this->success = detect_result.success;
+            this->body_width=detect_result.body_width;
+            this->loss=detect_result.loss;
+            return *this;
+        }
+    };
+    detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr left_grid_cloud,
+                         pcl::PointCloud<pcl::PointXYZ>::Ptr right_grid_cloud);
+    ~detect_wheel_ceres3d();
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_vec, Detect_result &detect_result, std::string &error_info);
+
+    void save_debug_data(std::string file );
+protected:
+    void save_model(std::string file);
+
+    bool Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info);
+    void get_costs(double* variable,double &lf, double &rf, double &lr, double &rr);
+public:
+    pcl::PointCloud<pcl::PointXYZ> m_left_front_transformed_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ> m_right_front_transformed_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ> m_left_rear_transformed_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ> m_right_rear_transformed_cloud;     //右后点云
+
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud;     //右后点云
+ private:
+    HybridGrid*                                  mp_left_grid;
+    HybridGrid*                                  mp_right_grid;
+
+
+};
+
+
+#endif //CERES_TEST_DETECT_WHEEL_CERES_H

velodyne_lidar/match3d/hybrid_grid.hpp

@@ -0,0 +1,621 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_
+#define CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_
+
+#include <array>
+#include <cmath>
+#include <limits>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include "common/math.h"
+#include "common/port.h"
+//#include "probability_values.h"
+#include "glog/logging.h"
+
+
+// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat
+// z-major index.
+inline int ToFlatIndex(const Eigen::Array3i& index, const int bits) {
+  DCHECK((index >= 0).all() && (index < (1 << bits)).all()) << index;
+  return (((index.z() << bits) + index.y()) << bits) + index.x();
+}
+
+// Converts a flat z-major 'index' to a 3-dimensional index with each dimension
+// from 0 to 2^'bits' - 1.
+inline Eigen::Array3i To3DIndex(const int index, const int bits) {
+  DCHECK_LT(index, 1 << (3 * bits));
+  const int mask = (1 << bits) - 1;
+  return Eigen::Array3i(index & mask, (index >> bits) & mask,
+                        (index >> bits) >> bits);
+}
+
+// A function to compare value to the default value. (Allows specializations).
+template <typename TValueType>
+bool IsDefaultValue(const TValueType& v) {
+  return v == TValueType();
+}
+
+// Specialization to compare a std::vector to the default value.
+template <typename TElementType>
+bool IsDefaultValue(const std::vector<TElementType>& v) {
+  return v.empty();
+}
+
+// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of
+// type 'ValueType' in contiguous memory. Indices in each dimension are 0-based.
+
+
+// 包含自定义类型的数组
+template <typename TValueType, int kBits>
+class FlatGrid
+{
+ public:
+    using ValueType = TValueType;
+
+    // Creates a new flat grid with all values being default constructed.
+    FlatGrid()
+    {
+        for (ValueType &value : cells_)
+        {
+            value = ValueType();
+        }
+    }
+
+    FlatGrid(const FlatGrid &) = delete;
+
+    FlatGrid &operator=(const FlatGrid &) = delete;
+
+    // Returns the number of voxels per dimension.
+    static int grid_size()
+    {
+        return 1 << kBits;
+    }
+
+    // Returns the value stored at 'index', each dimension of 'index' being
+    // between 0 and grid_size() - 1.
+    ValueType value(const Eigen::Array3i &index) const
+    {
+        return cells_[ToFlatIndex(index, kBits)];
+    }
+
+    // Returns a pointer to a value to allow changing it.
+    ValueType *mutable_value(const Eigen::Array3i &index)
+    {
+        return &cells_[ToFlatIndex(index, kBits)];
+    }
+
+    // An iterator for iterating over all values not comparing equal to the
+    // default constructed value.
+    class Iterator
+    {
+     public:
+        Iterator() : current_(nullptr), end_(nullptr)
+        {
+        }
+
+        explicit Iterator(const FlatGrid &flat_grid)
+                : current_(flat_grid.cells_.data()),
+                  end_(flat_grid.cells_.data() + flat_grid.cells_.size())
+        {
+            while (!Done() && IsDefaultValue(*current_))
+            {
+                ++current_;
+            }
+        }
+
+        void Next()
+        {
+            DCHECK(!Done());
+            do
+            {
+                ++current_;
+            } while (!Done() && IsDefaultValue(*current_));
+        }
+
+        bool Done() const
+        {
+            return current_ == end_;
+        }
+
+        Eigen::Array3i GetCellIndex() const
+        {
+            DCHECK(!Done());
+            const int index = (1 << (3 * kBits)) - (end_ - current_);
+            return To3DIndex(index, kBits);
+        }
+
+        const ValueType &GetValue() const
+        {
+            DCHECK(!Done());
+            return *current_;
+        }
+
+     private:
+        const ValueType *current_;
+        const ValueType *end_;
+    };
+
+ private:
+    std::array<ValueType, 1 << (3 * kBits)> cells_;
+};
+
+// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type
+// 'WrappedGrid'. Wrapped grids are constructed on first access via
+// 'mutable_value()'.
+// grid 的 grid
+template <typename WrappedGrid, int kBits>
+class NestedGrid
+{
+ public:
+    using ValueType = typename WrappedGrid::ValueType;
+
+    // Returns the number of voxels per dimension.
+    static int grid_size()
+    {
+        return WrappedGrid::grid_size() << kBits;
+    }
+
+    // Returns the value stored at 'index', each dimension of 'index' being
+    // between 0 and grid_size() - 1.
+    ValueType value(const Eigen::Array3i &index) const
+    {
+        const Eigen::Array3i meta_index = GetMetaIndex(index);
+        const WrappedGrid *const meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, kBits)].get();
+        if (meta_cell == nullptr)
+        {
+            return ValueType();
+        }
+        const Eigen::Array3i inner_index =
+                index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->value(inner_index);
+    }
+
+    // Returns a pointer to the value at 'index' to allow changing it. If
+    // necessary a new wrapped grid is constructed to contain that value.
+    ValueType *mutable_value(const Eigen::Array3i &index)
+    {
+        const Eigen::Array3i meta_index = GetMetaIndex(index);
+        std::unique_ptr<WrappedGrid> &meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, kBits)];
+        if (meta_cell == nullptr)
+        {
+            meta_cell = std::unique_ptr<WrappedGrid>(new WrappedGrid);
+        }
+        const Eigen::Array3i inner_index =
+                index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->mutable_value(inner_index);
+    }
+
+    // An iterator for iterating over all values not comparing equal to the
+    // default constructed value.
+    class Iterator
+    {
+     public:
+        Iterator() : current_(nullptr), end_(nullptr), nested_iterator_()
+        {
+        }
+
+        explicit Iterator(const NestedGrid &nested_grid)
+                : current_(nested_grid.meta_cells_.data()),
+                  end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()),
+                  nested_iterator_()
+        {
+            AdvanceToValidNestedIterator();
+        }
+
+        void Next()
+        {
+            DCHECK(!Done());
+            nested_iterator_.Next();
+            if (!nested_iterator_.Done())
+            {
+                return;
+            }
+            ++current_;
+            AdvanceToValidNestedIterator();
+        }
+
+        bool Done() const
+        {
+            return current_ == end_;
+        }
+
+        Eigen::Array3i GetCellIndex() const
+        {
+            DCHECK(!Done());
+            const int index = (1 << (3 * kBits)) - (end_ - current_);
+            return To3DIndex(index, kBits) * WrappedGrid::grid_size() +
+                    nested_iterator_.GetCellIndex();
+        }
+
+        const ValueType &GetValue() const
+        {
+            DCHECK(!Done());
+            return nested_iterator_.GetValue();
+        }
+
+     private:
+        void AdvanceToValidNestedIterator()
+        {
+            for (; !Done(); ++current_)
+            {
+                if (*current_ != nullptr)
+                {
+                    nested_iterator_ = typename WrappedGrid::Iterator(**current_);
+                    if (!nested_iterator_.Done())
+                    {
+                        break;
+                    }
+                }
+            }
+        }
+
+        const std::unique_ptr<WrappedGrid> *current_;
+        const std::unique_ptr<WrappedGrid> *end_;
+        typename WrappedGrid::Iterator nested_iterator_;
+    };
+
+ private:
+    // Returns the Eigen::Array3i (meta) index of the meta cell containing
+    // 'index'.
+    Eigen::Array3i GetMetaIndex(const Eigen::Array3i &index) const
+    {
+        DCHECK((index >= 0).all()) << index;
+        const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
+        DCHECK((meta_index < (1 << kBits)).all()) << index;
+        return meta_index;
+    }
+
+    std::array<std::unique_ptr<WrappedGrid>, 1 << (3 * kBits)> meta_cells_;
+};
+
+// A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped
+// grids are constructed on first access via 'mutable_value()'. If necessary,
+// the grid grows to twice the size in each dimension. The range of indices is
+// (almost) symmetric around the origin, i.e. negative indices are allowed.
+
+
+template <typename WrappedGrid>
+class DynamicGrid
+{
+ public:
+    using ValueType = typename WrappedGrid::ValueType;
+
+    DynamicGrid() : bits_(1), meta_cells_(8)
+    {
+    }
+
+    DynamicGrid(DynamicGrid &&) = default;
+
+    DynamicGrid &operator=(DynamicGrid &&) = default;
+
+    // Returns the current number of voxels per dimension.
+    int grid_size() const
+    {
+        return WrappedGrid::grid_size() << bits_;
+    }
+
+    // Returns the value stored at 'index'.
+    ValueType value(const Eigen::Array3i &index) const
+    {
+        const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
+        // The cast to unsigned is for performance to check with 3 comparisons
+        // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
+        if ((shifted_index.cast<unsigned int>() >= grid_size()).any())
+        {
+            return ValueType();
+        }
+        const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
+        const WrappedGrid *const meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, bits_)].get();
+        if (meta_cell == nullptr)
+        {
+            return ValueType();
+        }
+        const Eigen::Array3i inner_index =
+                shifted_index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->value(inner_index);
+    }
+
+    // Returns a pointer to the value at 'index' to allow changing it, dynamically
+    // growing the DynamicGrid and constructing new WrappedGrids as needed.
+    ValueType *mutable_value(const Eigen::Array3i &index)
+    {
+        const Eigen::Array3i shifted_index = index+ (grid_size() >> 1);
+        // The cast to unsigned is for performance to check with 3 comparisons
+         //shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
+        if ((shifted_index.cast<unsigned int>() >= grid_size()).any())
+        {
+            LOG(ERROR)<<" out of bound";
+            //Grow();
+            return 0;//mutable_value(index);
+        }
+        const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
+        std::unique_ptr<WrappedGrid> &meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, bits_)];
+        if (meta_cell == nullptr)
+        {
+            meta_cell = std::unique_ptr<WrappedGrid>(new WrappedGrid);
+        }
+        const Eigen::Array3i inner_index =
+                shifted_index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->mutable_value(inner_index);
+    }
+
+    // An iterator for iterating over all values not comparing equal to the
+    // default constructed value.
+    class Iterator
+    {
+     public:
+        explicit Iterator(const DynamicGrid &dynamic_grid)
+                : bits_(dynamic_grid.bits_),
+                  current_(dynamic_grid.meta_cells_.data()),
+                  end_(dynamic_grid.meta_cells_.data() +
+                               dynamic_grid.meta_cells_.size()),
+                  nested_iterator_()
+        {
+            AdvanceToValidNestedIterator();
+        }
+
+        void Next()
+        {
+            DCHECK(!Done());
+            nested_iterator_.Next();
+            if (!nested_iterator_.Done())
+            {
+                return;
+            }
+            ++current_;
+            AdvanceToValidNestedIterator();
+        }
+
+        bool Done() const
+        {
+            return current_ == end_;
+        }
+
+        Eigen::Array3i GetCellIndex() const
+        {
+            DCHECK(!Done());
+            const int outer_index = (1 << (3 * bits_)) - (end_ - current_);
+            const Eigen::Array3i shifted_index =
+                    To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() +
+                            nested_iterator_.GetCellIndex();
+            return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size());
+        }
+
+        const ValueType &GetValue() const
+        {
+            DCHECK(!Done());
+            return nested_iterator_.GetValue();
+        }
+
+        void AdvanceToEnd()
+        {
+            current_ = end_;
+        }
+
+        const std::pair<Eigen::Array3i, ValueType> operator*() const
+        {
+            return std::pair<Eigen::Array3i, ValueType>(GetCellIndex(), GetValue());
+        }
+
+        Iterator &operator++()
+        {
+            Next();
+            return *this;
+        }
+
+        bool operator!=(const Iterator &it) const
+        {
+            return it.current_ != current_;
+        }
+
+     private:
+        void AdvanceToValidNestedIterator()
+        {
+            for (; !Done(); ++current_)
+            {
+                if (*current_ != nullptr)
+                {
+                    nested_iterator_ = typename WrappedGrid::Iterator(**current_);
+                    if (!nested_iterator_.Done())
+                    {
+                        break;
+                    }
+                }
+            }
+        }
+
+        int bits_;
+        const std::unique_ptr<WrappedGrid> *current_;
+        const std::unique_ptr<WrappedGrid> *const end_;
+        typename WrappedGrid::Iterator nested_iterator_;
+    };
+
+ private:
+    // Returns the Eigen::Array3i (meta) index of the meta cell containing
+    // 'index'.
+    Eigen::Array3i GetMetaIndex(const Eigen::Array3i &index) const
+    {
+        DCHECK((index >= 0).all()) << index;
+        const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
+        DCHECK((meta_index < (1 << bits_)).all()) << index;
+        return meta_index;
+    }
+
+    // Grows this grid by a factor of 2 in each of the 3 dimensions.
+    void Grow()
+    {
+        const int new_bits = bits_ + 1;
+        CHECK_LE(new_bits, 8);
+        std::vector<std::unique_ptr<WrappedGrid>> new_meta_cells_(
+                8 * meta_cells_.size());
+        for (int z = 0; z != (1 << bits_); ++z)
+        {
+            for (int y = 0; y != (1 << bits_); ++y)
+            {
+                for (int x = 0; x != (1 << bits_); ++x)
+                {
+                    const Eigen::Array3i original_meta_index(x, y, z);
+                    const Eigen::Array3i new_meta_index =
+                            original_meta_index + (1 << (bits_ - 1));
+                    new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] =
+                            std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]);
+                }
+            }
+        }
+        meta_cells_ = std::move(new_meta_cells_);
+        bits_ = new_bits;
+    }
+
+    int bits_;
+    std::vector<std::unique_ptr<WrappedGrid>> meta_cells_;
+};
+
+template <typename ValueType>
+using GridBase = DynamicGrid<NestedGrid<FlatGrid<ValueType, 3>, 3>>;
+
+// Represents a 3D grid as a wide, shallow tree.
+template <typename ValueType>
+class HybridGridBase : public GridBase<ValueType>
+{
+ public:
+    using Iterator = typename GridBase<ValueType>::Iterator;
+
+    // Creates a new tree-based probability grid with voxels having edge length
+    // 'resolution' around the origin which becomes the center of the cell at
+    // index (0, 0, 0).
+    explicit HybridGridBase(const float resolution) : resolution_(resolution)
+    {
+    }
+
+    float resolution() const
+    {
+        return resolution_;
+    }
+
+    // Returns the index of the cell containing the 'point'. Indices are integer
+    // vectors identifying cells, for this the coordinates are rounded to the next
+    // multiple of the resolution.
+    Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
+    {
+        Eigen::Array3f index = point.array() / resolution_;
+        return Eigen::Array3i(common::RoundToInt(index.x()),
+                              common::RoundToInt(index.y()),
+                              common::RoundToInt(index.z()));
+    }
+
+    // Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1).
+    static Eigen::Array3i GetOctant(const int i)
+    {
+        DCHECK_GE(i, 0);
+        DCHECK_LT(i, 8);
+        return Eigen::Array3i(static_cast<bool>(i & 1), static_cast<bool>(i & 2),
+                              static_cast<bool>(i & 4));
+    }
+
+    // Returns the center of the cell at 'index'.
+    Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i &index) const
+    {
+        return index.matrix().cast<float>() * resolution_;
+    }
+
+    // Iterator functions for range-based for loops.
+    Iterator begin() const
+    {
+        return Iterator(*this);
+    }
+
+    Iterator end() const
+    {
+        Iterator it(*this);
+        it.AdvanceToEnd();
+        return it;
+    }
+
+ private:
+    // Edge length of each voxel.
+    const float resolution_;
+};
+
+
+class HybridGrid : public HybridGridBase<float>
+{
+ public:
+    explicit HybridGrid(const float resolution)
+            : HybridGridBase<float>(resolution)
+    {
+    }
+
+
+    // Sets the probability of the cell at 'index' to the given 'probability'.
+    void SetProbability(const Eigen::Array3i &index, const float probability)
+    {
+        *mutable_value(index) = probability;
+    }
+
+
+    // Returns the probability of the cell with 'index'.
+    float GetProbability(const Eigen::Array3i &index) const
+    {
+        return value(index);
+    }
+
+    // Returns true if the probability at the specified 'index' is known.
+    bool IsKnown(const Eigen::Array3i &index) const
+    {
+        return value(index) != 0;
+    }
+
+ private:
+    // Markers at changed cells.
+    std::vector<ValueType *> update_indices_;
+};
+
+/*struct AverageIntensityData {
+  float sum = 0.f;
+  int count = 0;
+};
+
+class IntensityHybridGrid : public HybridGridBase<AverageIntensityData> {
+ public:
+  explicit IntensityHybridGrid(const float resolution)
+      : HybridGridBase<AverageIntensityData>(resolution) {}
+
+  void AddIntensity(const Eigen::Array3i& index, const float intensity) {
+    AverageIntensityData* const cell = mutable_value(index);
+    cell->count += 1;
+    cell->sum += intensity;
+  }
+
+  float GetIntensity(const Eigen::Array3i& index) const {
+    const AverageIntensityData& cell = value(index);
+    if (cell.count == 0) {
+      return 0.f;
+    } else {
+      return cell.sum / cell.count;
+    }
+  }
+};
+*/
+
+#endif  // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_

velodyne_lidar/match3d/interpolated_grid.hpp

@@ -0,0 +1,157 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
+
+#include <cmath>
+
+#include "hybrid_grid.hpp"
+
+
+// Interpolates between HybridGrid voxels. We use the tricubic
+// interpolation which interpolates the values and has vanishing derivative at
+// these points.
+//
+// This class is templated to work with the autodiff that Ceres provides.
+// For this reason, it is also important that the interpolation scheme be
+// continuously differentiable.
+template <class HybridGridType>
+class InterpolatedGrid {
+ public:
+  explicit InterpolatedGrid(const HybridGridType& hybrid_grid)
+      : hybrid_grid_(hybrid_grid) {}
+
+  InterpolatedGrid(const InterpolatedGrid<HybridGridType>&) = delete;
+  InterpolatedGrid& operator=(const InterpolatedGrid<HybridGridType>&) = delete;
+
+  // Returns the interpolated value at (x, y, z) of the HybridGrid
+  // used to perform the interpolation.
+  //
+  // This is a piecewise, continuously differentiable function. We use the
+  // scalar part of Jet parameters to select our interval below. It is the
+  // tensor product volume of piecewise cubic polynomials that interpolate
+  // the values, and have vanishing derivative at the interval boundaries.
+  template <typename T>
+  T GetInterpolatedValue(const T& x, const T& y, const T& z) const {
+    double x1, y1, z1, x2, y2, z2;
+    ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2);
+
+    const Eigen::Array3i index1 =
+        hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1));
+    const double q111 = GetValue(hybrid_grid_, index1);
+    const double q112 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 0, 1));
+    const double q121 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 0));
+    const double q122 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 1));
+    const double q211 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 0));
+    const double q212 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 1));
+    const double q221 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 0));
+    const double q222 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 1));
+
+    const T normalized_x = (x - x1) / (x2 - x1);
+    const T normalized_y = (y - y1) / (y2 - y1);
+    const T normalized_z = (z - z1) / (z2 - z1);
+
+    // Compute pow(..., 2) and pow(..., 3). Using pow() here is very expensive.
+    const T normalized_xx = normalized_x * normalized_x;
+    const T normalized_xxx = normalized_x * normalized_xx;
+    const T normalized_yy = normalized_y * normalized_y;
+    const T normalized_yyy = normalized_y * normalized_yy;
+    const T normalized_zz = normalized_z * normalized_z;
+    const T normalized_zzz = normalized_z * normalized_zz;
+
+    // We first interpolate in z, then y, then x. All 7 times this uses the same
+    // scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2).
+    // The first polynomial is 1 at t=0, 0 at t=1, the second polynomial is 0
+    // at t=0, 1 at t=1. Both polynomials have derivative zero at t=0 and t=1.
+    const T q11 = (q111 - q112) * normalized_zzz * 2. +
+                  (q112 - q111) * normalized_zz * 3. + q111;
+    const T q12 = (q121 - q122) * normalized_zzz * 2. +
+                  (q122 - q121) * normalized_zz * 3. + q121;
+    const T q21 = (q211 - q212) * normalized_zzz * 2. +
+                  (q212 - q211) * normalized_zz * 3. + q211;
+    const T q22 = (q221 - q222) * normalized_zzz * 2. +
+                  (q222 - q221) * normalized_zz * 3. + q221;
+    const T q1 = (q11 - q12) * normalized_yyy * 2. +
+                 (q12 - q11) * normalized_yy * 3. + q11;
+    const T q2 = (q21 - q22) * normalized_yyy * 2. +
+                 (q22 - q21) * normalized_yy * 3. + q21;
+    return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. +
+           q1;
+  }
+
+ private:
+  template <typename T>
+  void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z,
+                                      double* x1, double* y1, double* z1,
+                                      double* x2, double* y2,
+                                      double* z2) const {
+    const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z);
+    *x1 = lower.x();
+    *y1 = lower.y();
+    *z1 = lower.z();
+    *x2 = lower.x() + hybrid_grid_.resolution();
+    *y2 = lower.y() + hybrid_grid_.resolution();
+    *z2 = lower.z() + hybrid_grid_.resolution();
+  }
+
+  // Center of the next lower voxel, i.e., not necessarily the voxel containing
+  // (x, y, z). For each dimension, the largest voxel index so that the
+  // corresponding center is at most the given coordinate.
+  Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y,
+                                     const double z) const {
+    // Center of the cell containing (x, y, z).
+    Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell(
+        hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
+    // Move to the next lower voxel center.
+    if (center.x() > x) {
+      center.x() -= hybrid_grid_.resolution();
+    }
+    if (center.y() > y) {
+      center.y() -= hybrid_grid_.resolution();
+    }
+    if (center.z() > z) {
+      center.z() -= hybrid_grid_.resolution();
+    }
+    return center;
+  }
+
+  // Uses the scalar part of a Ceres Jet.
+  template <typename T>
+  Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y,
+                                     const T& jet_z) const {
+    return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
+  }
+
+  static float GetValue(const HybridGrid& probability_grid,
+                        const Eigen::Array3i& index) {
+    return probability_grid.GetProbability(index);
+  }
+
+  const HybridGridType& hybrid_grid_;
+};
+
+using InterpolatedProbabilityGrid = InterpolatedGrid<HybridGrid>;
+
+
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_

velodyne_lidar/velodyne_config.pb.h

@@ -0,0 +1,742 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: velodyne_config.proto
+
+#ifndef PROTOBUF_velodyne_5fconfig_2eproto__INCLUDED
+#define PROTOBUF_velodyne_5fconfig_2eproto__INCLUDED
+
+#include <string>
+
+#include <google/protobuf/stubs/common.h>
+
+#if GOOGLE_PROTOBUF_VERSION < 3005000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please update
+#error your headers.
+#endif
+#if 3005000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_table_driven.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/metadata.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>  // IWYU pragma: export
+#include <google/protobuf/extension_set.h>  // IWYU pragma: export
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+
+namespace protobuf_velodyne_5fconfig_2eproto {
+// Internal implementation detail -- do not use these members.
+struct TableStruct {
+  static const ::google::protobuf::internal::ParseTableField entries[];
+  static const ::google::protobuf::internal::AuxillaryParseTableField aux[];
+  static const ::google::protobuf::internal::ParseTable schema[2];
+  static const ::google::protobuf::internal::FieldMetadata field_metadata[];
+  static const ::google::protobuf::internal::SerializationTable serialization_table[];
+  static const ::google::protobuf::uint32 offsets[];
+};
+void AddDescriptors();
+void InitDefaultsvelodyneManagerParamsImpl();
+void InitDefaultsvelodyneManagerParams();
+void InitDefaultsvelodyneLidarParamsImpl();
+void InitDefaultsvelodyneLidarParams();
+inline void InitDefaults() {
+  InitDefaultsvelodyneManagerParams();
+  InitDefaultsvelodyneLidarParams();
+}
+}  // namespace protobuf_velodyne_5fconfig_2eproto
+namespace velodyne {
+class velodyneLidarParams;
+class velodyneLidarParamsDefaultTypeInternal;
+extern velodyneLidarParamsDefaultTypeInternal _velodyneLidarParams_default_instance_;
+class velodyneManagerParams;
+class velodyneManagerParamsDefaultTypeInternal;
+extern velodyneManagerParamsDefaultTypeInternal _velodyneManagerParams_default_instance_;
+}  // namespace velodyne
+namespace velodyne {
+
+// ===================================================================
+
+class velodyneManagerParams : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:velodyne.velodyneManagerParams) */ {
+ public:
+  velodyneManagerParams();
+  virtual ~velodyneManagerParams();
+
+  velodyneManagerParams(const velodyneManagerParams& from);
+
+  inline velodyneManagerParams& operator=(const velodyneManagerParams& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  #if LANG_CXX11
+  velodyneManagerParams(velodyneManagerParams&& from) noexcept
+    : velodyneManagerParams() {
+    *this = ::std::move(from);
+  }
+
+  inline velodyneManagerParams& operator=(velodyneManagerParams&& from) noexcept {
+    if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+  #endif
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const velodyneManagerParams& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const velodyneManagerParams* internal_default_instance() {
+    return reinterpret_cast<const velodyneManagerParams*>(
+               &_velodyneManagerParams_default_instance_);
+  }
+  static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
+    0;
+
+  void Swap(velodyneManagerParams* other);
+  friend void swap(velodyneManagerParams& a, velodyneManagerParams& b) {
+    a.Swap(&b);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline velodyneManagerParams* New() const PROTOBUF_FINAL { return New(NULL); }
+
+  velodyneManagerParams* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
+  void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void CopyFrom(const velodyneManagerParams& from);
+  void MergeFrom(const velodyneManagerParams& from);
+  void Clear() PROTOBUF_FINAL;
+  bool IsInitialized() const PROTOBUF_FINAL;
+
+  size_t ByteSizeLong() const PROTOBUF_FINAL;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
+  int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const PROTOBUF_FINAL;
+  void InternalSwap(velodyneManagerParams* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return NULL;
+  }
+  inline void* MaybeArenaPtr() const {
+    return NULL;
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // @@protoc_insertion_point(class_scope:velodyne.velodyneManagerParams)
+ private:
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::internal::HasBits<1> _has_bits_;
+  mutable int _cached_size_;
+  friend struct ::protobuf_velodyne_5fconfig_2eproto::TableStruct;
+  friend void ::protobuf_velodyne_5fconfig_2eproto::InitDefaultsvelodyneManagerParamsImpl();
+};
+// -------------------------------------------------------------------
+
+class velodyneLidarParams : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:velodyne.velodyneLidarParams) */ {
+ public:
+  velodyneLidarParams();
+  virtual ~velodyneLidarParams();
+
+  velodyneLidarParams(const velodyneLidarParams& from);
+
+  inline velodyneLidarParams& operator=(const velodyneLidarParams& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  #if LANG_CXX11
+  velodyneLidarParams(velodyneLidarParams&& from) noexcept
+    : velodyneLidarParams() {
+    *this = ::std::move(from);
+  }
+
+  inline velodyneLidarParams& operator=(velodyneLidarParams&& from) noexcept {
+    if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+  #endif
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const velodyneLidarParams& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const velodyneLidarParams* internal_default_instance() {
+    return reinterpret_cast<const velodyneLidarParams*>(
+               &_velodyneLidarParams_default_instance_);
+  }
+  static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
+    1;
+
+  void Swap(velodyneLidarParams* other);
+  friend void swap(velodyneLidarParams& a, velodyneLidarParams& b) {
+    a.Swap(&b);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline velodyneLidarParams* New() const PROTOBUF_FINAL { return New(NULL); }
+
+  velodyneLidarParams* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
+  void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void CopyFrom(const velodyneLidarParams& from);
+  void MergeFrom(const velodyneLidarParams& from);
+  void Clear() PROTOBUF_FINAL;
+  bool IsInitialized() const PROTOBUF_FINAL;
+
+  size_t ByteSizeLong() const PROTOBUF_FINAL;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
+  int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const PROTOBUF_FINAL;
+  void InternalSwap(velodyneLidarParams* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return NULL;
+  }
+  inline void* MaybeArenaPtr() const {
+    return NULL;
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // required string ip = 1 [default = ""];
+  bool has_ip() const;
+  void clear_ip();
+  static const int kIpFieldNumber = 1;
+  const ::std::string& ip() const;
+  void set_ip(const ::std::string& value);
+  #if LANG_CXX11
+  void set_ip(::std::string&& value);
+  #endif
+  void set_ip(const char* value);
+  void set_ip(const char* value, size_t size);
+  ::std::string* mutable_ip();
+  ::std::string* release_ip();
+  void set_allocated_ip(::std::string* ip);
+
+  // required string model = 3 [default = "VLP16"];
+  bool has_model() const;
+  void clear_model();
+  static const int kModelFieldNumber = 3;
+  const ::std::string& model() const;
+  void set_model(const ::std::string& value);
+  #if LANG_CXX11
+  void set_model(::std::string&& value);
+  #endif
+  void set_model(const char* value);
+  void set_model(const char* value, size_t size);
+  ::std::string* mutable_model();
+  ::std::string* release_model();
+  void set_allocated_model(::std::string* model);
+
+  // required string calibrationFile = 4 [default = ""];
+  bool has_calibrationfile() const;
+  void clear_calibrationfile();
+  static const int kCalibrationFileFieldNumber = 4;
+  const ::std::string& calibrationfile() const;
+  void set_calibrationfile(const ::std::string& value);
+  #if LANG_CXX11
+  void set_calibrationfile(::std::string&& value);
+  #endif
+  void set_calibrationfile(const char* value);
+  void set_calibrationfile(const char* value, size_t size);
+  ::std::string* mutable_calibrationfile();
+  ::std::string* release_calibrationfile();
+  void set_allocated_calibrationfile(::std::string* calibrationfile);
+
+  // optional int32 min_angle = 7 [default = 0];
+  bool has_min_angle() const;
+  void clear_min_angle();
+  static const int kMinAngleFieldNumber = 7;
+  ::google::protobuf::int32 min_angle() const;
+  void set_min_angle(::google::protobuf::int32 value);
+
+  // optional int32 rpm = 9 [default = 600];
+  bool has_rpm() const;
+  void clear_rpm();
+  static const int kRpmFieldNumber = 9;
+  ::google::protobuf::int32 rpm() const;
+  void set_rpm(::google::protobuf::int32 value);
+
+  // required int32 port = 2 [default = 2368];
+  bool has_port() const;
+  void clear_port();
+  static const int kPortFieldNumber = 2;
+  ::google::protobuf::int32 port() const;
+  void set_port(::google::protobuf::int32 value);
+
+  // optional float max_range = 5 [default = 80];
+  bool has_max_range() const;
+  void clear_max_range();
+  static const int kMaxRangeFieldNumber = 5;
+  float max_range() const;
+  void set_max_range(float value);
+
+  // optional float min_range = 6 [default = 0.3];
+  bool has_min_range() const;
+  void clear_min_range();
+  static const int kMinRangeFieldNumber = 6;
+  float min_range() const;
+  void set_min_range(float value);
+
+  // optional int32 max_angle = 8 [default = 360];
+  bool has_max_angle() const;
+  void clear_max_angle();
+  static const int kMaxAngleFieldNumber = 8;
+  ::google::protobuf::int32 max_angle() const;
+  void set_max_angle(::google::protobuf::int32 value);
+
+  // @@protoc_insertion_point(class_scope:velodyne.velodyneLidarParams)
+ private:
+  void set_has_ip();
+  void clear_has_ip();
+  void set_has_port();
+  void clear_has_port();
+  void set_has_model();
+  void clear_has_model();
+  void set_has_calibrationfile();
+  void clear_has_calibrationfile();
+  void set_has_max_range();
+  void clear_has_max_range();
+  void set_has_min_range();
+  void clear_has_min_range();
+  void set_has_min_angle();
+  void clear_has_min_angle();
+  void set_has_max_angle();
+  void clear_has_max_angle();
+  void set_has_rpm();
+  void clear_has_rpm();
+
+  // helper for ByteSizeLong()
+  size_t RequiredFieldsByteSizeFallback() const;
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::internal::HasBits<1> _has_bits_;
+  mutable int _cached_size_;
+  ::google::protobuf::internal::ArenaStringPtr ip_;
+  static ::google::protobuf::internal::ExplicitlyConstructed< ::std::string> _default_model_;
+  ::google::protobuf::internal::ArenaStringPtr model_;
+  ::google::protobuf::internal::ArenaStringPtr calibrationfile_;
+  ::google::protobuf::int32 min_angle_;
+  ::google::protobuf::int32 rpm_;
+  ::google::protobuf::int32 port_;
+  float max_range_;
+  float min_range_;
+  ::google::protobuf::int32 max_angle_;
+  friend struct ::protobuf_velodyne_5fconfig_2eproto::TableStruct;
+  friend void ::protobuf_velodyne_5fconfig_2eproto::InitDefaultsvelodyneLidarParamsImpl();
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#endif  // __GNUC__
+// velodyneManagerParams
+
+// -------------------------------------------------------------------
+
+// velodyneLidarParams
+
+// required string ip = 1 [default = ""];
+inline bool velodyneLidarParams::has_ip() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void velodyneLidarParams::set_has_ip() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void velodyneLidarParams::clear_has_ip() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void velodyneLidarParams::clear_ip() {
+  ip_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  clear_has_ip();
+}
+inline const ::std::string& velodyneLidarParams::ip() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.ip)
+  return ip_.GetNoArena();
+}
+inline void velodyneLidarParams::set_ip(const ::std::string& value) {
+  set_has_ip();
+  ip_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.ip)
+}
+#if LANG_CXX11
+inline void velodyneLidarParams::set_ip(::std::string&& value) {
+  set_has_ip();
+  ip_.SetNoArena(
+    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
+  // @@protoc_insertion_point(field_set_rvalue:velodyne.velodyneLidarParams.ip)
+}
+#endif
+inline void velodyneLidarParams::set_ip(const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  set_has_ip();
+  ip_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
+  // @@protoc_insertion_point(field_set_char:velodyne.velodyneLidarParams.ip)
+}
+inline void velodyneLidarParams::set_ip(const char* value, size_t size) {
+  set_has_ip();
+  ip_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
+      ::std::string(reinterpret_cast<const char*>(value), size));
+  // @@protoc_insertion_point(field_set_pointer:velodyne.velodyneLidarParams.ip)
+}
+inline ::std::string* velodyneLidarParams::mutable_ip() {
+  set_has_ip();
+  // @@protoc_insertion_point(field_mutable:velodyne.velodyneLidarParams.ip)
+  return ip_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline ::std::string* velodyneLidarParams::release_ip() {
+  // @@protoc_insertion_point(field_release:velodyne.velodyneLidarParams.ip)
+  clear_has_ip();
+  return ip_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline void velodyneLidarParams::set_allocated_ip(::std::string* ip) {
+  if (ip != NULL) {
+    set_has_ip();
+  } else {
+    clear_has_ip();
+  }
+  ip_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ip);
+  // @@protoc_insertion_point(field_set_allocated:velodyne.velodyneLidarParams.ip)
+}
+
+// required int32 port = 2 [default = 2368];
+inline bool velodyneLidarParams::has_port() const {
+  return (_has_bits_[0] & 0x00000020u) != 0;
+}
+inline void velodyneLidarParams::set_has_port() {
+  _has_bits_[0] |= 0x00000020u;
+}
+inline void velodyneLidarParams::clear_has_port() {
+  _has_bits_[0] &= ~0x00000020u;
+}
+inline void velodyneLidarParams::clear_port() {
+  port_ = 2368;
+  clear_has_port();
+}
+inline ::google::protobuf::int32 velodyneLidarParams::port() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.port)
+  return port_;
+}
+inline void velodyneLidarParams::set_port(::google::protobuf::int32 value) {
+  set_has_port();
+  port_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.port)
+}
+
+// required string model = 3 [default = "VLP16"];
+inline bool velodyneLidarParams::has_model() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
+}
+inline void velodyneLidarParams::set_has_model() {
+  _has_bits_[0] |= 0x00000002u;
+}
+inline void velodyneLidarParams::clear_has_model() {
+  _has_bits_[0] &= ~0x00000002u;
+}
+inline void velodyneLidarParams::clear_model() {
+  model_.ClearToDefaultNoArena(&::velodyne::velodyneLidarParams::_default_model_.get());
+  clear_has_model();
+}
+inline const ::std::string& velodyneLidarParams::model() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.model)
+  return model_.GetNoArena();
+}
+inline void velodyneLidarParams::set_model(const ::std::string& value) {
+  set_has_model();
+  model_.SetNoArena(&::velodyne::velodyneLidarParams::_default_model_.get(), value);
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.model)
+}
+#if LANG_CXX11
+inline void velodyneLidarParams::set_model(::std::string&& value) {
+  set_has_model();
+  model_.SetNoArena(
+    &::velodyne::velodyneLidarParams::_default_model_.get(), ::std::move(value));
+  // @@protoc_insertion_point(field_set_rvalue:velodyne.velodyneLidarParams.model)
+}
+#endif
+inline void velodyneLidarParams::set_model(const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  set_has_model();
+  model_.SetNoArena(&::velodyne::velodyneLidarParams::_default_model_.get(), ::std::string(value));
+  // @@protoc_insertion_point(field_set_char:velodyne.velodyneLidarParams.model)
+}
+inline void velodyneLidarParams::set_model(const char* value, size_t size) {
+  set_has_model();
+  model_.SetNoArena(&::velodyne::velodyneLidarParams::_default_model_.get(),
+      ::std::string(reinterpret_cast<const char*>(value), size));
+  // @@protoc_insertion_point(field_set_pointer:velodyne.velodyneLidarParams.model)
+}
+inline ::std::string* velodyneLidarParams::mutable_model() {
+  set_has_model();
+  // @@protoc_insertion_point(field_mutable:velodyne.velodyneLidarParams.model)
+  return model_.MutableNoArena(&::velodyne::velodyneLidarParams::_default_model_.get());
+}
+inline ::std::string* velodyneLidarParams::release_model() {
+  // @@protoc_insertion_point(field_release:velodyne.velodyneLidarParams.model)
+  clear_has_model();
+  return model_.ReleaseNoArena(&::velodyne::velodyneLidarParams::_default_model_.get());
+}
+inline void velodyneLidarParams::set_allocated_model(::std::string* model) {
+  if (model != NULL) {
+    set_has_model();
+  } else {
+    clear_has_model();
+  }
+  model_.SetAllocatedNoArena(&::velodyne::velodyneLidarParams::_default_model_.get(), model);
+  // @@protoc_insertion_point(field_set_allocated:velodyne.velodyneLidarParams.model)
+}
+
+// required string calibrationFile = 4 [default = ""];
+inline bool velodyneLidarParams::has_calibrationfile() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+inline void velodyneLidarParams::set_has_calibrationfile() {
+  _has_bits_[0] |= 0x00000004u;
+}
+inline void velodyneLidarParams::clear_has_calibrationfile() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+inline void velodyneLidarParams::clear_calibrationfile() {
+  calibrationfile_.ClearToEmptyNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+  clear_has_calibrationfile();
+}
+inline const ::std::string& velodyneLidarParams::calibrationfile() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.calibrationFile)
+  return calibrationfile_.GetNoArena();
+}
+inline void velodyneLidarParams::set_calibrationfile(const ::std::string& value) {
+  set_has_calibrationfile();
+  calibrationfile_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), value);
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.calibrationFile)
+}
+#if LANG_CXX11
+inline void velodyneLidarParams::set_calibrationfile(::std::string&& value) {
+  set_has_calibrationfile();
+  calibrationfile_.SetNoArena(
+    &::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::move(value));
+  // @@protoc_insertion_point(field_set_rvalue:velodyne.velodyneLidarParams.calibrationFile)
+}
+#endif
+inline void velodyneLidarParams::set_calibrationfile(const char* value) {
+  GOOGLE_DCHECK(value != NULL);
+  set_has_calibrationfile();
+  calibrationfile_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), ::std::string(value));
+  // @@protoc_insertion_point(field_set_char:velodyne.velodyneLidarParams.calibrationFile)
+}
+inline void velodyneLidarParams::set_calibrationfile(const char* value, size_t size) {
+  set_has_calibrationfile();
+  calibrationfile_.SetNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(),
+      ::std::string(reinterpret_cast<const char*>(value), size));
+  // @@protoc_insertion_point(field_set_pointer:velodyne.velodyneLidarParams.calibrationFile)
+}
+inline ::std::string* velodyneLidarParams::mutable_calibrationfile() {
+  set_has_calibrationfile();
+  // @@protoc_insertion_point(field_mutable:velodyne.velodyneLidarParams.calibrationFile)
+  return calibrationfile_.MutableNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline ::std::string* velodyneLidarParams::release_calibrationfile() {
+  // @@protoc_insertion_point(field_release:velodyne.velodyneLidarParams.calibrationFile)
+  clear_has_calibrationfile();
+  return calibrationfile_.ReleaseNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited());
+}
+inline void velodyneLidarParams::set_allocated_calibrationfile(::std::string* calibrationfile) {
+  if (calibrationfile != NULL) {
+    set_has_calibrationfile();
+  } else {
+    clear_has_calibrationfile();
+  }
+  calibrationfile_.SetAllocatedNoArena(&::google::protobuf::internal::GetEmptyStringAlreadyInited(), calibrationfile);
+  // @@protoc_insertion_point(field_set_allocated:velodyne.velodyneLidarParams.calibrationFile)
+}
+
+// optional float max_range = 5 [default = 80];
+inline bool velodyneLidarParams::has_max_range() const {
+  return (_has_bits_[0] & 0x00000040u) != 0;
+}
+inline void velodyneLidarParams::set_has_max_range() {
+  _has_bits_[0] |= 0x00000040u;
+}
+inline void velodyneLidarParams::clear_has_max_range() {
+  _has_bits_[0] &= ~0x00000040u;
+}
+inline void velodyneLidarParams::clear_max_range() {
+  max_range_ = 80;
+  clear_has_max_range();
+}
+inline float velodyneLidarParams::max_range() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.max_range)
+  return max_range_;
+}
+inline void velodyneLidarParams::set_max_range(float value) {
+  set_has_max_range();
+  max_range_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.max_range)
+}
+
+// optional float min_range = 6 [default = 0.3];
+inline bool velodyneLidarParams::has_min_range() const {
+  return (_has_bits_[0] & 0x00000080u) != 0;
+}
+inline void velodyneLidarParams::set_has_min_range() {
+  _has_bits_[0] |= 0x00000080u;
+}
+inline void velodyneLidarParams::clear_has_min_range() {
+  _has_bits_[0] &= ~0x00000080u;
+}
+inline void velodyneLidarParams::clear_min_range() {
+  min_range_ = 0.3f;
+  clear_has_min_range();
+}
+inline float velodyneLidarParams::min_range() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.min_range)
+  return min_range_;
+}
+inline void velodyneLidarParams::set_min_range(float value) {
+  set_has_min_range();
+  min_range_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.min_range)
+}
+
+// optional int32 min_angle = 7 [default = 0];
+inline bool velodyneLidarParams::has_min_angle() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
+}
+inline void velodyneLidarParams::set_has_min_angle() {
+  _has_bits_[0] |= 0x00000008u;
+}
+inline void velodyneLidarParams::clear_has_min_angle() {
+  _has_bits_[0] &= ~0x00000008u;
+}
+inline void velodyneLidarParams::clear_min_angle() {
+  min_angle_ = 0;
+  clear_has_min_angle();
+}
+inline ::google::protobuf::int32 velodyneLidarParams::min_angle() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.min_angle)
+  return min_angle_;
+}
+inline void velodyneLidarParams::set_min_angle(::google::protobuf::int32 value) {
+  set_has_min_angle();
+  min_angle_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.min_angle)
+}
+
+// optional int32 max_angle = 8 [default = 360];
+inline bool velodyneLidarParams::has_max_angle() const {
+  return (_has_bits_[0] & 0x00000100u) != 0;
+}
+inline void velodyneLidarParams::set_has_max_angle() {
+  _has_bits_[0] |= 0x00000100u;
+}
+inline void velodyneLidarParams::clear_has_max_angle() {
+  _has_bits_[0] &= ~0x00000100u;
+}
+inline void velodyneLidarParams::clear_max_angle() {
+  max_angle_ = 360;
+  clear_has_max_angle();
+}
+inline ::google::protobuf::int32 velodyneLidarParams::max_angle() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.max_angle)
+  return max_angle_;
+}
+inline void velodyneLidarParams::set_max_angle(::google::protobuf::int32 value) {
+  set_has_max_angle();
+  max_angle_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.max_angle)
+}
+
+// optional int32 rpm = 9 [default = 600];
+inline bool velodyneLidarParams::has_rpm() const {
+  return (_has_bits_[0] & 0x00000010u) != 0;
+}
+inline void velodyneLidarParams::set_has_rpm() {
+  _has_bits_[0] |= 0x00000010u;
+}
+inline void velodyneLidarParams::clear_has_rpm() {
+  _has_bits_[0] &= ~0x00000010u;
+}
+inline void velodyneLidarParams::clear_rpm() {
+  rpm_ = 600;
+  clear_has_rpm();
+}
+inline ::google::protobuf::int32 velodyneLidarParams::rpm() const {
+  // @@protoc_insertion_point(field_get:velodyne.velodyneLidarParams.rpm)
+  return rpm_;
+}
+inline void velodyneLidarParams::set_rpm(::google::protobuf::int32 value) {
+  set_has_rpm();
+  rpm_ = value;
+  // @@protoc_insertion_point(field_set:velodyne.velodyneLidarParams.rpm)
+}
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic pop
+#endif  // __GNUC__
+// -------------------------------------------------------------------
+
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace velodyne
+
+// @@protoc_insertion_point(global_scope)
+
+#endif  // PROTOBUF_velodyne_5fconfig_2eproto__INCLUDED

velodyne_lidar/velodyne_config.proto

@@ -0,0 +1,20 @@
+syntax = "proto2";
+package velodyne;
+
+message velodyneManagerParams
+{
+
+}
+
+message velodyneLidarParams
+{
+    required string ip=1[default=""];
+    required int32 port=2[default=2368];
+    required string model=3[default="VLP16"];
+    required string calibrationFile=4[default=""];
+    optional float max_range=5[default=10.0];
+    optional float min_range=6[default=0.15];
+    optional int32 min_angle=7[default=0];
+    optional int32 max_angle=8[default=360];
+    optional int32 rpm=9[default=600];
+}

velodyne_lidar/velodyne_driver/angles.h

@@ -0,0 +1,284 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef GEOMETRY_ANGLES_UTILS_H
+#define GEOMETRY_ANGLES_UTILS_H
+
+#include <algorithm>
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif // !M_PI
+
+
+namespace angles
+{
+
+  /*!
+   * \brief Convert degrees to radians
+   */
+
+  static inline double from_degrees(double degrees)
+  {
+    return degrees * M_PI / 180.0;
+  }
+
+  /*!
+   * \brief Convert radians to degrees
+   */
+  static inline double to_degrees(double radians)
+  {
+    return radians * 180.0 / M_PI;
+  }
+
+
+  /*!
+   * \brief normalize_angle_positive
+   *
+   *        Normalizes the angle to be 0 to 2*M_PI
+   *        It takes and returns radians.
+   */
+  static inline double normalize_angle_positive(double angle)
+  {
+    return fmod(fmod(angle, 2.0*M_PI) + 2.0*M_PI, 2.0*M_PI);
+  }
+
+
+  /*!
+   * \brief normalize
+   *
+   * Normalizes the angle to be -M_PI circle to +M_PI circle
+   * It takes and returns radians.
+   *
+   */
+  static inline double normalize_angle(double angle)
+  {
+    double a = normalize_angle_positive(angle);
+    if (a > M_PI)
+      a -= 2.0 *M_PI;
+    return a;
+  }
+
+
+  /*!
+   * \function
+   * \brief shortest_angular_distance
+   *
+   * Given 2 angles, this returns the shortest angular
+   * difference.  The inputs and ouputs are of course radians.
+   *
+   * The result
+   * would always be -pi <= result <= pi.  Adding the result
+   * to "from" will always get you an equivelent angle to "to".
+   */
+
+  static inline double shortest_angular_distance(double from, double to)
+  {
+    return normalize_angle(to-from);
+  }
+
+  /*!
+   * \function
+   *
+   * \brief returns the angle in [-2*M_PI, 2*M_PI]  going the other way along the unit circle.
+   * \param angle The angle to which you want to turn in the range [-2*M_PI, 2*M_PI]
+   * E.g. two_pi_complement(-M_PI/4) returns 7_M_PI/4
+   * two_pi_complement(M_PI/4) returns -7*M_PI/4
+   *
+   */
+  static inline double two_pi_complement(double angle)
+  {
+    //check input conditions
+    if (angle > 2*M_PI || angle < -2.0*M_PI)
+      angle = fmod(angle, 2.0*M_PI);
+    if(angle < 0)
+      return (2*M_PI+angle);
+    else if (angle > 0)
+      return (-2*M_PI+angle);
+
+    return(2*M_PI);
+  }
+
+  /*!
+   * \function
+   *
+   * \brief This function is only intended for internal use and not intended for external use. If you do use it, read the documentation very carefully. Returns the min and max amount (in radians) that can be moved from "from" angle to "left_limit" and "right_limit".
+   * \return returns false if "from" angle does not lie in the interval [left_limit,right_limit]
+   * \param from - "from" angle - must lie in [-M_PI, M_PI)
+   * \param left_limit - left limit of valid interval for angular position - must lie in [-M_PI, M_PI], left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
+   * \param right_limit - right limit of valid interval for angular position - must lie in [-M_PI, M_PI], left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
+   * \param result_min_delta - minimum (delta) angle (in radians) that can be moved from "from" position before hitting the joint stop
+   * \param result_max_delta - maximum (delta) angle (in radians) that can be movedd from "from" position before hitting the joint stop
+   */
+  static bool find_min_max_delta(double from, double left_limit, double right_limit, double &result_min_delta, double &result_max_delta)
+  {
+    double delta[4];
+
+    delta[0] = shortest_angular_distance(from,left_limit);
+    delta[1] = shortest_angular_distance(from,right_limit);
+
+    delta[2] = two_pi_complement(delta[0]);
+    delta[3] = two_pi_complement(delta[1]);
+
+    if(delta[0] == 0)
+    {
+      result_min_delta = delta[0];
+      result_max_delta = std::max<double>(delta[1],delta[3]);
+      return true;
+    }
+
+    if(delta[1] == 0)
+    {
+        result_max_delta = delta[1];
+        result_min_delta = std::min<double>(delta[0],delta[2]);
+        return true;
+    }
+
+
+    double delta_min = delta[0];
+    double delta_min_2pi = delta[2];
+    if(delta[2] < delta_min)
+    {
+      delta_min = delta[2];
+      delta_min_2pi = delta[0];
+    }
+
+    double delta_max = delta[1];
+    double delta_max_2pi = delta[3];
+    if(delta[3] > delta_max)
+    {
+      delta_max = delta[3];
+      delta_max_2pi = delta[1];
+    }
+
+
+    //    printf("%f %f %f %f\n",delta_min,delta_min_2pi,delta_max,delta_max_2pi);
+    if((delta_min <= delta_max_2pi) || (delta_max >= delta_min_2pi))
+    {
+      result_min_delta = delta_max_2pi;
+      result_max_delta = delta_min_2pi;
+      if(left_limit == -M_PI && right_limit == M_PI)
+        return true;
+      else
+        return false;
+    }
+    result_min_delta = delta_min;
+    result_max_delta = delta_max;
+    return true;
+  }
+
+
+  /*!
+   * \function
+   *
+   * \brief Returns the delta from "from_angle" to "to_angle" making sure it does not violate limits specified by left_limit and right_limit.
+   * The valid interval of angular positions is [left_limit,right_limit]. E.g., [-0.25,0.25] is a 0.5 radians wide interval that contains 0.
+   * But [0.25,-0.25] is a 2*M_PI-0.5 wide interval that contains M_PI (but not 0).
+   * The value of shortest_angle is the angular difference between "from" and "to" that lies within the defined valid interval.
+   * E.g. shortest_angular_distance_with_limits(-0.5,0.5,0.25,-0.25,ss) evaluates ss to 2*M_PI-1.0 and returns true while
+   * shortest_angular_distance_with_limits(-0.5,0.5,-0.25,0.25,ss) returns false since -0.5 and 0.5 do not lie in the interval [-0.25,0.25]
+   *
+   * \return true if "from" and "to" positions are within the limit interval, false otherwise
+   * \param from - "from" angle
+   * \param to - "to" angle
+   * \param left_limit - left limit of valid interval for angular position, left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
+   * \param right_limit - right limit of valid interval for angular position, left and right limits are specified on the unit circle w.r.t to a reference pointing inwards
+   * \param shortest_angle - result of the shortest angle calculation
+   */
+  static inline bool shortest_angular_distance_with_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
+  {
+
+    double min_delta = -2*M_PI;
+    double max_delta = 2*M_PI;
+    double min_delta_to = -2*M_PI;
+    double max_delta_to = 2*M_PI;
+    bool flag    = find_min_max_delta(from,left_limit,right_limit,min_delta,max_delta);
+    double delta = shortest_angular_distance(from,to);
+    double delta_mod_2pi  = two_pi_complement(delta);
+
+
+    if(flag)//from position is within the limits
+    {
+      if(delta >= min_delta && delta <= max_delta)
+      {
+        shortest_angle = delta;
+        return true;
+      }
+      else if(delta_mod_2pi >= min_delta && delta_mod_2pi <= max_delta)
+      {
+        shortest_angle = delta_mod_2pi;
+        return true;
+      }
+      else //to position is outside the limits
+      {
+        find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
+          if(fabs(min_delta_to) < fabs(max_delta_to))
+            shortest_angle = std::max<double>(delta,delta_mod_2pi);
+          else if(fabs(min_delta_to) > fabs(max_delta_to))
+            shortest_angle =  std::min<double>(delta,delta_mod_2pi);
+          else
+          {
+            if (fabs(delta) < fabs(delta_mod_2pi))
+              shortest_angle = delta;
+            else
+              shortest_angle = delta_mod_2pi;
+          }
+          return false;
+      }
+    }
+    else // from position is outside the limits
+    {
+        find_min_max_delta(to,left_limit,right_limit,min_delta_to,max_delta_to);
+
+          if(fabs(min_delta) < fabs(max_delta))
+            shortest_angle = std::min<double>(delta,delta_mod_2pi);
+          else if (fabs(min_delta) > fabs(max_delta))
+            shortest_angle =  std::max<double>(delta,delta_mod_2pi);
+          else
+          {
+            if (fabs(delta) < fabs(delta_mod_2pi))
+              shortest_angle = delta;
+            else
+              shortest_angle = delta_mod_2pi;
+          }
+      return false;
+    }
+
+    shortest_angle = delta;
+    return false;
+  }
+}
+
+#endif

velodyne_lidar/velodyne_driver/calibration.cc

@@ -0,0 +1,282 @@
+/**
+ * \file  calibration.cc
+ * \brief  
+ *
+ * \author  Piyush Khandelwal (piyushk@cs.utexas.edu)
+ * Copyright (C) 2012, Austin Robot Technology,
+ *                     The University of Texas at Austin
+ *
+ * License: Modified BSD License
+ *
+ * $ Id: 02/14/2012 11:36:36 AM piyushk $
+ */
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <cmath>
+#include <limits>
+#include <yaml-cpp/yaml.h>
+
+#ifdef HAVE_NEW_YAMLCPP
+namespace YAML {
+
+  // The >> operator disappeared in yaml-cpp 0.5, so this function is
+  // added to provide support for code written under the yaml-cpp 0.3 API.
+  template<typename T>
+  void operator >> (const YAML::Node& node, T& i) {
+    i = node.as<T>();
+  }
+} /* YAML */
+#endif // HAVE_NEW_YAMLCPP
+
+#include "calibration.h"
+
+namespace velodyne_pointcloud 
+{
+
+  const std::string NUM_LASERS = "num_lasers";
+  const std::string DISTANCE_RESOLUTION = "distance_resolution";
+  const std::string LASERS = "lasers";
+  const std::string LASER_ID = "laser_id";
+  const std::string ROT_CORRECTION = "rot_correction";
+  const std::string VERT_CORRECTION = "vert_correction";
+   const std::string DIST_CORRECTION = "dist_correction";
+  const std::string TWO_PT_CORRECTION_AVAILABLE =
+    "two_pt_correction_available";
+  const std::string DIST_CORRECTION_X = "dist_correction_x";
+  const std::string DIST_CORRECTION_Y = "dist_correction_y";
+  const std::string VERT_OFFSET_CORRECTION = "vert_offset_correction";
+  const std::string HORIZ_OFFSET_CORRECTION = "horiz_offset_correction";
+  const std::string MAX_INTENSITY = "max_intensity";
+  const std::string MIN_INTENSITY = "min_intensity";
+  const std::string FOCAL_DISTANCE = "focal_distance";
+  const std::string FOCAL_SLOPE = "focal_slope";
+
+  /** Read calibration for a single laser. */
+  void operator >> (const YAML::Node& node,
+                    std::pair<int, LaserCorrection>& correction)
+  {
+    node[LASER_ID] >> correction.first;
+    node[ROT_CORRECTION] >> correction.second.rot_correction;
+    node[VERT_CORRECTION] >> correction.second.vert_correction;
+    node[DIST_CORRECTION] >> correction.second.dist_correction;
+#ifdef HAVE_NEW_YAMLCPP
+    if (node[TWO_PT_CORRECTION_AVAILABLE])
+      node[TWO_PT_CORRECTION_AVAILABLE] >>
+        correction.second.two_pt_correction_available;
+#else
+    if (const YAML::Node *pName = node.FindValue(TWO_PT_CORRECTION_AVAILABLE))
+      *pName >> correction.second.two_pt_correction_available;
+#endif
+    else
+      correction.second.two_pt_correction_available = false;
+    node[DIST_CORRECTION_X] >> correction.second.dist_correction_x;
+    node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y;
+    node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction;
+#ifdef HAVE_NEW_YAMLCPP
+    if (node[HORIZ_OFFSET_CORRECTION])
+      node[HORIZ_OFFSET_CORRECTION] >>
+        correction.second.horiz_offset_correction;
+#else
+    if (const YAML::Node *pName = node.FindValue(HORIZ_OFFSET_CORRECTION))
+      *pName >> correction.second.horiz_offset_correction;
+#endif
+    else
+      correction.second.horiz_offset_correction = 0;
+
+    const YAML::Node * max_intensity_node = NULL;
+#ifdef HAVE_NEW_YAMLCPP
+    if (node[MAX_INTENSITY]) {
+      const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY];
+      max_intensity_node = &max_intensity_node_ref;
+    }
+#else
+    if (const YAML::Node *pName = node.FindValue(MAX_INTENSITY))
+      max_intensity_node = pName;
+#endif
+    if (max_intensity_node) {
+      float max_intensity_float;
+      *max_intensity_node >> max_intensity_float;
+      correction.second.max_intensity = floor(max_intensity_float);
+    }
+    else {
+      correction.second.max_intensity = 255;
+    }
+
+    const YAML::Node * min_intensity_node = NULL;
+#ifdef HAVE_NEW_YAMLCPP
+    if (node[MIN_INTENSITY]) {
+      const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY];
+      min_intensity_node = &min_intensity_node_ref;
+    }
+#else
+    if (const YAML::Node *pName = node.FindValue(MIN_INTENSITY))
+      min_intensity_node = pName;
+#endif
+    if (min_intensity_node) {
+      float min_intensity_float;
+      *min_intensity_node >> min_intensity_float;
+      correction.second.min_intensity = floor(min_intensity_float);
+    }
+    else {
+      correction.second.min_intensity = 0;
+    }
+    node[FOCAL_DISTANCE] >> correction.second.focal_distance;
+    node[FOCAL_SLOPE] >> correction.second.focal_slope;
+
+    // Calculate cached values
+    correction.second.cos_rot_correction =
+      cosf(correction.second.rot_correction);
+    correction.second.sin_rot_correction =
+      sinf(correction.second.rot_correction);
+    correction.second.cos_vert_correction =
+      cosf(correction.second.vert_correction);
+    correction.second.sin_vert_correction =
+      sinf(correction.second.vert_correction);
+
+    correction.second.laser_ring = 0;   // clear initially (set later)
+  }
+
+  /** Read entire calibration file. */
+  void operator >> (const YAML::Node& node, Calibration& calibration) 
+  {
+    int num_lasers;
+    node[NUM_LASERS] >> num_lasers;
+    float distance_resolution_m;
+    node[DISTANCE_RESOLUTION] >> distance_resolution_m;
+    const YAML::Node& lasers = node[LASERS];
+    calibration.laser_corrections.clear();
+    calibration.num_lasers = num_lasers;
+    calibration.distance_resolution_m = distance_resolution_m;
+    calibration.laser_corrections.resize(num_lasers);
+    for (int i = 0; i < num_lasers; i++) {
+      std::pair<int, LaserCorrection> correction;
+      lasers[i] >> correction;
+      const int index = correction.first;
+      if( index >= calibration.laser_corrections.size() )
+      {
+        calibration.laser_corrections.resize( index+1 );
+      }
+      calibration.laser_corrections[index] = (correction.second);
+      calibration.laser_corrections_map.insert(correction);
+    }
+
+    // For each laser ring, find the next-smallest vertical angle.
+    //
+    // This implementation is simple, but not efficient.  That is OK,
+    // since it only runs while starting up.
+    double next_angle = -std::numeric_limits<double>::infinity();
+    for (int ring = 0; ring < num_lasers; ++ring) {
+
+      // find minimum remaining vertical offset correction
+      double min_seen = std::numeric_limits<double>::infinity();
+      int next_index = num_lasers;
+      for (int j = 0; j < num_lasers; ++j) {
+
+        double angle = calibration.laser_corrections[j].vert_correction;
+        if (next_angle < angle && angle < min_seen) {
+          min_seen = angle;
+          next_index = j;
+        }
+      }
+
+      if (next_index < num_lasers) {    // anything found in this ring?
+
+        // store this ring number with its corresponding laser number
+        calibration.laser_corrections[next_index].laser_ring = ring;
+        next_angle = min_seen;
+        // if (calibration.ros_info) {
+        //   printf("laser_ring[%2u] = %2u, angle = %+.6f",
+        //            next_index, ring, next_angle);
+        // }
+      }
+    }
+  }
+
+  YAML::Emitter& operator << (YAML::Emitter& out,
+                              const std::pair<int, LaserCorrection> correction)
+  {
+    out << YAML::BeginMap;
+    out << YAML::Key << LASER_ID << YAML::Value << correction.first;
+    out << YAML::Key << ROT_CORRECTION <<
+      YAML::Value << correction.second.rot_correction;
+    out << YAML::Key << VERT_CORRECTION <<
+      YAML::Value << correction.second.vert_correction;
+    out << YAML::Key << DIST_CORRECTION <<
+      YAML::Value << correction.second.dist_correction;
+    out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE <<
+      YAML::Value << correction.second.two_pt_correction_available;
+    out << YAML::Key << DIST_CORRECTION_X <<
+      YAML::Value << correction.second.dist_correction_x;
+    out << YAML::Key << DIST_CORRECTION_Y <<
+      YAML::Value << correction.second.dist_correction_y;
+    out << YAML::Key << VERT_OFFSET_CORRECTION <<
+      YAML::Value << correction.second.vert_offset_correction;
+    out << YAML::Key << HORIZ_OFFSET_CORRECTION <<
+      YAML::Value << correction.second.horiz_offset_correction;
+    out << YAML::Key << MAX_INTENSITY <<
+      YAML::Value << correction.second.max_intensity;
+    out << YAML::Key << MIN_INTENSITY <<
+      YAML::Value << correction.second.min_intensity;
+    out << YAML::Key << FOCAL_DISTANCE <<
+      YAML::Value << correction.second.focal_distance;
+    out << YAML::Key << FOCAL_SLOPE <<
+      YAML::Value << correction.second.focal_slope;
+    out << YAML::EndMap;
+    return out;
+  }
+
+  YAML::Emitter& operator <<
+  (YAML::Emitter& out, const Calibration& calibration)
+  {
+    out << YAML::BeginMap;
+    out << YAML::Key << NUM_LASERS <<
+      YAML::Value << calibration.laser_corrections.size();
+    out << YAML::Key << DISTANCE_RESOLUTION <<
+      YAML::Value << calibration.distance_resolution_m;
+    out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
+    for (std::map<int, LaserCorrection>::const_iterator
+           it = calibration.laser_corrections_map.begin();
+         it != calibration.laser_corrections_map.end(); it++)
+      {
+        out << *it; 
+      }
+    out << YAML::EndSeq;
+    out << YAML::EndMap;
+    return out;
+  }
+
+  void Calibration::read(const std::string& calibration_file) {
+    std::ifstream fin(calibration_file.c_str());
+    if (!fin.is_open()) {
+      initialized = false;
+      return;
+    }
+    initialized = true;
+    try {
+      YAML::Node doc;
+#ifdef HAVE_NEW_YAMLCPP
+      fin.close();
+      doc = YAML::LoadFile(calibration_file);
+#else
+      YAML::Parser parser(fin);
+      parser.GetNextDocument(doc);
+#endif
+      doc >> *this;
+    } catch (YAML::Exception &e) {
+      std::cerr << "YAML Exception: " << e.what() << std::endl;
+      initialized = false;
+    }
+    fin.close();
+  }
+
+  void Calibration::write(const std::string& calibration_file) {
+    std::ofstream fout(calibration_file.c_str());
+    YAML::Emitter out;
+    out << *this;
+    fout << out.c_str();
+    fout.close();
+  }
+  
+} /* velodyne_pointcloud */

velodyne_lidar/velodyne_driver/calibration.h

@@ -0,0 +1,107 @@
+// Copyright (C) 2012, 2019 Austin Robot Technology, Piyush Khandelwal, Joshua Whitley
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef VELODYNE_POINTCLOUD_CALIBRATION_H
+#define VELODYNE_POINTCLOUD_CALIBRATION_H
+
+#include <map>
+#include <vector>
+#include <string>
+
+namespace velodyne_pointcloud
+{
+
+/** \brief correction values for a single laser
+ *
+ * Correction values for a single laser (as provided by db.xml from
+ * Velodyne).  Includes parameters for Velodyne HDL-64E S2.1.
+ *
+ * http://velodynelidar.com/lidar/products/manual/63-HDL64E%20S2%20Manual_Rev%20D_2011_web.pdf
+ */
+
+/** \brief Correction information for a single laser. */
+struct LaserCorrection
+{
+  /** parameters in db.xml */
+  float rot_correction;
+  float vert_correction;
+  float dist_correction;
+  bool two_pt_correction_available;
+  float dist_correction_x;
+  float dist_correction_y;
+  float vert_offset_correction;
+  float horiz_offset_correction;
+  int max_intensity;
+  int min_intensity;
+  float focal_distance;
+  float focal_slope;
+
+  /** cached values calculated when the calibration file is read */
+  float cos_rot_correction;              ///< cosine of rot_correction
+  float sin_rot_correction;              ///< sine of rot_correction
+  float cos_vert_correction;             ///< cosine of vert_correction
+  float sin_vert_correction;             ///< sine of vert_correction
+
+  int laser_ring;                        ///< ring number for this laser
+};
+
+/** \brief Calibration information for the entire device. */
+class Calibration
+{
+public:
+  float distance_resolution_m;
+  std::map<int, LaserCorrection> laser_corrections_map;
+  std::vector<LaserCorrection> laser_corrections;
+  int num_lasers;
+  bool initialized;
+  // bool ros_info;
+
+public:
+  explicit Calibration(bool info = true)
+  : distance_resolution_m(0.002f),
+    num_lasers(0),
+    initialized(false) {}
+  explicit Calibration(
+    const std::string& calibration_file,
+    bool info = true)
+  : distance_resolution_m(0.002f)
+  {
+    read(calibration_file);
+  }
+
+  void read(const std::string& calibration_file);
+  void write(const std::string& calibration_file);
+};
+
+}  // namespace velodyne_pointcloud
+
+#endif  // VELODYNE_POINTCLOUD_CALIBRATION_H

velodyne_lidar/velodyne_driver/input.cc

@@ -0,0 +1,200 @@
+// Copyright (C) 2007, 2009, 2010, 2015 Austin Robot Technology, Patrick Beeson, Jack O'Quin
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/** \file
+ *
+ *  Input classes for the Velodyne HDL-64E 3D LIDAR:
+ *
+ *     Input -- base class used to access the data independently of
+ *              its source
+ *
+ *     InputSocket -- derived class reads live data from the device
+ *              via a UDP socket
+ *
+ *     InputPCAP -- derived class provides a similar interface from a
+ *              PCAP dump
+ */
+
+#include <unistd.h>
+#include <string>
+#include <sstream>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+
+#include "input.h"
+
+namespace velodyne_driver
+{
+  ////////////////////////////////////////////////////////////////////////
+  // InputSocket class implementation
+  ////////////////////////////////////////////////////////////////////////
+
+  bool InputSocket::init(std::string ip, uint16_t port)
+  {
+    m_port = port;
+    m_devip_str = ip;
+    m_sockfd = -1;
+
+    if (!m_devip_str.empty())
+    {
+      inet_aton(m_devip_str.c_str(), &m_devip);
+    }
+
+    // connect to Velodyne UDP port
+    LOG(INFO) << "Opening UDP socket: port " << m_port;
+    m_sockfd = socket(PF_INET, SOCK_DGRAM, 0);
+    if (m_sockfd == -1)
+    {
+      perror("socket"); // TODO: ROS_ERROR errno
+      return false;
+    }
+
+    sockaddr_in my_addr;                  // my address information
+    memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
+    my_addr.sin_family = AF_INET;         // host byte order
+    my_addr.sin_port = htons(m_port);       // port in network byte order
+    my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
+
+    if (bind(m_sockfd, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
+    {
+      perror("bind"); // TODO: ROS_ERROR errno
+      return false;
+    }
+
+    if (fcntl(m_sockfd, F_SETFL, O_NONBLOCK | FASYNC) < 0)
+    {
+      perror("non-block");
+      return false;
+    }
+
+    LOG(INFO) << "Velodyne socket fd is " << m_sockfd;
+    return true;
+  }
+
+  void InputSocket::uninit()
+  {
+    (void)close(m_sockfd);
+  }
+
+  /** @brief Get one velodyne packet. */
+  int InputSocket::getPacket(unsigned char* pkt)
+  {
+    struct pollfd fds[1];
+    fds[0].fd = m_sockfd;
+    fds[0].events = POLLIN;
+    static const int POLL_TIMEOUT = 1000; // one second (in msec)
+
+    sockaddr_in sender_address;
+    socklen_t sender_address_len = sizeof(sender_address);
+
+    while (true)
+    {
+      // Unfortunately, the Linux kernel recvfrom() implementation
+      // uses a non-interruptible sleep() when waiting for data,
+      // which would cause this method to hang if the device is not
+      // providing data.  We poll() the device first to make sure
+      // the recvfrom() will not block.
+      //
+      // Note, however, that there is a known Linux kernel bug:
+      //
+      //   Under Linux, select() may report a socket file descriptor
+      //   as "ready for reading", while nevertheless a subsequent
+      //   read blocks.  This could for example happen when data has
+      //   arrived but upon examination has wrong checksum and is
+      //   discarded.  There may be other circumstances in which a
+      //   file descriptor is spuriously reported as ready.  Thus it
+      //   may be safer to use O_NONBLOCK on sockets that should not
+      //   block.
+
+      // poll() until input available
+      do
+      {
+        int retval = poll(fds, 1, POLL_TIMEOUT);
+        if (retval < 0) // poll() error?
+        {
+          if (errno != EINTR)
+            LOG(ERROR) << "poll() error:" << strerror(errno);
+          return -1;
+        }
+        if (retval == 0) // poll() timeout?
+        {
+          LOG(WARNING) << "Velodyne poll() timeout";
+          return -1;
+        }
+        if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
+        {
+          LOG(ERROR) << "poll() reports Velodyne error";
+          return -1;
+        }
+      } while ((fds[0].revents & POLLIN) == 0);
+
+      // Receive packets that should now be available from the
+      // socket using a blocking read.
+      ssize_t nbytes = recvfrom(m_sockfd, pkt,
+                                PACKET_SIZE, 0,
+                                (sockaddr *)&sender_address,
+                                &sender_address_len);
+
+      if (nbytes < 0)
+      {
+        if (errno != EWOULDBLOCK)
+        {
+          perror("recvfail");
+          LOG(INFO) << "recvfail";
+          return -1;
+        }
+      }
+      else if ((size_t)nbytes == PACKET_SIZE)
+      {
+        // read successful,
+        // if packet is not from the lidar scanner we selected by IP,
+        // continue otherwise we are done
+        if (m_devip_str != "" && sender_address.sin_addr.s_addr != m_devip.s_addr)
+          continue;
+        else
+        {
+          // memcpy(&packet, pkt, PACKET_SIZE);
+          break; //done
+        }
+      }
+
+      LOG(INFO) << "incomplete Velodyne packet read: " << nbytes << " bytes";
+    }
+
+    return 0;
+  }
+
+} // velodyne namespace

velodyne_lidar/velodyne_driver/input.h

@@ -0,0 +1,137 @@
+// Copyright (C) 2007, 2009, 2010, 2012, 2015Yaxin Liu, Patrick Beeson, Austin Robot Technology, Jack O'Quin
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/** @file
+ *
+ *  Velodyne 3D LIDAR data input classes
+ *
+ *    These classes provide raw Velodyne LIDAR input packets from
+ *    either a live socket interface or a previously-saved PCAP dump
+ *    file.
+ *
+ *  Classes:
+ *
+ *     velodyne::Input -- base class for accessing the data
+ *                      independently of its source
+ *
+ *     velodyne::InputSocket -- derived class reads live data from the
+ *                      device via a UDP socket
+ *
+ *     velodyne::InputPCAP -- derived class provides a similar interface
+ *                      from a PCAP dump file
+ */
+
+#ifndef VELODYNE_DRIVER_INPUT_H
+#define VELODYNE_DRIVER_INPUT_H
+
+#include <unistd.h>
+#include <stdio.h>
+#include <netinet/in.h>
+#include <string>
+#include <vector>
+
+#include <glog/logging.h>
+// #include <Eigen/Core>
+
+const int HDL_NUM_ROT_ANGLES = 36001;
+const int HDL_LASER_PER_FIRING = 32;
+const int VLP_LASER_PER_FIRING = 16;
+const int HDL_MAX_NUM_LASERS = 64;
+const int HDL_FIRING_PER_PKT = 12;
+const int PACKET_SIZE = 1206;
+
+// #pragma pack(push, 1)
+
+// struct HDLLaserReturn
+// {
+//   unsigned short distance;
+//   unsigned char intensity;
+// };
+
+// struct HDLFiringData
+// {
+//   unsigned short blockIdentifier;
+//   unsigned short rotationalPosition;
+//   HDLLaserReturn laserReturns[HDL_LASER_PER_FIRING];
+// };
+
+// enum HDLBlock
+// {
+//   BLOCK_0_TO_31 = 0xeeff,
+//   BLOCK_32_TO_63 = 0xddff
+// };
+
+// struct HDLDataPacket
+// {
+//   HDLFiringData firingData[HDL_FIRING_PER_PKT];
+//   unsigned int gpsTimestamp;
+//   unsigned char blank1;
+//   unsigned char blank2;
+// };
+
+// #pragma pack(pop)
+
+namespace velodyne_driver
+{
+
+static uint16_t DATA_PORT_NUMBER = 2368;      // default data port
+
+/** @brief Live Velodyne input from socket. */
+class InputSocket
+{
+public:
+  /** @brief constructor
+   *
+   *  @param ip desired ip for lidar, input empty string to listen any data from assigned udp port.
+   *  @param port UDP port number
+   */
+  InputSocket(){}
+  ~InputSocket(){}
+
+  bool init(std::string ip, uint16_t port = DATA_PORT_NUMBER);
+
+  void uninit();
+
+  // return 0 success, -1 error
+  int getPacket(unsigned char* pkt);
+
+private:
+  uint16_t m_port;
+  std::string m_devip_str;
+
+  int m_sockfd;
+  in_addr m_devip;
+};
+
+}  // namespace velodyne_driver
+
+#endif  // VELODYNE_DRIVER_INPUT_H

velodyne_lidar/velodyne_driver/rawdata.cc

@@ -0,0 +1,867 @@
+/*
+ *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
+ *  Copyright (C) 2009, 2010, 2012 Austin Robot Technology, Jack O'Quin
+ *  Copyright (C) 2019, Kaarta Inc, Shawn Hanna
+ *
+ *  License: Modified BSD Software License Agreement
+ *
+ *  $Id$
+ */
+
+/**
+ *  @file
+ *
+ *  Velodyne 3D LIDAR data accessor class implementation.
+ *
+ *  Class for unpacking raw Velodyne LIDAR packets into useful
+ *  formats.
+ *
+ *  Derived classes accept raw Velodyne data for either single packets
+ *  or entire rotations, and provide it in various formats for either
+ *  on-line or off-line processing.
+ *
+ *  @author Patrick Beeson
+ *  @author Jack O'Quin
+ *  @author Shawn Hanna
+ *
+ *  HDL-64E S2 calibration support provided by Nick Hillier
+ */
+
+#include <fstream>
+#include <math.h>
+
+#include "angles.h"
+#include "rawdata.h"
+
+namespace velodyne_rawdata
+{
+  inline float SQR(float val) { return val * val; }
+
+  ////////////////////////////////////////////////////////////////////////
+  //
+  // RawData base class implementation
+  //
+  ////////////////////////////////////////////////////////////////////////
+
+  RawData::RawData() {}
+
+  /** Update parameters: conversions and update */
+  void RawData::setParameters(double min_range,
+                              double max_range,
+                              double view_direction,
+                              double view_width)
+  {
+    config_.min_range = min_range;
+    config_.max_range = max_range;
+
+    //converting angle parameters into the velodyne reference (rad)
+    config_.tmp_min_angle = view_direction + view_width / 2;
+    config_.tmp_max_angle = view_direction - view_width / 2;
+
+    //computing positive modulo to keep theses angles into [0;2*M_PI]
+    config_.tmp_min_angle = fmod(fmod(config_.tmp_min_angle, 2 * M_PI) + 2 * M_PI, 2 * M_PI);
+    config_.tmp_max_angle = fmod(fmod(config_.tmp_max_angle, 2 * M_PI) + 2 * M_PI, 2 * M_PI);
+
+    //converting into the hardware velodyne ref (negative yaml and degrees)
+    //adding 0.5 perfomrs a centered double to int conversion
+    config_.min_angle = 100 * (2 * M_PI - config_.tmp_min_angle) * 180 / M_PI + 0.5;
+    config_.max_angle = 100 * (2 * M_PI - config_.tmp_max_angle) * 180 / M_PI + 0.5;
+    if (config_.min_angle == config_.max_angle)
+    {
+      //avoid returning empty cloud if min_angle = max_angle
+      config_.min_angle = 0;
+      config_.max_angle = 36000;
+    }
+  }
+
+  int RawData::scansPerPacket() const
+  {
+    if (calibration_.num_lasers == 16)
+    {
+      return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK *
+             VLP16_SCANS_PER_FIRING;
+    }
+    else
+    {
+      return BLOCKS_PER_PACKET * SCANS_PER_BLOCK;
+    }
+  }
+
+  /**
+   * Build a timing table for each block/firing. Stores in timing_offsets vector
+   */
+  bool RawData::buildTimings()
+  {
+    // vlp16
+    if (config_.model == "VLP16")
+    {
+      // timing table calculation, from velodyne user manual
+      timing_offsets.resize(12);
+      for (size_t i = 0; i < timing_offsets.size(); ++i)
+      {
+        timing_offsets[i].resize(32);
+      }
+      // constants
+      double full_firing_cycle = 55.296 * 1e-6; // seconds
+      double single_firing = 2.304 * 1e-6;      // seconds
+      double dataBlockIndex, dataPointIndex;
+      bool dual_mode = false;
+      // compute timing offsets
+      for (size_t x = 0; x < timing_offsets.size(); ++x)
+      {
+        for (size_t y = 0; y < timing_offsets[x].size(); ++y)
+        {
+          if (dual_mode)
+          {
+            dataBlockIndex = (x - (x % 2)) + (y / 16);
+          }
+          else
+          {
+            dataBlockIndex = (x * 2) + (y / 16);
+          }
+          dataPointIndex = y % 16;
+          //timing_offsets[block][firing]
+          timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
+        }
+      }
+    }
+    // vlp32
+    else if (config_.model == "32C")
+    {
+      // timing table calculation, from velodyne user manual
+      timing_offsets.resize(12);
+      for (size_t i = 0; i < timing_offsets.size(); ++i)
+      {
+        timing_offsets[i].resize(32);
+      }
+      // constants
+      double full_firing_cycle = 55.296 * 1e-6; // seconds
+      double single_firing = 2.304 * 1e-6;      // seconds
+      double dataBlockIndex, dataPointIndex;
+      bool dual_mode = false;
+      // compute timing offsets
+      for (size_t x = 0; x < timing_offsets.size(); ++x)
+      {
+        for (size_t y = 0; y < timing_offsets[x].size(); ++y)
+        {
+          if (dual_mode)
+          {
+            dataBlockIndex = x / 2;
+          }
+          else
+          {
+            dataBlockIndex = x;
+          }
+          dataPointIndex = y / 2;
+          timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
+        }
+      }
+    }
+    // hdl32
+    else if (config_.model == "32E")
+    {
+      // timing table calculation, from velodyne user manual
+      timing_offsets.resize(12);
+      for (size_t i = 0; i < timing_offsets.size(); ++i)
+      {
+        timing_offsets[i].resize(32);
+      }
+      // constants
+      double full_firing_cycle = 46.080 * 1e-6; // seconds
+      double single_firing = 1.152 * 1e-6;      // seconds
+      double dataBlockIndex, dataPointIndex;
+      bool dual_mode = false;
+      // compute timing offsets
+      for (size_t x = 0; x < timing_offsets.size(); ++x)
+      {
+        for (size_t y = 0; y < timing_offsets[x].size(); ++y)
+        {
+          if (dual_mode)
+          {
+            dataBlockIndex = x / 2;
+          }
+          else
+          {
+            dataBlockIndex = x;
+          }
+          dataPointIndex = y / 2;
+          timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
+        }
+      }
+    }
+    else if (config_.model == "VLS128")
+    {
+
+      timing_offsets.resize(3);
+      for (size_t i = 0; i < timing_offsets.size(); ++i)
+      {
+        timing_offsets[i].resize(17); // 17 (+1 for the maintenance time after firing group 8)
+      }
+
+      double full_firing_cycle = VLS128_SEQ_TDURATION * 1e-6;  //seconds
+      double single_firing = VLS128_CHANNEL_TDURATION * 1e-6;  // seconds
+      double offset_paket_time = VLS128_TOH_ADJUSTMENT * 1e-6; //seconds
+      double sequenceIndex, firingGroupIndex;
+      // Compute timing offsets
+      for (size_t x = 0; x < timing_offsets.size(); ++x)
+      {
+        for (size_t y = 0; y < timing_offsets[x].size(); ++y)
+        {
+
+          sequenceIndex = x;
+          firingGroupIndex = y;
+          timing_offsets[x][y] = (full_firing_cycle * sequenceIndex) + (single_firing * firingGroupIndex) - offset_paket_time;
+          LOG(INFO) << " firing_seque" << x << " firing_group" << y << "offset" << timing_offsets[x][y];
+        }
+      }
+    }
+    else
+    {
+      timing_offsets.clear();
+      LOG(WARNING) << "Timings not supported for model" << config_.model.c_str();
+    }
+
+    if (timing_offsets.size())
+    {
+      // ROS_INFO("VELODYNE TIMING TABLE:");
+      for (size_t x = 0; x < timing_offsets.size(); ++x)
+      {
+        for (size_t y = 0; y < timing_offsets[x].size(); ++y)
+        {
+          printf("%04.3f ", timing_offsets[x][y] * 1e6);
+        }
+        printf("\n");
+      }
+      return true;
+    }
+    else
+    {
+      LOG(WARNING) << "NO TIMING OFFSETS CALCULATED. ARE YOU USING A SUPPORTED VELODYNE SENSOR?";
+    }
+    return false;
+  }
+
+  /** Set up for on-line operation. */
+  bool RawData::setup(std::string model, std::string calib_file, double max_range, double min_range, int min_angle, int max_angle)
+  {
+
+    config_.model = model;
+    config_.calibrationFile = calib_file;
+    config_.max_range = max_range;
+    config_.min_range = min_range;
+    config_.min_angle = min_angle*100;
+    config_.max_angle = max_angle*100;
+
+    buildTimings();
+
+    // // get path to angles.config file for this device
+    // if (!private_nh.getParam("calibration", config_.calibrationFile))
+    // {
+    //   ROS_ERROR_STREAM("No calibration angles specified! Using test values!");
+
+    //   // have to use something: grab unit test version as a default
+    //   std::string pkgPath = ros::package::getPath("velodyne_pointcloud");
+    //   config_.calibrationFile = pkgPath + "/params/64e_utexas.yaml";
+    // }
+
+    // ROS_INFO_STREAM("correction angles: " << config_.calibrationFile);
+
+    if (!loadCalibration())
+    {
+      mb_initialized = false;
+      return false;
+    }
+    // ROS_INFO_STREAM("Number of lasers: " << calibration_.num_lasers << ".");
+
+    setupSinCosCache();
+    setupAzimuthCache();
+    mb_initialized = true;
+    return true;
+  }
+
+  /** Set up for offline operation */
+  int RawData::setupOffline(std::string calibration_file, double max_range_, double min_range_)
+  {
+
+    config_.max_range = max_range_;
+    config_.min_range = min_range_;
+    LOG(INFO) << "data ranges to publish: ["
+              << config_.min_range << ", "
+              << config_.max_range << "]";
+
+    config_.calibrationFile = calibration_file;
+
+    LOG(INFO) << "correction angles: " << config_.calibrationFile;
+
+    if (!loadCalibration())
+    {
+      return -1;
+    }
+    LOG(INFO) << "Number of lasers: " << calibration_.num_lasers << ".";
+
+    setupSinCosCache();
+    setupAzimuthCache();
+
+    return 0;
+  }
+
+  bool RawData::loadCalibration()
+  {
+
+    calibration_.read(config_.calibrationFile);
+    if (!calibration_.initialized)
+    {
+      LOG(ERROR) << "Unable to open calibration file: " << config_.calibrationFile;
+      return false;
+    }
+    return true;
+  }
+  void RawData::setupSinCosCache()
+  {
+    // Set up cached values for sin and cos of all the possible headings
+    for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index)
+    {
+      float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
+      cos_rot_table_[rot_index] = cosf(rotation);
+      sin_rot_table_[rot_index] = sinf(rotation);
+    }
+
+    if (config_.model == "VLS128")
+    {
+      for (uint8_t i = 0; i < 16; i++)
+      {
+        vls_128_laser_azimuth_cache[i] =
+            (VLS128_CHANNEL_TDURATION / VLS128_SEQ_TDURATION) * (i + i / 8);
+      }
+    }
+  }
+
+  void RawData::setupAzimuthCache()
+  {
+    if (config_.model == "VLS128")
+    {
+      for (uint8_t i = 0; i < 16; i++)
+      {
+        vls_128_laser_azimuth_cache[i] =
+            (VLS128_CHANNEL_TDURATION / VLS128_SEQ_TDURATION) * (i + i / 8);
+      }
+    }
+    else
+    {
+      LOG(WARNING) << "No Azimuth Cache configured for model " << config_.model.c_str();
+    }
+  }
+
+  /** @brief convert raw packet to point cloud
+   *
+   *  @param pkt raw packet to unpack
+   *  @param pc shared pointer to point cloud (points are appended)
+   */
+  void RawData::unpack(const unsigned char *pkt, std::vector<Lidar_point> &data)
+  {
+    using velodyne_pointcloud::LaserCorrection;
+    // LOG(INFO)<<"Received packet";
+
+    /** special parsing for the VLS128 **/
+    if (pkt[1205] == VLS128_MODEL_ID)
+    { // VLS 128
+      unpack_vls128(pkt, data);
+      return;
+    }
+
+    /** special parsing for the VLP16 **/
+    if (calibration_.num_lasers == 16)
+    {
+      unpack_vlp16(pkt, data);
+      return;
+    }
+
+    // float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
+
+    const raw_packet_t *raw = (const raw_packet_t *)&pkt[0];
+
+    for (int i = 0; i < BLOCKS_PER_PACKET; i++)
+    {
+
+      // upper bank lasers are numbered [0..31]
+      // NOTE: this is a change from the old velodyne_common implementation
+
+      int bank_origin = 0;
+      if (raw->blocks[i].header == LOWER_BANK)
+      {
+        // lower bank lasers are [32..63]
+        bank_origin = 32;
+      }
+
+      for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE)
+      {
+
+        float x, y, z;
+        float intensity;
+        const uint8_t laser_number = j + bank_origin;
+        float time = 0;
+
+        const LaserCorrection &corrections = calibration_.laser_corrections[laser_number];
+
+        /** Position Calculation */
+        const raw_block_t &block = raw->blocks[i];
+        union two_bytes tmp;
+        tmp.bytes[0] = block.data[k];
+        tmp.bytes[1] = block.data[k + 1];
+
+        /*condition added to avoid calculating points which are not
+          in the interesting defined area (min_angle < area < max_angle)*/
+        if ((block.rotation >= config_.min_angle && block.rotation <= config_.max_angle && config_.min_angle < config_.max_angle) || (config_.min_angle > config_.max_angle && (raw->blocks[i].rotation <= config_.max_angle || raw->blocks[i].rotation >= config_.min_angle)))
+        {
+
+          if (timing_offsets.size())
+          {
+            time = timing_offsets[i][j]; //      +time_diff_start_to_this_packet;
+          }
+
+          if (tmp.uint == 0) // no valid laser beam return
+          {
+            // call to addPoint is still required since output could be organized
+            data.push_back(Lidar_point(0.0f, 0.0f, 0.0f, corrections.laser_ring, raw->blocks[i].rotation, 0.0f, 0.0f, time));
+            continue;
+          }
+
+          float distance = tmp.uint * calibration_.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;
+
+          // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
+          // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
+          float cos_rot_angle =
+              cos_rot_table_[block.rotation] * cos_rot_correction +
+              sin_rot_table_[block.rotation] * sin_rot_correction;
+          float sin_rot_angle =
+              sin_rot_table_[block.rotation] * cos_rot_correction -
+              cos_rot_table_[block.rotation] * sin_rot_correction;
+
+          float horiz_offset = corrections.horiz_offset_correction;
+          float vert_offset = corrections.vert_offset_correction;
+
+          // Compute the distance in the xy plane (w/o accounting for rotation)
+          /**the new term of 'vert_offset * sin_vert_angle'
+           * was added to the expression due to the mathemathical
+           * model we used.
+           */
+          float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;
+
+          // Calculate temporal X, use absolute value.
+          float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
+          // Calculate temporal Y, use absolute value
+          float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
+          if (xx < 0)
+            xx = -xx;
+          if (yy < 0)
+            yy = -yy;
+
+          // Get 2points calibration values,Linear interpolation to get distance
+          // correction for X and Y, that means distance correction use
+          // different value at different distance
+          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;
+          /**the new term of 'vert_offset * sin_vert_angle'
+           * was added to the expression due to the mathemathical
+           * model we used.
+           */
+          xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle;
+          ///the expression wiht '-' is proved to be better than the one with '+'
+          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;
+          /**the new term of 'vert_offset * sin_vert_angle'
+           * was added to the expression due to the mathemathical
+           * model we used.
+           */
+          y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
+
+          // Using distance_y is not symmetric, but the velodyne manual
+          // does this.
+          /**the new term of 'vert_offset * cos_vert_angle'
+           * was added to the expression due to the mathemathical
+           * model we used.
+           */
+          z = distance_y * sin_vert_angle + vert_offset * cos_vert_angle;
+
+          /** Use standard ROS coordinate system (right-hand rule) */
+          float x_coord = y;
+          float y_coord = -x;
+          float z_coord = z;
+
+          /** Intensity Calculation */
+
+          float min_intensity = corrections.min_intensity;
+          float max_intensity = corrections.max_intensity;
+
+          intensity = raw->blocks[i].data[k + 2];
+
+          float focal_offset = 256 * (1 - corrections.focal_distance / 13100) * (1 - corrections.focal_distance / 13100);
+          float focal_slope = corrections.focal_slope;
+          intensity += focal_slope * (std::abs(focal_offset - 256 *
+                                                                  SQR(1 - static_cast<float>(tmp.uint) / 65535)));
+          intensity = (intensity < min_intensity) ? min_intensity : intensity;
+          intensity = (intensity > max_intensity) ? max_intensity : intensity;
+
+          data.push_back(Lidar_point(x_coord, y_coord, z_coord, corrections.laser_ring, raw->blocks[i].rotation, distance, intensity, time));
+        }
+      }
+      // data.newLine();
+    }
+  }
+
+  /** @brief convert raw VLS128 packet to point cloud
+ *
+ *  @param pkt raw packet to unpack
+ *  @param pc shared pointer to point cloud (points are appended)
+ */
+  void RawData::unpack_vls128(const unsigned char *pkt, std::vector<Lidar_point> &data)
+  {
+    float azimuth_diff, azimuth_corrected_f;
+    float last_azimuth_diff = 0;
+    uint16_t azimuth, azimuth_next, azimuth_corrected;
+    float x_coord, y_coord, z_coord;
+    float distance;
+    const raw_packet_t *raw = (const raw_packet_t *)&pkt[0];
+    union two_bytes tmp;
+
+    float cos_vert_angle, sin_vert_angle, cos_rot_correction, sin_rot_correction;
+    float cos_rot_angle, sin_rot_angle;
+    float xy_distance;
+
+    // float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
+
+    uint8_t laser_number, firing_order;
+    bool dual_return = (pkt[1204] == 57);
+
+    for (int block = 0; block < BLOCKS_PER_PACKET - (4 * dual_return); block++)
+    {
+      // cache block for use
+      const raw_block_t &current_block = raw->blocks[block];
+      float time = 0;
+
+      int bank_origin = 0;
+      // Used to detect which bank of 32 lasers is in this block
+      switch (current_block.header)
+      {
+      case VLS128_BANK_1:
+        bank_origin = 0;
+        break;
+      case VLS128_BANK_2:
+        bank_origin = 32;
+        break;
+      case VLS128_BANK_3:
+        bank_origin = 64;
+        break;
+      case VLS128_BANK_4:
+        bank_origin = 96;
+        break;
+      default:
+        // ignore packets with mangled or otherwise different contents
+        // Do not flood the log with messages, only issue at most one
+        // of these warnings per minute.
+        LOG_EVERY_N(INFO, 10000) << "skipping invalid VLS-128 packet: block "
+                                 << block << " header value is "
+                                 << raw->blocks[block].header;
+        return; // bad packet: skip the rest
+      }
+
+      // Calculate difference between current and next block's azimuth angle.
+      if (block == 0)
+      {
+        azimuth = current_block.rotation;
+      }
+      else
+      {
+        azimuth = azimuth_next;
+      }
+      if (block < (BLOCKS_PER_PACKET - (1 + dual_return)))
+      {
+        // Get the next block rotation to calculate how far we rotate between blocks
+        azimuth_next = raw->blocks[block + (1 + dual_return)].rotation;
+
+        // Finds the difference between two successive blocks
+        azimuth_diff = (float)((36000 + azimuth_next - azimuth) % 36000);
+
+        // This is used when the last block is next to predict rotation amount
+        last_azimuth_diff = azimuth_diff;
+      }
+      else
+      {
+        // This makes the assumption the difference between the last block and the next packet is the
+        // same as the last to the second to last.
+        // Assumes RPM doesn't change much between blocks
+        azimuth_diff = (block == BLOCKS_PER_PACKET - (4 * dual_return) - 1) ? 0 : last_azimuth_diff;
+      }
+
+      // condition added to avoid calculating points which are not in the interesting defined area (min_angle < area < max_angle)
+      if ((config_.min_angle < config_.max_angle && azimuth >= config_.min_angle && azimuth <= config_.max_angle) || (config_.min_angle > config_.max_angle))
+      {
+        for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE)
+        {
+          // distance extraction
+          tmp.bytes[0] = current_block.data[k];
+          tmp.bytes[1] = current_block.data[k + 1];
+          distance = tmp.uint * VLS128_DISTANCE_RESOLUTION;
+
+          if (pointInRange(distance))
+          {
+            laser_number = j + bank_origin;  // Offset the laser in this block by which block it's in
+            firing_order = laser_number / 8; // VLS-128 fires 8 lasers at a time
+
+            if (timing_offsets.size())
+            {
+              time = timing_offsets[block / 4][firing_order + laser_number / 64]; // + time_diff_start_to_this_packet;
+            }
+
+            velodyne_pointcloud::LaserCorrection &corrections = calibration_.laser_corrections[laser_number];
+
+            // correct for the laser rotation as a function of timing during the firings
+            azimuth_corrected_f = azimuth + (azimuth_diff * vls_128_laser_azimuth_cache[firing_order]);
+            azimuth_corrected = ((uint16_t)round(azimuth_corrected_f)) % 36000;
+
+            // convert polar coordinates to Euclidean XYZ
+            cos_vert_angle = corrections.cos_vert_correction;
+            sin_vert_angle = corrections.sin_vert_correction;
+            cos_rot_correction = corrections.cos_rot_correction;
+            sin_rot_correction = corrections.sin_rot_correction;
+
+            // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
+            // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
+            cos_rot_angle =
+                cos_rot_table_[azimuth_corrected] * cos_rot_correction +
+                sin_rot_table_[azimuth_corrected] * sin_rot_correction;
+            sin_rot_angle =
+                sin_rot_table_[azimuth_corrected] * cos_rot_correction -
+                cos_rot_table_[azimuth_corrected] * sin_rot_correction;
+
+            // Compute the distance in the xy plane (w/o accounting for rotation)
+            xy_distance = distance * cos_vert_angle;
+
+            data.push_back(Lidar_point(xy_distance * cos_rot_angle,
+                                       -(xy_distance * sin_rot_angle),
+                                       distance * sin_vert_angle,
+                                       corrections.laser_ring,
+                                       azimuth_corrected,
+                                       distance,
+                                       current_block.data[k + 2],
+                                       time));
+          }
+        }
+        // data.newLine();
+      }
+    }
+  }
+
+  /** @brief convert raw VLP16 packet to point cloud
+   *
+   *  @param pkt raw packet to unpack
+   *  @param data vector of self-defined point cloud (points are appended)
+   */
+  void RawData::unpack_vlp16(const unsigned char *pkt, std::vector<Lidar_point> &data)
+  {
+    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;
+
+    // float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
+
+    const raw_packet_t *raw = (const raw_packet_t *)&pkt[0];
+    float packet_timestamp = ((unsigned long)(raw->revolution & 0xff) << 8 | (unsigned long)(raw->revolution & 0xff00) | (unsigned long)(raw->status[0]) << 16 | (unsigned long)(raw->status[1]) << 24) * 1e-6;
+    // LOG_EVERY_N(INFO, 10) << "packet timestamp: " << packet_timestamp;
+
+    for (int block = 0; block < BLOCKS_PER_PACKET; block++)
+    {
+      // ignore packets with mangled or otherwise different contents
+      if (UPPER_BANK != raw->blocks[block].header)
+      {
+        // Do not flood the log with messages, only issue at most one
+        // of these warnings per minute.
+        LOG_EVERY_N(INFO, 1000) << "skipping invalid VLP-16 packet: block "
+                                << block << " header value is "
+                                << raw->blocks[block].header;
+        return; // bad packet: skip the rest
+      }
+
+      // Calculate difference between current and next block's azimuth angle.
+      azimuth = (float)(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);
+        // some packets contain an angle overflow where azimuth_diff < 0
+        if (raw_azimuth_diff < 0) //raw->blocks[block+1].rotation - raw->blocks[block].rotation < 0)
+        {
+          LOG_EVERY_N(INFO, 1) << "Packet containing angle overflow, first angle: " << raw->blocks[block].rotation << " second angle: " << raw->blocks[block + 1].rotation;
+          // if last_azimuth_diff was not zero, we can assume that the velodyne's speed did not change very much and use the same difference
+          if (last_azimuth_diff > 0)
+          {
+            azimuth_diff = last_azimuth_diff;
+          }
+          // otherwise we are not able to use this data
+          // TODO: we might just not use the second 16 firings
+          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)
+        {
+          velodyne_pointcloud::LaserCorrection &corrections = calibration_.laser_corrections[dsr];
+
+          /** Position Calculation */
+          union two_bytes tmp;
+          tmp.bytes[0] = raw->blocks[block].data[k];
+          tmp.bytes[1] = raw->blocks[block].data[k + 1];
+
+          /** correct for the laser rotation as a function of timing during the firings **/
+          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;
+
+          /*condition added to avoid calculating points which are not
+            in the interesting defined area (min_angle < area < max_angle)*/
+          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)))
+          {
+
+            // convert polar coordinates to Euclidean XYZ
+            float distance = tmp.uint * calibration_.distance_resolution_m;
+            distance += corrections.dist_correction;
+
+            // added by yct, range limit
+            if(!pointInRange(distance))
+              continue;
+
+            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;
+
+            // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
+            // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
+            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;
+
+            // Compute the distance in the xy plane (w/o accounting for rotation)
+            /**the new term of 'vert_offset * sin_vert_angle'
+             * was added to the expression due to the mathemathical
+             * model we used.
+             */
+            float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;
+
+            // Calculate temporal X, use absolute value.
+            float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
+            // Calculate temporal Y, use absolute value
+            float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
+            if (xx < 0)
+              xx = -xx;
+            if (yy < 0)
+              yy = -yy;
+
+            // Get 2points calibration values,Linear interpolation to get distance
+            // correction for X and Y, that means distance correction use
+            // different value at different distance
+            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;
+            /**the new term of 'vert_offset * sin_vert_angle'
+             * was added to the expression due to the mathemathical
+             * model we used.
+             */
+            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;
+            /**the new term of 'vert_offset * sin_vert_angle'
+             * was added to the expression due to the mathemathical
+             * model we used.
+             */
+            xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle;
+            y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
+
+            // Using distance_y is not symmetric, but the velodyne manual
+            // does this.
+            /**the new term of 'vert_offset * cos_vert_angle'
+             * was added to the expression due to the mathemathical
+             * model we used.
+             */
+            z = distance_y * sin_vert_angle + vert_offset * cos_vert_angle;
+
+            /** Use standard ROS coordinate system (right-hand rule) */
+            float x_coord = y;
+            float y_coord = -x;
+            float z_coord = z;
+
+            /** Intensity Calculation */
+            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;
+
+            float time = 0;
+            if (timing_offsets.size())
+              time = timing_offsets[block][firing * 16 + dsr]; // + time_diff_start_to_this_packet;
+
+            data.push_back(Lidar_point(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity, time+packet_timestamp));
+          }
+        }
+        // data.newLine();
+      }
+    }
+  }
+} // namespace velodyne_rawdata

velodyne_lidar/velodyne_driver/rawdata.h

@@ -0,0 +1,349 @@
+// Copyright (C) 2007, 2009, 2010, 2012, 2019 Yaxin Liu, Patrick Beeson, Jack O'Quin, Joshua Whitley
+// All rights reserved.
+//
+// Software License Agreement (BSD License 2.0)
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions
+// are met:
+//
+//  * Redistributions of source code must retain the above copyright
+//    notice, this list of conditions and the following disclaimer.
+//  * Redistributions in binary form must reproduce the above
+//    copyright notice, this list of conditions and the following
+//    disclaimer in the documentation and/or other materials provided
+//    with the distribution.
+//  * Neither the name of {copyright_holder} nor the names of its
+//    contributors may be used to endorse or promote products derived
+//    from this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+/** @file
+ *
+ *  @brief Interfaces for interpreting raw packets from the Velodyne 3D LIDAR.
+ *
+ *  @author Yaxin Liu
+ *  @author Patrick Beeson
+ *  @author Jack O'Quin
+ *  @author Youchen Tang
+ */
+
+#ifndef VELODYNE_POINTCLOUD_RAWDATA_H
+#define VELODYNE_POINTCLOUD_RAWDATA_H
+
+#include <errno.h>
+#include <stdint.h>
+#include <string>
+#include <boost/format.hpp>
+#include <math.h>
+#include <vector>
+#include <mutex>
+#include <atomic>
+#include <chrono>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include "calibration.h"
+
+struct Lidar_point
+{
+  double x;
+  double y;
+  double z;
+  uint16_t ring;
+  uint16_t azimuth;
+  float distance;
+  float intensity;
+  float timestamp;
+
+  Lidar_point()
+  {
+    x = y = z = distance = 0.0;
+    intensity = 0;
+    ring = 0;
+    azimuth = 0;
+    timestamp = 0;
+  }
+
+  Lidar_point(double x, double y, double z, uint16_t ring, uint16_t azimuth, float distance, float intensity, float timestamp)
+  {
+    this->x = x;
+    this->y = y;
+    this->z = z;
+    this->ring = ring;
+    this->azimuth = azimuth;
+    this->distance = distance;
+    this->intensity = intensity;
+    this->timestamp = timestamp;
+  }
+
+  Lidar_point(const Lidar_point &point)
+  {
+    *this = point;
+  }
+
+  Lidar_point &operator=(const Lidar_point &point)
+  {
+    x = point.x;
+    y = point.y;
+    z = point.z;
+    intensity = point.intensity;
+    ring = point.ring;
+    azimuth = point.azimuth;
+    distance = point.distance;
+    timestamp = point.timestamp;
+    return *this;
+  }
+
+  void transform(Eigen::Matrix<double, 4, 4> transform_matrix)
+  {
+    Eigen::Vector4d point{x, y, z, 1};
+    point = transform_matrix * point;
+    x = point.x() / point.w();
+    y = point.y() / point.w();
+    z = point.z() / point.w();
+    // printf("point: %.6f %.6f %.6f %.6f \n", x, y, z, point.w());
+  }
+};
+
+struct PointXYZIRT
+{
+  PCL_ADD_POINT4D;                // quad-word XYZ
+  float intensity;                ///< laser intensity reading
+  uint16_t ring;                  ///< laser ring number
+  float time;                     ///< laser time reading
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
+                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring)(float, time, time))
+
+namespace velodyne_rawdata
+{
+
+  /**
+ * Raw Velodyne packet constants and structures.
+ */
+  static const int SIZE_BLOCK = 100;
+  static const int RAW_SCAN_SIZE = 3;
+  static const int SCANS_PER_BLOCK = 32;
+  static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
+
+  static const float ROTATION_RESOLUTION = 0.01f;    // [deg]
+  static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100]
+
+  /** @todo make this work for both big and little-endian machines */
+  static const uint16_t UPPER_BANK = 0xeeff;
+  static const uint16_t LOWER_BANK = 0xddff;
+
+  /** Special Defines for VLP16 support **/
+  static const int VLP16_FIRINGS_PER_BLOCK = 2;
+  static const int VLP16_SCANS_PER_FIRING = 16;
+  static const float VLP16_BLOCK_TDURATION = 110.592f; // [µs]
+  static const float VLP16_DSR_TOFFSET = 2.304f;       // [µs]
+  static const float VLP16_FIRING_TOFFSET = 55.296f;   // [µs]
+
+  /** \brief Raw Velodyne data block.
+ *
+ *  Each block contains data from either the upper or lower laser
+ *  bank.  The device returns three times as many upper bank blocks.
+ *
+ *  use stdint.h types, so things work with both 64 and 32-bit machines
+ */
+  typedef struct raw_block
+  {
+    uint16_t header;   ///< UPPER_BANK or LOWER_BANK
+    uint16_t rotation; ///< 0-35999, divide by 100 to get degrees
+    uint8_t data[BLOCK_DATA_SIZE];
+  } raw_block_t;
+
+  /** used for unpacking the first two data bytes in a block
+ *
+ *  They are packed into the actual data stream misaligned.  I doubt
+ *  this works on big endian machines.
+ */
+  union two_bytes
+  {
+    uint16_t uint;
+    uint8_t bytes[2];
+  };
+
+  static const int PACKET_SIZE = 1206;
+  static const int BLOCKS_PER_PACKET = 12;
+  static const int PACKET_STATUS_SIZE = 4;
+  static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
+
+  /** Special Definitions for VLS128 support **/
+  // These are used to detect which bank of 32 lasers is in this block
+  static const uint16_t VLS128_BANK_1 = 0xeeff;
+  static const uint16_t VLS128_BANK_2 = 0xddff;
+  static const uint16_t VLS128_BANK_3 = 0xccff;
+  static const uint16_t VLS128_BANK_4 = 0xbbff;
+
+  static const float VLS128_CHANNEL_TDURATION = 2.665f;   // [µs] Channels corresponds to one laser firing
+  static const float VLS128_SEQ_TDURATION = 53.3f;        // [µs] Sequence is a set of laser firings including recharging
+  static const float VLS128_TOH_ADJUSTMENT = 8.7f;        // [µs] μs. Top Of the Hour is aligned with the fourth firing group in a firing sequence.
+  static const float VLS128_DISTANCE_RESOLUTION = 0.004f; // [m]
+  static const float VLS128_MODEL_ID = 161;
+
+  /** \brief Raw Velodyne packet.
+ *
+ *  revolution is described in the device manual as incrementing
+ *    (mod 65536) for each physical turn of the device.  Our device
+ *    seems to alternate between two different values every third
+ *    packet.  One value increases, the other decreases.
+ *
+ *  \todo figure out if revolution is only present for one of the
+ *  two types of status fields
+ *
+ *  status has either a temperature encoding or the microcode level
+ */
+  typedef struct raw_packet
+  {
+    raw_block_t blocks[BLOCKS_PER_PACKET];
+    uint16_t revolution;
+    uint8_t status[PACKET_STATUS_SIZE];
+  } raw_packet_t;
+
+  /** \brief Velodyne data conversion class */
+  class RawData
+  {
+  public:
+    /** configuration parameters */
+    typedef struct
+    {
+      std::string model;
+      std::string calibrationFile; ///< calibration file name
+      double max_range;            ///< maximum range to publish
+      double min_range;            ///< minimum range to publish
+      int min_angle;               ///< minimum angle to publish
+      int max_angle;               ///< maximum angle to publish
+
+      double tmp_min_angle;
+      double tmp_max_angle;
+    } Config;
+
+    RawData();
+    ~RawData()
+    {
+    }
+
+    /** \brief Set up for data processing.
+   *
+   *  Perform initializations needed before data processing can
+   *  begin:
+   *
+   *    - read device-specific angles calibration
+   *
+   *  @param model model name, i.e. VLP16 32E 32C 128S
+   *  @param calib_file absolute path to the calibration file
+   *  @param max_range maximum range of point distance, in meter
+   *  @param min_range minimum range of point distance, in meter
+   *  @param min_angle minimum azimuth of point, in degree
+   *  @param max_angle maximum azimuth of point, in degree
+   *  @returns an optional calibration
+   */
+    bool setup(std::string model, std::string calib_file, double max_range, double min_range, int min_angle, int max_angle);
+
+    void setupSinCosCache();
+    void setupAzimuthCache();
+    bool loadCalibration();
+
+    /** \brief Set up for data processing offline.
+   * Performs the same initialization as in setup, in the abscence of a ros::NodeHandle.
+   * this method is useful if unpacking data directly from bag files, without passing
+   * through a communication overhead.
+   *
+   * @param calibration_file path to the calibration file
+   * @param max_range_ cutoff for maximum range
+   * @param min_range_ cutoff for minimum range
+   * @returns 0 if successful;
+   *           errno value for failure
+   */
+    int setupOffline(std::string calibration_file, double max_range_, double min_range_);
+
+    // unpack velodyne lidar packets, convert to points and stored in data vector
+    void unpack(const unsigned char *pkt, std::vector<Lidar_point> &data);
+
+    void setParameters(double min_range, double max_range, double view_direction, double view_width);
+
+    int scansPerPacket() const;
+
+    void getCloud(std::vector<Lidar_point> &data);
+
+    void getConfig(Config &conf){
+      conf.model = config_.model;
+      conf.calibrationFile = config_.calibrationFile;
+      conf.max_range = config_.max_range;
+      conf.min_range = config_.min_range;
+      conf.min_angle = config_.min_angle;
+      conf.max_angle = config_.max_angle;
+      conf.tmp_min_angle = config_.tmp_min_angle;
+      conf.tmp_max_angle = config_.tmp_max_angle;
+    }
+
+    void getCalib(velodyne_pointcloud::Calibration &calib)
+    {
+      calib.distance_resolution_m = calibration_.distance_resolution_m;
+      calib.laser_corrections_map = calibration_.laser_corrections_map;
+      calib.laser_corrections = calibration_.laser_corrections;
+      calib.num_lasers = calibration_.num_lasers;
+      calib.initialized = calibration_.initialized;
+    }
+
+    bool isInitialized() { return mb_initialized; }
+
+  private:
+    Config config_;
+    bool mb_initialized;
+
+    /**
+   * Calibration file
+   */
+    velodyne_pointcloud::Calibration calibration_;
+    float sin_rot_table_[ROTATION_MAX_UNITS];
+    float cos_rot_table_[ROTATION_MAX_UNITS];
+
+    // Caches the azimuth percent offset for the VLS-128 laser firings
+    float vls_128_laser_azimuth_cache[16];
+
+    // timing offset lookup table
+    std::vector<std::vector<float>> timing_offsets;
+
+    /** \brief setup per-point timing offsets
+   * 
+   *  Runs during initialization and determines the firing time for each point in the scan
+   * 
+   *  NOTE: Does not support all sensors yet (vlp16, vlp32, and hdl32 are currently supported)
+   */
+    bool buildTimings();
+
+    /** add private function to handle the VLP16 **/
+    // add range limit and utc timestamp by yct
+    void unpack_vlp16(const unsigned char *pkt, std::vector<Lidar_point> &data);
+
+    void unpack_vls128(const unsigned char *pkt, std::vector<Lidar_point> &data);
+
+    /** in-line test whether a point is in range */
+    inline bool pointInRange(float range)
+    {
+      return (range >= config_.min_range && range <= config_.max_range);
+    }
+  };
+
+} // namespace velodyne_rawdata
+
+#endif // VELODYNE_POINTCLOUD_RAWDATA_H

velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp

@@ -0,0 +1,638 @@
+#include "velodyne_lidar_device.h"
+
+// 万集雷达封装类构造函数
+Velodyne_lidar_device::Velodyne_lidar_device()
+{
+	m_velodyne_lidar_device_status = E_UNKNOWN;
+	mp_velodyne_lidar_scan_task = NULL;
+}
+
+// 万集雷达封装类析构函数
+Velodyne_lidar_device::~Velodyne_lidar_device()
+{
+	uninit();
+}
+
+void Velodyne_lidar_device::dev_info_config(std::string model, int rpm)
+{
+	double packet_rate; // packet frequency (Hz)
+	std::string model_full_name;
+	if ((model == "VLS128"))
+	{
+		packet_rate = 6253.9; //  3 firing cycles in a data packet. 3 x 53.3 μs = 0.1599 ms is the accumulation delay per packet.
+							  //   1 packet/0.1599 ms = 6253.9 packets/second
+
+		model_full_name = model;
+	}
+	else if ((model == "64E_S2") ||
+			 (model == "64E_S2.1")) // generates 1333312 points per second
+	{								// 1 packet holds 384 points
+		packet_rate = 3472.17;		// 1333312 / 384
+		model_full_name = std::string("HDL-") + model;
+	}
+	else if (model == "64E")
+	{
+		packet_rate = 2600.0;
+		model_full_name = std::string("HDL-") + model;
+	}
+	else if (model == "64E_S3") // generates 2222220 points per second (half for strongest and half for lastest)
+	{							// 1 packet holds 384 points
+		packet_rate = 5787.03;	// 2222220 / 384
+		model_full_name = std::string("HDL-") + model;
+	}
+	else if (model == "32E")
+	{
+		packet_rate = 1808.0;
+		model_full_name = std::string("HDL-") + model;
+	}
+	else if (model == "32C")
+	{
+		packet_rate = 1507.0;
+		model_full_name = std::string("VLP-") + model;
+	}
+	else if (model == "VLP16")
+	{
+		packet_rate = 754; // 754 Packets/Second for Last or Strongest mode 1508 for dual (VLP-16 User Manual)
+		model_full_name = "VLP-16";
+	}
+	else
+	{
+		LOG(WARNING) << "unknown Velodyne LIDAR model: " << model;
+		packet_rate = 2600.0;
+	}
+	std::string deviceName(std::string("Velodyne ") + model_full_name);
+
+	LOG(INFO) << deviceName << " rotating at " << rpm << " RPM";
+	double frequency = (rpm / 60.0); // expected Hz rate
+
+	// default number of packets for each scan is a single revolution
+	// (fractions rounded up)
+	m_num_packet_per_scan = (int)ceil(packet_rate / frequency);
+	LOG(INFO) << "publishing " << m_num_packet_per_scan << " packets per scan";
+}
+
+// 初始化
+Error_manager Velodyne_lidar_device::init(velodyne::velodyneLidarParams params)
+{
+	Error_manager t_error;
+	LOG(INFO) << " Velodyne_lidar_device::init " << this;
+	m_num_packet_inside = 0;
+	dev_info_config(params.model(), params.rpm());
+	m_velodyne_lidar_device_status = E_UNKNOWN;
+	m_velodyne_socket_status = E_UNKNOWN;
+	m_velodyne_protocol_status = E_UNKNOWN;
+
+	//唤醒队列
+	m_communication_data_queue.wake_queue();
+
+	//初始化通信协议
+	t_error = m_protocol.setup(params.ip(), params.calibrationfile(), params.max_range(), params.min_range(), params.min_angle(), params.max_angle()) == true ? SUCCESS : Error_manager(PARAMETER_ERROR, MINOR_ERROR, "protocol parameter error");
+	if (t_error != SUCCESS)
+	{
+		return t_error;
+	}
+	m_velodyne_protocol_status = E_READY;
+
+	//初始化雷达通信模块
+	std::string t_ip = params.ip();
+	int t_port = params.port();
+
+	t_error = m_input_socket.init(t_ip, t_port) == true ? SUCCESS : Error_manager(VELODYNE_LIDAR_CONNECT_FAILED, MINOR_ERROR, "socket connect failed");
+	if (t_error != SUCCESS)
+	{
+		return t_error;
+	}
+	m_velodyne_socket_status = E_READY;
+
+	m_capture_condition.reset(false, false, true);
+	mp_capture_thread = new std::thread(&Velodyne_lidar_device::capture_thread_fun, this);
+
+	m_parse_condition.reset(false, false, false);
+	mp_parse_thread = new std::thread(&Velodyne_lidar_device::parse_thread_fun, this);
+
+	//启动 内部线程。默认wait。
+	m_execute_condition.reset(false, false, false);
+	mp_execute_thread = new std::thread(&Velodyne_lidar_device::execute_thread_fun, this);
+
+	m_velodyne_lidar_device_status = E_READY;
+
+	return t_error;
+}
+
+//反初始化
+Error_manager Velodyne_lidar_device::uninit()
+{
+	//终止队列，防止 wait_and_pop 阻塞线程。
+	m_communication_data_queue.termination_queue();
+
+	if (mp_capture_thread)
+	{
+		m_capture_condition.kill_all();
+	}
+	if (mp_capture_thread)
+	{
+		mp_capture_thread->join();
+		delete mp_capture_thread;
+		mp_capture_thread = NULL;
+	}
+
+	if (mp_parse_thread)
+	{
+		m_parse_condition.kill_all();
+	}
+	if (mp_parse_thread)
+	{
+		mp_parse_thread->join();
+		delete mp_parse_thread;
+		mp_parse_thread = NULL;
+	}
+
+	//关闭线程
+	if (mp_execute_thread)
+	{
+		m_execute_condition.kill_all();
+	}
+	//回收线程的资源
+	if (mp_execute_thread)
+	{
+		mp_execute_thread->join();
+		delete mp_execute_thread;
+		mp_execute_thread = NULL;
+	}
+
+	m_input_socket.uninit();
+
+	//清空队列
+	m_communication_data_queue.clear_and_delete();
+
+	if (m_velodyne_lidar_device_status != E_FAULT)
+	{
+		m_velodyne_lidar_device_status = E_UNKNOWN;
+	}
+	return Error_code::SUCCESS;
+}
+
+//对外的接口函数，负责接受并处理任务单，
+Error_manager Velodyne_lidar_device::execute_task(Task_Base *p_velodyne_lidar_scan_task)
+{
+	LOG(INFO) << " ---Laser_manager::execute_task run---  " << this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//检查指针
+	if (p_velodyne_lidar_scan_task == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Velodyne_lidar_device::execute_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型，
+	if (p_velodyne_lidar_scan_task->get_task_type() != VELODYNE_LIDAR_SCAN)
+	{
+		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 " task type error ！= VELODYNE_LIDAR_SCAN ");
+	}
+
+	//检查接收方的状态
+	t_error = check_status();
+	if (t_error != SUCCESS)
+	{
+		t_result.compare_and_cover_error(t_error);
+	}
+	else
+	{
+		//接受任务，并将任务的状态改为TASK_SIGNED已签收
+		mp_velodyne_lidar_scan_task = (Velodyne_lidar_scan_task *)p_velodyne_lidar_scan_task;
+		mp_velodyne_lidar_scan_task->set_task_statu(TASK_SIGNED);
+
+		//检查消息内容是否正确，
+		//检查三维点云指针
+		if (mp_velodyne_lidar_scan_task->get_task_point_cloud().get() == NULL)
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_point_cloud is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else if (mp_velodyne_lidar_scan_task->get_task_cloud_lock() == NULL)
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_cloud_lock is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else
+		{
+			m_velodyne_lidar_device_status = E_BUSY;
+			//启动雷达管理模块，的核心工作线程
+			m_execute_condition.notify_all(true);
+			//通知 thread_work 子线程启动。
+
+			//将任务的状态改为 TASK_WORKING 处理中
+			mp_velodyne_lidar_scan_task->set_task_statu(TASK_WORKING);
+		}
+
+		//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
+		{
+			//忽略轻微故障
+			if (t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+				//将任务的状态改为 TASK_ERROR 结束错误
+				mp_velodyne_lidar_scan_task->set_task_statu(TASK_ERROR);
+			}
+			//返回错误码
+			mp_velodyne_lidar_scan_task->compare_and_cover_task_error_manager(t_result);
+		}
+	}
+
+	return t_result;
+}
+
+//检查雷达状态,是否正常运行
+Error_manager Velodyne_lidar_device::check_status()
+{
+	updata_status();
+	if (m_velodyne_lidar_device_status == E_READY)
+	{
+		return Error_code::SUCCESS;
+	}
+	else if (m_velodyne_lidar_device_status == E_BUSY)
+	{
+		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
+							 "  Velodyne_lidar_device::check_status() error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " Velodyne_lidar_device::check_status() error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+Error_manager Velodyne_lidar_device::end_task()
+{
+	m_execute_condition.notify_all(false);
+
+	if (m_velodyne_lidar_device_status == E_BUSY)
+	{
+		m_velodyne_lidar_device_status = E_READY;
+	}
+	//else 状态不变
+
+	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
+	if (mp_velodyne_lidar_scan_task != NULL)
+	{
+		//判断任务单的错误等级，
+		if (mp_velodyne_lidar_scan_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
+		{
+			//强制改为TASK_OVER，不管它当前在做什么。
+			mp_velodyne_lidar_scan_task->set_task_statu(TASK_OVER);
+		}
+		else
+		{
+			//强制改为 TASK_ERROR，不管它当前在做什么。
+			mp_velodyne_lidar_scan_task->set_task_statu(TASK_ERROR);
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//取消任务单，由发送方提前取消任务单
+Error_manager Velodyne_lidar_device::cancel_task(Task_Base *p_wanji_lidar_scan_task)
+{
+	if (p_wanji_lidar_scan_task == mp_velodyne_lidar_scan_task)
+	{
+		m_execute_condition.notify_all(false);
+
+		if (m_velodyne_lidar_device_status == E_BUSY)
+		{
+			m_velodyne_lidar_device_status = E_READY;
+		}
+		//else 状态不变
+
+		//强制改为 TASK_DEAD，不管它当前在做什么。
+		mp_velodyne_lidar_scan_task->set_task_statu(TASK_DEAD);
+
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+							 " Velodyne_lidar_device::cancel_task PARAMRTER ERROR ");
+	}
+}
+
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Velodyne_lidar_device::is_ready()
+{
+	updata_status();
+	return m_velodyne_lidar_device_status == E_READY;
+}
+
+//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
+Error_manager Velodyne_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+															std::chrono::steady_clock::time_point command_time, int timeout_ms)
+{
+	if (p_mutex == NULL || p_cloud.get() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	//判断是否超时
+	while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
+	{
+		//获取指令时间之后的点云, 如果没有就会循环
+		if (m_parse_ring_time > command_time)
+		{
+			std::unique_lock<std::mutex> lck(*p_mutex);
+			{
+				std::lock_guard<std::mutex> lck(m_cloud_mutex);
+				if (m_ring_cloud.size() <= 0)
+					return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
+				for (size_t i = 0; i < m_ring_cloud.size(); i++)
+				{
+					p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
+				}
+			}
+			return SUCCESS;
+		}
+		//else 等待下一次数据更新
+
+		//等1ms
+		std::this_thread::sleep_for(std::chrono::milliseconds(1));
+	}
+	//超时退出就报错
+	return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
+						 " Velodyne_lidar_device::get_new_cloud_and_wait error ");
+}
+
+//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
+Error_manager Velodyne_lidar_device::get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+													   std::chrono::steady_clock::time_point command_time)
+{
+	if (p_mutex == NULL || p_cloud.get() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	//获取指令时间之后的点云, 如果没有就会报错, 不会等待
+	if (m_parse_time > command_time)
+	{
+		std::unique_lock<std::mutex> lck(*p_mutex);
+		{
+			std::lock_guard<std::mutex> lck(m_cloud_mutex);
+			if (m_cloud_buf.size() <= 0)
+				return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
+			for (size_t i = 0; i < m_cloud_buf.size(); i++)
+			{
+				p_cloud->push_back(pcl::PointXYZ(m_cloud_buf[i].x, m_cloud_buf[i].y, m_cloud_buf[i].z));
+			}
+		}
+		return SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+							 " Velodyne_lidar_device::get_current_cloud error ");
+	}
+}
+
+//外部调用获取最新的点云, 如果没有就会报错, 不会等待
+Error_manager Velodyne_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+													std::chrono::steady_clock::time_point command_time)
+{
+	if (p_mutex == NULL || p_cloud.get() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	if (m_parse_ring_time > command_time)
+	{
+		std::unique_lock<std::mutex> lck(*p_mutex);
+		{
+			std::lock_guard<std::mutex> lck(m_cloud_mutex);
+			if (m_ring_cloud.size() <= 0)
+				return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
+			for (size_t i = 0; i < m_ring_cloud.size(); i++)
+			{
+				p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
+			}
+		}
+		return SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+							 " Velodyne_lidar_device::get_current_cloud error ");
+	}
+}
+
+//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+//注:函数内部不加锁, 由外部统一加锁.
+Error_manager Velodyne_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+													std::chrono::steady_clock::time_point command_time)
+{
+	if (p_cloud.get() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	if (m_parse_ring_time > command_time)
+	{
+		std::lock_guard<std::mutex> lck(m_cloud_mutex);
+		if (m_ring_cloud.size() <= 0)
+			return VELODYNE_LIDAR_DEVICE_NO_CLOUD;
+		for (size_t i = 0; i < m_ring_cloud.size(); i++)
+		{
+			p_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
+		}
+		return SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::VELODYNE_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+							 " Velodyne_lidar_device::get_current_cloud error ");
+	}
+}
+
+Velodyne_lidar_device::Velodyne_lidar_device_status Velodyne_lidar_device::get_status()
+{
+	updata_status();
+	return m_velodyne_lidar_device_status;
+}
+
+void Velodyne_lidar_device::updata_status()
+{
+	if (m_velodyne_socket_status == E_FAULT || m_velodyne_protocol_status == E_FAULT)
+	{
+		m_velodyne_lidar_device_status = E_FAULT;
+	}
+	if (m_velodyne_socket_status == E_DISCONNECT)
+	{
+		m_velodyne_lidar_device_status = E_DISCONNECT;
+	}
+	else if (m_velodyne_socket_status == E_READY && m_velodyne_protocol_status == E_READY)
+	{
+		if (m_execute_condition.get_pass_ever())
+		{
+			m_velodyne_lidar_device_status = E_BUSY;
+		}
+		else
+		{
+			m_velodyne_lidar_device_status = E_READY;
+		}
+	}
+	else
+	{
+		m_velodyne_lidar_device_status = E_UNKNOWN;
+	}
+}
+
+void Velodyne_lidar_device::capture_thread_fun()
+{
+	LOG(INFO) << " Velodyne_lidar_device::capture_thread_fun() start " << this;
+	Error_manager t_error;
+
+	while (m_capture_condition.is_alive())
+	{
+		m_capture_condition.wait_for_millisecond(1);
+		if (m_capture_condition.is_alive())
+		{
+			// socket状态正常才更新状态
+			if (m_velodyne_socket_status == E_READY)
+				m_velodyne_socket_status = E_BUSY;
+			if (m_input_socket.getPacket(m_buf) == 0)
+			{
+				Binary_buf *tp_binary_buf = new Binary_buf(reinterpret_cast<char *>(m_buf), PACKET_SIZE);
+				//将数据添加到队列中, 然后内存权限转交给队列.
+				m_communication_data_queue.push(tp_binary_buf);
+				m_capture_time = std::chrono::steady_clock::now();
+				m_velodyne_socket_status = E_READY;
+			}
+			else
+			{
+				m_velodyne_socket_status = E_DISCONNECT;
+			}
+			// 队列有数据，激活解析线程
+			if (m_communication_data_queue.size() > 0)
+				m_parse_condition.notify_one(false, true);
+		}
+	}
+	LOG(INFO) << " Velodyne_lidar_device::capture_thread_fun() end :" << this;
+	return;
+}
+
+void Velodyne_lidar_device::parse_thread_fun()
+{
+	LOG(INFO) << " Velodyne_lidar_device::parse_thread_fun() start " << this;
+	Error_manager t_error;
+
+	while (m_parse_condition.is_alive())
+	{
+		m_parse_condition.wait();
+		if (m_parse_condition.is_alive())
+		{
+			if (m_velodyne_protocol_status == E_READY)
+				m_velodyne_protocol_status = E_BUSY;
+			if (m_protocol.isInitialized())
+			{
+				Binary_buf *buf = *m_communication_data_queue.wait_and_pop();
+				if (buf != nullptr)
+				{
+					std::lock_guard<std::mutex> lck(m_cloud_mutex);
+					m_protocol.unpack(reinterpret_cast<unsigned char *>(buf->get_buf()), m_cloud_buf);
+					m_num_packet_inside++;
+					m_parse_time = std::chrono::steady_clock::now();
+					// 雷达掉线，清除不完整数据，不更新时间戳
+					if (m_velodyne_socket_status != E_READY && m_velodyne_socket_status != E_BUSY)
+					{
+						m_num_packet_inside = 0;
+						m_cloud_buf.clear();
+						m_ring_cloud.clear();
+					}
+					else if (m_num_packet_inside >= m_num_packet_per_scan) // 一环数据填满，同时更新时间戳
+					{
+						m_num_packet_inside = 0;
+						m_ring_cloud.clear();
+						m_ring_cloud = m_cloud_buf;
+						m_cloud_buf.clear();
+						m_parse_ring_time = std::chrono::steady_clock::now();
+					}
+				}
+				m_velodyne_protocol_status = E_READY;
+			}
+			else
+			{
+				m_velodyne_protocol_status = E_FAULT;
+			}
+		}
+	}
+	LOG(INFO) << " Velodyne_lidar_device::parse_thread_fun() end " << this;
+	return;
+}
+
+//mp_laser_manager_thread 线程执行函数，
+//thread_work 内部线程负责获取点云结果
+void Velodyne_lidar_device::execute_thread_fun()
+{
+	LOG(INFO) << " Velodyne_lidar_device::execute_thread_fun() start " << this;
+	Error_manager t_error;
+
+	//雷达管理的独立线程，负责控制管理所有的雷达。
+	while (m_execute_condition.is_alive())
+	{
+		m_execute_condition.wait();
+		if (m_execute_condition.is_alive())
+		{
+			std::this_thread::yield();
+
+			if (mp_velodyne_lidar_scan_task == NULL)
+			{
+				//没有任务就忽略, 直接关闭线程
+				m_velodyne_lidar_device_status = E_READY;
+				m_execute_condition.notify_all(false);
+			}
+			else
+			{
+				if (mp_velodyne_lidar_scan_task->is_over_time())
+				{
+					t_error.error_manager_reset(TASK_TIMEOVER, MINOR_ERROR, "Velodyne_lidar_device::get_cloud_thread() TASK_TIMEOVER");
+					mp_velodyne_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
+					//故障结束任务
+					end_task();
+				}
+				else
+				{
+					//获取这个时间后面的点云.
+					if (m_parse_ring_time > mp_velodyne_lidar_scan_task->get_command_time())
+					{
+						std::mutex *tp_mutex = mp_velodyne_lidar_scan_task->get_task_cloud_lock();
+						pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud = mp_velodyne_lidar_scan_task->get_task_point_cloud();
+						{
+							std::unique_lock<std::mutex> lck(*tp_mutex);
+							{
+								std::lock_guard<std::mutex> lck(m_cloud_mutex);
+								for (size_t i = 0; i < m_ring_cloud.size(); i++)
+								{
+									tp_cloud->push_back(pcl::PointXYZ(m_ring_cloud[i].x, m_ring_cloud[i].y, m_ring_cloud[i].z));
+								}
+							}
+						}
+						mp_velodyne_lidar_scan_task->compare_and_cover_task_error_manager(t_error);
+						//正常结束任务
+						end_task();
+					}
+					else
+					{
+						//等1ms
+						std::this_thread::sleep_for(std::chrono::milliseconds(1));
+					}
+				}
+			}
+		}
+	}
+	LOG(INFO) << " Velodyne_lidar_device::execute_thread_fun() end :" << this;
+	return;
+}

velodyne_lidar/velodyne_driver/velodyne_lidar_device.h

@@ -0,0 +1,138 @@
+#ifndef VELODYNE_LIDAR_ENCAPSULATION_HH
+#define VELODYNE_LIDAR_ENCAPSULATION_HH
+
+#include "input.h"
+#include "rawdata.h"
+#include "velodyne_lidar_scan_task.h"
+#include "../velodyne_config.pb.h"
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <thread>
+
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_queue.h"
+#include "../tool/thread_condition.h"
+
+class Velodyne_lidar_device
+{
+public:
+	//万集设备身状态
+	enum Velodyne_lidar_device_status
+	{
+		E_UNKNOWN              	= 0,    //未知
+		E_READY	             	= 1,    //正常待机
+		E_DISCONNECT			= 2,	//断连
+
+		E_BUSY					= 3, 	//工作正忙
+
+		E_FAULT					=10,	//故障
+	};
+
+public:
+    // 无参构造函数
+    Velodyne_lidar_device();
+    // 析构函数
+    ~Velodyne_lidar_device();
+
+	// 初始化
+	Error_manager init(velodyne::velodyneLidarParams params);
+	//反初始化
+	Error_manager uninit();
+
+	//对外的接口函数，负责接受并处理任务单，
+	Error_manager execute_task(Task_Base* p_wanji_lidar_scan_task);
+
+	//检查雷达状态,是否正常运行
+	Error_manager check_status();
+	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+	Error_manager end_task();
+	//取消任务单，由发送方提前取消任务单
+	Error_manager cancel_task(Task_Base* p_wanji_lidar_scan_task);
+
+	//判断是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+
+	//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
+	Error_manager get_new_cloud_and_wait(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+							std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);
+
+	//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
+	// 注意！！！当前最新点云未成环，或许缺失数据
+	Error_manager get_current_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+									std::chrono::steady_clock::time_point command_time);
+	//外部调用获取最新的点云, 如果没有就会报错, 不会等待
+	Error_manager get_last_cloud(std::mutex* p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+							std::chrono::steady_clock::time_point command_time);
+
+	//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+	//注:函数内部不加锁, 由外部统一加锁.
+	Error_manager get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+								 std::chrono::steady_clock::time_point command_time);
+public:
+	Velodyne_lidar_device_status get_status();
+protected:
+
+	// 根据model名与转速，获得吞吐量信息，设置每圈packet个数
+	void dev_info_config(std::string model, int rpm);
+
+	void updata_status();
+	//mp_laser_manager_thread 线程执行函数，
+	//thread_work 内部线程负责获取点云结果
+	void execute_thread_fun();
+
+	// 获取点云线程函数
+	void capture_thread_fun();
+
+	// 解析点云线程函数
+	void parse_thread_fun();
+
+protected:
+	//状态
+	Velodyne_lidar_device_status m_velodyne_lidar_device_status;	//万集设备状态
+	Velodyne_lidar_device_status m_velodyne_socket_status;
+	Velodyne_lidar_device_status m_velodyne_protocol_status;
+
+	//子模块
+	velodyne_driver::InputSocket m_input_socket;					   //负责雷达通信, 接受雷达数据
+	velodyne_rawdata::RawData						m_protocol;        // velodyne雷达协议
+
+	//中间缓存
+	Thread_safe_queue<Binary_buf*>				m_communication_data_queue;	//通信数据队列
+
+	// 数据缓存
+	unsigned char m_buf[5000];
+	// 点云互斥锁
+	std::mutex m_cloud_mutex;
+	// 每圈packet个数
+	int m_num_packet_per_scan;
+	// packet获取计数
+	int m_num_packet_inside;
+	// 点云缓存
+	std::vector<Lidar_point> m_cloud_buf;
+	// 完整可获取点云
+	std::vector<Lidar_point> m_ring_cloud;
+
+	// 最新数据获取时间
+	std::chrono::steady_clock::time_point m_capture_time;
+	//点云获取线程
+	std::thread *mp_capture_thread;		  // 获取点云数据线程指针，内存由本类管理
+	Thread_condition m_capture_condition; // 获取点云数据条件变量
+
+	// 最新解析环点云时间
+	std::chrono::steady_clock::time_point m_parse_ring_time;
+	// 解析点云时间
+	std::chrono::steady_clock::time_point m_parse_time;
+	//点云解析线程
+	std::thread *mp_parse_thread; // 解析点云线程指针，内存由本类管理
+	Thread_condition m_parse_condition; // 解析点云条件变量
+
+	//任务执行线程
+	std::thread*        						mp_execute_thread;   			//执行的线程指针，内存由本类管理
+	Thread_condition							m_execute_condition;			//执行的条件变量
+
+	Velodyne_lidar_scan_task *  					mp_velodyne_lidar_scan_task;   //雷达设备任务单的指针，内存由发送方管理。
+
+};
+
+#endif // !VELODYNE_LIDAR_ENCAPSULATION_HH

velodyne_lidar/velodyne_driver/velodyne_lidar_scan_task.cpp

@@ -0,0 +1,114 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#include "velodyne_lidar_scan_task.h"
+
+//构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
+Velodyne_lidar_scan_task::Velodyne_lidar_scan_task()
+{
+	//构造函数锁定任务类型为 WANJI_LIDAR_SCAN，后续不允许更改
+	m_task_type = WANJI_LIDAR_SCAN;
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	mp_task_point_cloud = NULL;
+	mp_task_cloud_lock = NULL;
+}
+//析构函数
+Velodyne_lidar_scan_task::~Velodyne_lidar_scan_task()
+{
+
+}
+
+//初始化任务单，必须初始化之后才可以使用，（必选参数）
+// input:p_task_cloud_lock 三维点云的数据保护锁
+// output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Velodyne_lidar_scan_task::task_init(std::chrono::steady_clock::time_point command_time,
+											   std::mutex* p_task_cloud_lock,
+											   pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
+{
+	if(p_task_point_cloud.get() == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Velodyne_lidar_scan_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_command_time = command_time;
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
+
+	return Error_code::SUCCESS;
+}
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_frame_maxnum 点云的采集帧数最大值
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Velodyne_lidar_scan_task::task_init(Task_statu task_statu,
+											   std::string task_statu_information,
+											   void* p_tast_receiver,
+											   std::chrono::milliseconds task_over_time,
+											   std::chrono::steady_clock::time_point command_time,
+											   std::mutex* p_task_cloud_lock,
+											   pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud
+)
+{
+	if(p_task_point_cloud.get() == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Velodyne_lidar_scan_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = task_statu;
+	m_task_statu_information = task_statu_information;
+	mp_tast_receiver = p_tast_receiver;
+	m_task_over_time = task_over_time;
+	m_task_error_manager.error_manager_clear_all();
+
+	m_command_time = command_time;
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
+
+	return Error_code::SUCCESS;
+}
+
+std::chrono::steady_clock::time_point Velodyne_lidar_scan_task::get_command_time()
+{
+	return m_command_time;
+}
+
+
+//获取 三维点云容器的智能指针
+std::mutex*  Velodyne_lidar_scan_task::get_task_cloud_lock()
+{
+	return mp_task_cloud_lock;
+}
+//获取 三维点云容器的智能指针
+pcl::PointCloud<pcl::PointXYZ>::Ptr Velodyne_lidar_scan_task::get_task_point_cloud()
+{
+	return mp_task_point_cloud;
+}

velodyne_lidar/velodyne_driver/velodyne_lidar_scan_task.h

@@ -0,0 +1,75 @@
+//
+// Created by huli on 2020/9/8.
+//
+
+#ifndef VELODYNE_LIDAR_TASK_H
+#define VELODYNE_LIDAR_TASK_H
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include "../error_code/error_code.h"
+
+#include <vector>
+#include <mutex>
+
+#include "../task/task_command_manager.h"
+
+
+
+//万集雷达模块的任务指令，从Task_Base继承，
+class Velodyne_lidar_scan_task:public Task_Base
+{
+public:
+	Velodyne_lidar_scan_task();
+	Velodyne_lidar_scan_task(const Velodyne_lidar_scan_task& other)= default;
+	Velodyne_lidar_scan_task& operator =(const Velodyne_lidar_scan_task& other)= default;
+	~Velodyne_lidar_scan_task();
+public://API functions
+
+	//初始化任务单，必须初始化之后才可以使用，（必选参数）
+	// input:p_task_cloud_lock 三维点云的数据保护锁
+	// output:p_task_point_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager task_init(std::chrono::steady_clock::time_point command_time,
+							std::mutex* p_task_cloud_lock,
+							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
+
+	//初始化任务单，必须初始化之后才可以使用，（可选参数）
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	//    input:p_task_cloud_lock 三维点云的数据保护锁
+	//    output:p_task_pmutexoint_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager task_init(Task_statu task_statu,
+							std::string task_statu_information,
+							void* p_tast_receiver,
+							std::chrono::milliseconds task_over_time,
+							std::chrono::steady_clock::time_point command_time,
+							std::mutex* p_task_cloud_lock,
+							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud
+	);
+public://get or set member variable
+
+	std::chrono::steady_clock::time_point get_command_time();
+	//获取 三维点云容器的智能指针;
+	std::mutex*  get_task_cloud_lock();
+	//获取 三维点云容器的智能指针
+	pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
+
+protected://member variable
+	//下发指令的时间点. 从command_time提前一个扫描周期, 然后取最新的点
+	std::chrono::steady_clock::time_point 		m_command_time;
+
+	//三维点云的数据保护锁，任务输入, (实际上多个雷达会同时往一个pcl点云容器里面填充数据,防止数据冲突,需要对容器加锁.)
+	std::mutex*                     			mp_task_cloud_lock;
+	//采集结果，三维点云容器的智能指针，任务输出
+	//这里只是智能指针，实际内存由任务发送方管理管理，初始化时必须保证内存有效。
+	pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+
+private:
+
+};
+
+
+#endif //VELODYNE_LIDAR_TASK_H

velodyne_lidar/velodyne_manager.cpp

@@ -0,0 +1,7 @@
+/*
+ * @Description: 
+ * @Author: yct
+ * @Date: 2021-07-23 16:39:04
+ * @LastEditTime: 2021-07-23 16:39:05
+ * @LastEditors: yct
+ */

velodyne_lidar/velodyne_manager.h

@@ -0,0 +1,65 @@
+/*
+ * @Description: velodyne多线雷达管理类
+ * @Author: yct
+ * @Date: 2021-07-23 16:37:33
+ * @LastEditTime: 2021-07-27 16:04:00
+ * @LastEditors: yct
+ */
+
+#ifndef VELODYNE_MANAGER_HH
+#define VELODYNE_MANAGER_HH
+#include <thread>
+#include <map>
+
+#include "../tool/singleton.h"
+#include "../error_code/error_code.h"
+
+
+
+class Velodyne_manager : public Singleton<Velodyne_manager>
+{
+    // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+    friend class Singleton<Velodyne_manager>;
+    //万集管理任务超时时间1000ms,
+    #define VELODYNE_MANAGER_EXECUTE_TIMEOUT_MS 1000
+    //万集配置参数的默认路径
+    #define VELODYNE_MANAGER_PARAMETER_PATH "../setting/velodyne_manager.prototxt"
+	//雷达管理的状态
+	enum Velodyne_manager_status
+	{
+		E_UNKNOWN              	= 0,    //未知
+		E_READY               	= 1,    //准备，待机
+		E_BUSY					= 2, 	//工作正忙
+
+		E_ISSUED_SCAN			= 3, 	//下发任务, 获取点云
+		E_WAIT_SCAN				= 4,	//等待任务, 扫描点云
+
+		E_ISSUED_DETECT			= 5, 	//下发任务, 算法预测
+		E_WAIT_DETECT			= 6, 	//等待任务, 算法预测
+
+		E_FAULT					= 10,	//故障
+	};
+public:
+    // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Velodyne_manager(const Velodyne_manager &) = delete;
+    Velodyne_manager &operator=(const Velodyne_manager &) = delete;
+    ~Velodyne_manager() = default;
+
+    //初始化 雷达 管理模块。如下三选一
+	Error_manager velodyne_manager_init();
+	//初始化 雷达 管理模块。从文件读取
+	Error_manager velodyne_manager_init_from_protobuf(std::string prototxt_path);
+	//初始化 雷达 管理模块。从protobuf读取
+	Error_manager velodyne_manager_init_from_protobuf(wj::wjManagerParams& velodyne_parameters);
+
+    // 反初始化
+    Error_manager Velodyne_manager_uninit();
+
+private:
+    // 父类的构造函数必须保护，子类的构造函数必须私有。
+    Velodyne_manager() = default;
+
+    
+};
+
+#endif // !VELODYNE_MANAGER_HH