缺少未添加雷达解析和结果广播

CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(LidarMeasure)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++14)
+
+#add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+
+FIND_PACKAGE(PCL REQUIRED)
+FIND_PACKAGE(Protobuf REQUIRED)
+find_package(OpenCV REQUIRED)
+FIND_PACKAGE(Glog REQUIRED)
+FIND_PACKAGE(Ceres REQUIRED)
+set(CMAKE_MODULE_PATH "/usr/local/share/")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wl,--no-as-needed")
+set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+set(CMAKE_BUILD_TYPE "RELEASE")
+
+include_directories(
+        src/tool
+        src/error_code
+        /usr/local/include
+        /usr/local/include/modbus
+        /usr/local/include/snap7
+        ${OpenCV_INCLUDE_DIRS}
+        ${PCL_INCLUDE_DIRS}
+        ${CERES_INCLUDE_DIRS}
+)
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/tool TOOL_SRC )
+
+link_directories("/usr/local/lib")
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src/error_code ERROR_SRC )
+
+add_executable(vlp16_sample  src/vlp16_sample.cpp ${ERROR_SRC} ${TOOL_SRC}
+        src/vlp16.hpp src/ground_region.cpp
+        src/configure.pb.cc src/detect_wheel_ceres.cpp)
+target_link_libraries(vlp16_sample ${CERES_LIBRARIES} ${OpenCV_LIBS}
+        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES}
+        /usr/local/lib/libgflags.a /)
+
+

proto.sh

@@ -0,0 +1 @@
+protoc -I=./src configure.proto --cpp_out=./src

src/configure.proto

@@ -0,0 +1,36 @@
+syntax = "proto2";
+package ground_region;
+
+
+message Calib_parameter
+{
+    optional float r=1 [default=0];
+    optional float p=2 [default=0];
+    optional float y=3 [default=0];
+    optional float cx=4 [default=0];
+    optional float cy=5 [default=0];
+    optional float cz=6 [default=0];
+}
+
+message Lidar_configure
+{
+    required string             ip=1;
+    required int32              port=2;
+    optional Calib_parameter    calib=3;
+}
+
+message Box
+{
+    required float      minx=1;
+    required float      maxx=2;
+    required float      miny=3;
+    required float      maxy=4;
+    required float      minz=5;
+    required float      maxz=6;
+
+}
+
+message Configure {
+    repeated Lidar_configure     lidar=1;
+    required Box                box=2;
+}

src/detect_wheel_ceres.cpp

@@ -0,0 +1,895 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#include "detect_wheel_ceres.h"
+#include <ceres/cubic_interpolation.h>
+#include <pcl/common/transforms.h>
+
+
+constexpr float kMinProbability = 0.01f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+class GridArrayAdapter {
+public:
+    enum { DATA_DIMENSION = 1 };
+
+    explicit GridArrayAdapter(const cv::Mat& grid) : grid_(grid) {}
+
+    void GetValue(const int row, const int column, double* const value) const {
+        if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
+            column >= NumCols() - kPadding) {
+            *value = kMaxCorrespondenceCost;
+        } else {
+            *value = static_cast<double>(grid_.at<double >(row - kPadding, column - kPadding));
+        }
+    }
+
+    int NumRows() const {
+        return grid_.rows + 2 * kPadding;
+    }
+
+    int NumCols() const {
+        return grid_.cols + 2 * kPadding;
+    }
+private:
+    const cv::Mat& grid_;
+};
+
+class CostFunctor {
+private:
+    cv::Mat  m_map;
+    double m_scale;
+    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:
+    CostFunctor(pcl::PointCloud<pcl::PointXYZ> left_front, pcl::PointCloud<pcl::PointXYZ> right_front,
+                pcl::PointCloud<pcl::PointXYZ> left_rear, pcl::PointCloud<pcl::PointXYZ> right_rear,
+                cv::Mat &map, double scale)
+    {
+        m_map = map;
+        m_scale = scale;
+        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 costs[4]={T(0),T(0),T(0),T(0)};
+	std::vector<std::vector<T> > deviation_calc_vector;
+	deviation_calc_vector.resize(4);
+	T deviation_weight = T(0.05);
+
+        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];
+        T theta_front_weight = T(0.8);
+//        T wheel_length_img_ratio = variable[6];
+        T wheel_width_length_ratio = variable[6];
+        // [1/3, 5/3]
+        T limited_wheel_length_img_ratio = T(0.85);// = T(0.7) / (T(1) + ceres::exp(T(-wheel_length_img_ratio))) + T(0.85);
+        // [0.25, 0.35]
+        T limited_wheel_length_ratio = T(0.25)/(T(1) + ceres::exp(T(-wheel_width_length_ratio))) + T(0.25);
+
+        //整车旋转
+        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();
+
+
+        const GridArrayAdapter adapter(m_map);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        double rows = m_map.rows;
+        double cols = m_map.cols;
+        //左前轮
+        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;
+            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[i]);
+            //residual[i] += theta_front*theta_front_weight;
+            costs[0] += residual[i];
+	    deviation_calc_vector[0].push_back(residual[i]);
+        }
+        //右前轮
+        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;
+            interpolator.Evaluate(point_rotation(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio* m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  point_rotation(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + i]);
+            //residual[left_front_num + i] += theta_front*theta_front_weight;
+            costs[1] += residual[left_front_num+i];
+	    deviation_calc_vector[1].push_back(residual[left_front_num+i]);
+        }
+        //左后轮
+        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;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + i]);
+            costs[2] += residual[left_front_num + right_front_num + i];
+	    deviation_calc_vector[2].push_back(residual[left_front_num + right_front_num + i]);
+        }
+
+        //右后轮
+	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;
+
+            interpolator.Evaluate(tanslate_point(1, 0) * limited_wheel_length_img_ratio / limited_wheel_length_ratio * m_scale + rows / 2.0 + 0.5 + T(kPadding),
+                                  tanslate_point(0, 0) * limited_wheel_length_img_ratio * m_scale + cols / 2.0 + 0.5 + T(kPadding),
+                                  &residual[left_front_num + right_front_num + left_rear_num + i]);
+            costs[3] += residual[left_front_num + right_front_num + left_rear_num + i];
+	    deviation_calc_vector[3].push_back(residual[left_front_num + right_front_num + left_rear_num + i]);
+        }
+
+	// 前轮旋转loss
+	residual[left_front_num + right_front_num + left_rear_num + right_rear_num] = theta_front*theta_front_weight;
+	// 四轮方差loss
+	residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] = T(0);
+	//printf("--------------\n");
+	T avg_costs[4] = {T(costs[0]/T(left_front_num)), T(costs[1]/T(right_front_num)), T(costs[2]/T(left_rear_num)), T(costs[3]/T(right_rear_num))};
+	for (int i = 0; i < deviation_calc_vector.size(); ++i) {
+		T t_deviation = T(0);
+		for (int j = 0; j < deviation_calc_vector[i].size(); ++j) {
+			t_deviation += deviation_weight * ceres::abs(deviation_calc_vector[i][j] - avg_costs[i]);
+		}
+		residual[left_front_num + right_front_num + left_rear_num + right_rear_num+1] += t_deviation;
+		//printf("??? dev: %.6f\n", t_deviation);
+		//costs[i] = t_deviation;
+		//printf("dev: %.6f\n", costs[i]);
+	}
+
+            char buf[30];
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[0]);
+            double t_costs_lf = std::stod(buf) / left_front_num;
+
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[1]);
+            double t_costs_rf = std::stod(buf) / right_front_num;
+
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[2]);
+            double t_costs_lr = std::stod(buf) / left_rear_num;
+
+            memset(buf, 0, 30);
+            sprintf(buf, "%.12f", costs[3]);
+            double t_costs_rr = std::stod(buf) / right_rear_num;
+            if(t_costs_lf >= 0.001 && t_costs_rf >= 0.001 && t_costs_lr >= 0.001 && t_costs_rr >= 0.001)
+            {
+                m_costs_lf=t_costs_lf;
+                m_costs_rf=t_costs_rf;
+                m_costs_lr=t_costs_lr;
+                m_costs_rr=t_costs_rr;
+            }
+        // m_costs_lf = costs[0];
+	//printf("%.6f %.6f %.6f %.6f\n",m_costs_lf, m_costs_rf, m_costs_lr, m_costs_rr);
+        return true;
+    }
+    void get_costs(double &lf, double &rf, double &lr, double &rr)
+    {
+        lf = m_costs_lf;
+        rf = m_costs_rf;
+        lr = m_costs_lr;
+        rr = m_costs_rr;
+    }
+};
+
+bool detect_wheel_ceres::update_mat(int rows, int cols)
+{
+    /////创建地图
+    int L=std::min(rows,cols);
+    cv::Mat map=cv::Mat::ones(L,L,CV_64F);
+    //map=map*255;
+    cv::Point center(L/2,L/2);
+
+
+    float K=L*0.08;
+    // 从中心开始向外画矩形
+    // 内层高斯，外层二次函数
+    for(int n=0;n<L;n+=2)
+    {
+        cv::Size size(n+2,n+2);
+        cv::Rect rect(center-cv::Point(n/2,n/2),size);
+        double x = n / double(L);
+        double sigma = 0.05;
+        double miu = 0.0;
+        double scale = 0.02;
+        double translation = 0.03;
+        double gauss_value = (scale/(sqrt(2*M_PI)*sigma))*exp(-pow(x-miu, 2)/(2*sigma*sigma))+translation;
+        double quadratic_value = std::max(2*x-0.25, 0.045);
+        // LOG(INFO) << rect.tl().x<<", "<<rect.tl().y<<", "<<rect.br().x<<", "<<rect.br().y;
+        // 对内外分别取色
+        cv::rectangle(map,rect,std::max(gauss_value, quadratic_value));
+
+        // if(n<K*2){
+        //     cv::rectangle(map,rect,1.0*float(K*2-n)/float(L));
+        // }
+        // else{
+        //     cv::rectangle(map,rect,float(n-K*2)/float(L));
+        // }
+
+    }
+    cv::resize(map,map,cv::Size(cols,rows));
+    cv::GaussianBlur(map,m_map,cv::Size(7,7),3,3);
+}
+
+detect_wheel_ceres::detect_wheel_ceres()
+{
+    int cols=800;
+    int rows=800;
+    update_mat(rows, cols);
+}
+detect_wheel_ceres::~detect_wheel_ceres(){}
+
+
+
+bool detect_wheel_ceres::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.theta=-angle_x*M_PI/180.0;
+    // Loss_result one_shot_loss_result;
+    // return Solve(detect_result, one_shot_loss_result);
+
+    // 多次优化，获取最佳优化结果
+    detect_result.cx=center_x;
+    detect_result.cy=center_y;
+    // 已计算次数，初始传入值正常，之后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;
+    // for (int j = 2; j < 4&&!stop_sign; j++)
+    // {
+    //     // double input_vars[] = {detect_result.cx, detect_result.cy, detect_result.theta, detect_result.wheel_base, detect_result.width, detect_result.front_theta};
+    //     double map_rows = map_cols * 1.0 / j ;
+    //     update_mat(map_rows, map_cols);
+
+    // !!! 将车轮比例添加到优化模型中
+        Detect_result t_detect_result = detect_result;
+       // 寻找最小loss值对应的初始旋转角
+        for (int i = -3; i < 4&&!stop_sign; i+=6)
+        {
+            t_detect_result.theta = (-angle_x + i) * M_PI / 180.0;
+            // printf("double x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",input_vars[0],input_vars[1],input_vars[3],input_vars[4],input_vars[2],input_vars[5]);
+            Loss_result t_loss_result;
+            t_loss_result.total_avg_loss = 1000;
+            //输出角度已变化，需恢复成弧度
+            if(calc_count > 0)
+            {
+                // t_detect_result.theta *= (-M_PI) / 180.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)
+                continue;
+            // LOG(INFO) << "avg loss, current theta: " << t_loss << ", " << input_vars[2];
+            if (avg_loss > t_loss_result.total_avg_loss)
+            {
+                avg_loss = t_loss_result.total_avg_loss;
+                // final_theta = -input_vars[2] * M_PI / 180.0;
+                detect_result = t_detect_result;
+                solve_result = current_result;
+                loss_result = t_loss_result;
+                // optimized_map_rows = map_rows;
+                calc_count++;
+                // // 新增，优化时间足够长则认为已找到正确结果
+                // if(second > 0.02)
+                // {
+                //     stop_sign = true;
+                //     break;
+                // }
+            }
+        }
+    // }
+    // std::cout<<"solve time "<<total_solve_time<<std::endl;
+    // LOG(INFO) << "final avg cost for four wheels: "<<whole_loss[0]<<" "<<whole_loss[1]<<" "<<whole_loss[2]<<" "<<whole_loss[3]<<" ";
+
+//    if(solve_result)
+//        printf("final avg cost for four wheels: %7.6f %7.6f %7.6f %7.6f---%7.6f\n", loss_result.lf_loss, loss_result.rf_loss, loss_result.lb_loss, loss_result.rb_loss, loss_result.total_avg_loss);
+    
+    // // 生成并保存图片
+    // update_mat(optimized_map_rows, map_cols);
+    // Detect_result t_detect_result = detect_result;
+    // t_detect_result.theta *= (-M_PI) / 180.0;
+    // t_detect_result.front_theta *= (-M_PI) / 180.0;
+    // Loss_result t_loss_result;
+    // // LOG(INFO) <<"going to show img ";
+    // Solve(detect_result, t_loss_result, true);
+
+//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+//         detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
+    if(solve_result)
+        error_info="";
+    return solve_result;
+
+}
+
+// 根据测量结果，生成四轮各中心十字星点云
+void detect_wheel_ceres::transform_src(Detect_result detect_result)
+{
+
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(-detect_result.theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    //左前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(detect_result.wheel_base / 2.0, detect_result.width / 2.0);
+    //右前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(detect_result.wheel_base / 2.0, -detect_result.width / 2.0);
+    //左后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-detect_result.wheel_base / 2.0, detect_result.width / 2.0);
+    //右后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-detect_result.wheel_base / 2.0, -detect_result.width / 2.0);
+
+
+    Eigen::Matrix<double, 2, 1> translate_car(detect_result.cx,detect_result.cy);
+
+    const Eigen::Matrix<double, 2, 1> lt_center = rotation_matrix * wheel_center_normal_left_front+translate_car;
+    const Eigen::Matrix<double, 2, 1> rt_center = rotation_matrix * wheel_center_normal_right_front+translate_car;
+    const Eigen::Matrix<double, 2, 1> lb_center = rotation_matrix * wheel_center_normal_left_rear+translate_car;
+    const Eigen::Matrix<double, 2, 1> rb_center = rotation_matrix * wheel_center_normal_right_rear+translate_car;
+
+    create_mark(lt_center(0,0),lt_center(1,0),-detect_result.theta-detect_result.front_theta,m_left_front_cloud_out);
+    create_mark(rt_center(0,0),rt_center(1,0),-detect_result.theta-detect_result.front_theta,m_right_front_cloud_out);
+    create_mark(lb_center(0,0),lb_center(1,0),-detect_result.theta,m_left_rear_cloud_out);
+    create_mark(rb_center(0,0),rb_center(1,0),-detect_result.theta,m_right_rear_cloud_out);
+}
+
+// 创建车轮中心十字星点云
+void detect_wheel_ceres::create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud_out)
+{
+    cloud_out.clear();
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+
+    //std::cout<<"1111111111"<<std::endl;
+    int width=30;
+    int height=10;
+    double step=0.015;
+
+    for(int i=0;i<width;++i)
+    {
+        pcl::PointXYZ point((i-width/2)*step,0,0);
+        cloud.push_back(point);
+    }
+    for(int i=0;i<height;++i)
+    {
+        pcl::PointXYZ point(0,(i-height/2)*step,0);
+        cloud.push_back(point);
+    }
+    //std::cout<<"22222222222"<<std::endl;
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(theta);
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    for(int i=0;i<cloud.size();++i) {
+
+        const Eigen::Matrix<double, 2, 1> point((double(cloud[i].x)),
+                                                (double(cloud[i].y)));
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point;
+        pcl::PointXYZ point_out(tanslate_point(0,0)+x,tanslate_point(1,0)+y,0);
+        cloud_out.push_back(point_out);
+
+    }
+}
+
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, double &avg_loss)
+{
+    double SCALE=300.0;
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
+    double init_wheel_base=2.7;
+    double init_width=1.55;
+    double init_theta_front=0*M_PI/180.0;
+
+    double variable[] = {cx,cy,init_theta,init_wheel_base,init_width,init_theta_front};
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 6>(
+                    new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE),
+                    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.max_num_iterations=60;
+    options.num_threads=1;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+
+    /*printf("x:%.3f,y:%.3f,theta:%.3f\nlength:%.3f,width:%.3f\ntheta_front:%.3f\n",
+           x[0],x[1],x[2]*180.0/M_PI,x[3],x[4],x[5]*180.0/M_PI);*/
+
+    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(loss>0.01)
+        return false;
+    if (detect_result.width < 1.350 || detect_result.width > 2.000 || detect_result.wheel_base > 3.000 || detect_result.wheel_base < 2.200)
+    {
+        return false;
+    }
+
+    detect_result.width+=0.15;//车宽+10cm
+    // printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",x,y,wheel_base,width,theta,front_theta);
+
+    // //added by yct
+    avg_loss = loss; // 将loss传出
+
+    Detect_result t_detect_result = detect_result;
+    t_detect_result.theta *= -M_PI/180.0;
+    t_detect_result.front_theta *= -M_PI/180.0;
+    transform_src(t_detect_result);
+
+    return true;
+
+}
+
+bool detect_wheel_ceres::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info, bool save_img)
+{
+    double SCALE=300.0;
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
+    double init_wheel_base=2.7;
+    double init_width=1.55;
+    double init_theta_front=0*M_PI/180.0;
+    // 车轮长图比, 比例取值1/3-5/3, 当前变量无限制
+//    double init_wheel_length_img_ratio = 0.1;
+    // 车轮宽长比, 默认长为1,比例取值范围0.2-0.8, 当前变量范围无限制
+    double init_wheel_width_length_ratio = -0.5;
+
+    double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front, init_wheel_width_length_ratio};
+    // printf("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;
+    CostFunctor *cost_func = new CostFunctor(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_map,SCALE);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor, ceres::DYNAMIC, 7>(
+                    cost_func,
+                    m_left_front_cloud.size()+m_right_front_cloud.size()+m_left_rear_cloud.size()+m_right_rear_cloud.size()+2);
+    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=60;
+    options.num_threads=3;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+//    if(!summary.IsSolutionUsable())
+//    {
+//        error_info.append("检测失败\t");
+//        return false;
+//    }
+    // std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
+    // debug_img(detect_result, loss_result, true);
+    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;
+
+    //检验
+    // printf("loss: %.5f\n", loss);
+    // printf("middle 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]);
+    if(loss>0.0115)
+    {
+        error_info.append("总loss过大 ").append(std::to_string(loss)).append("\t");
+//        LOG(WARNING) <<"总loss过大 "<<loss;
+        return false;
+    }
+    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")));
+//        LOG(WARNING) <<"宽度(1.35, 2.00) 轴距(2.20, 3.30): "<<detect_result.width<<", "<<detect_result.wheel_base;
+        return false;
+    }
+
+    detect_result.width += 0.22; //车宽一边+11cm
+//     printf(" x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+//             detect_result.cx,detect_result.cy,detect_result.wheel_base,detect_result.width,detect_result.theta,detect_result.front_theta);
+
+    // //added by yct
+    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");
+//        LOG(WARNING) <<"前轮角度错误 "<<detect_result.front_theta;
+        return false;
+    }
+    // 将loss传出
+    if(cost_func!=nullptr)
+    {
+        
+        double costs_lf, costs_rf, costs_lr, costs_rr;
+        cost_func->get_costs(costs_lf, costs_rf, costs_lr, costs_rr);
+        // std::cout << "111"<< std::endl;
+        loss_result.lf_loss = costs_lf;
+        loss_result.rf_loss = costs_rf;
+        loss_result.lb_loss = costs_lr;
+        loss_result.rb_loss = costs_rr;
+        loss_result.total_avg_loss = loss;
+        // std::cout << "222"<< std::endl;
+        if (costs_lf == costs_rf && costs_lf == costs_lf && costs_lf == costs_rr)// (costs_lf <= 1e-6 && costs_rf <= 1e-6 && costs_lr <= 1e-6 && costs_rr <= 1e-6)
+        {
+	    return false;
+            loss_result.lf_loss = loss;
+            loss_result.rf_loss = loss;
+            loss_result.lb_loss = loss;
+            loss_result.rb_loss = loss;
+        }
+        // 判断每个轮子平均loss是否满足条件
+        double single_wheel_loss_threshold = 0.135;
+        if(loss_result.lf_loss > single_wheel_loss_threshold || loss_result.rf_loss > single_wheel_loss_threshold  || loss_result.lb_loss > single_wheel_loss_threshold || loss_result.rb_loss > single_wheel_loss_threshold)
+        {
+            error_info.append("四轮loss过大 ").append(std::to_string(loss_result.lf_loss)).append(", ")
+                    .append(std::to_string(loss_result.rf_loss)).append(", ")
+                    .append(std::to_string(loss_result.lb_loss)).append(", ")
+                    .append(std::to_string(loss_result.rb_loss)).append("\t");
+//            LOG(WARNING)<<error_info;
+//            LOG(WARNING) <<"四轮loss过大"<<loss_result.lf_loss<<", "<<loss_result.rf_loss<<", "<<loss_result.lb_loss<<", "<<loss_result.rb_loss;
+            return false;
+        }
+        //std::cout<<"save img: "<<save_img<<std::endl;
+       //debug_img(detect_result, loss_result, save_img);
+    }
+    else
+    {
+        return false;
+    }
+
+    Detect_result t_detect_result = detect_result;
+    t_detect_result.theta *= -M_PI/180.0;
+    t_detect_result.front_theta *= -M_PI/180.0;
+    t_detect_result.width -= 0.22;
+    transform_src(t_detect_result);
+
+
+
+    return true;
+}
+
+// 显示/保存图片供调试用
+void detect_wheel_ceres::debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img, std::string out_img_path)
+{
+    double SCALE=300.0;
+    cv::Mat lf = m_map.clone();
+    cv::Mat rf = m_map.clone();
+    cv::Mat lb = m_map.clone();
+    cv::Mat rb = m_map.clone();
+    //整车旋转
+    Eigen::Rotation2D<double> rotation(detect_result.theta/180.0*M_PI);
+
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+    //左前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(detect_result.wheel_base / 2.0, (detect_result.width - 0.22) / 2.0);
+    //右前偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(detect_result.wheel_base / 2.0, (-detect_result.width + 0.22) / 2.0);
+    //左后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-detect_result.wheel_base / 2.0, (detect_result.width - 0.22) / 2.0);
+    //右后偏移
+    Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-detect_result.wheel_base / 2.0, (-detect_result.width + 0.22) / 2.0);
+
+    //前轮旋转
+    Eigen::Rotation2D<double> rotation_front(detect_result.front_theta/180.0*M_PI);
+    Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+    // 左前轮
+    for (size_t i = 0; i < m_left_front_cloud.size(); i++)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) - detect_result.cx),
+                                          (double(m_left_front_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+        //旋转
+        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(lf, cv::Point(c, r), 2, cv::Scalar(150));
+        // std::cout<<"map r,c "<<m_map.rows<<", "<<m_map.cols<<std::endl;
+        // std::cout<<"r,c "<<r<<", "<<c<<std::endl;
+        // lf.at<uchar>(r, c) = 150;
+    }
+    //右前轮
+    for (int i = 0; i < m_right_front_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) - detect_result.cx),
+                                          (double(m_right_front_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
+        //旋转
+        Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+        int r = (int)(point_rotation(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(point_rotation(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(rf, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+    //左后轮
+    for (int i = 0; i < m_left_rear_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) - detect_result.cx),
+                                          (double(m_left_rear_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(lb, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+
+    //右后轮
+    for (int i = 0; i < m_right_rear_cloud.size(); ++i)
+    {
+        Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) - detect_result.cx),
+                                          (double(m_right_rear_cloud[i].y) - detect_result.cy));
+        //减去经过车辆旋转计算的左前中心
+        Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+        int r = (int)(tanslate_point(1, 0) * SCALE + m_map.rows / 2.0 + 0.5);
+        int c = (int)(tanslate_point(0, 0) * SCALE + m_map.cols / 2.0 + 0.5);
+        cv::circle(rb, cv::Point(c, r), 1, cv::Scalar(150));
+    }
+
+    // cv::Mat rot90;// = (cv::Mat_<double>(2, 3)<<cos(90), -sin(90), 0, sin(90), cos(90), 0);
+    // rot90 = cv::getRotationMatrix2D(cv::Point2f(lf.cols/2, lf.rows/2), 90, 1);
+    // // std::cout<<rot90<<std::endl;
+    // cv::warpAffine(lf, lf, rot90, cv::Size(lf.cols, lf.rows));
+    // cv::flip()
+    // cv::namedWindow("left front", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("right front", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("left back", cv::WINDOW_FREERATIO);
+    // cv::namedWindow("right back", cv::WINDOW_FREERATIO);
+    cv::flip(lf, lf, -1);
+    cv::flip(rf, rf, -1);
+    cv::flip(lb, lb, -1);
+    cv::flip(rb, rb, -1);
+    cv::Mat lft = lf.t(), rft = rf.t(), lbt = lb.t(), rbt = rb.t();
+    // cv::imshow("left front", lf.t());
+    // cv::imshow("right front", rf.t());
+    // cv::imshow("left back", lb.t());
+    // cv::imshow("right back", rb.t());
+    // 写入各轮平均误差
+    cv::putText(lft, (boost::format("lf %.6f") % (loss_result.lf_loss)).str().c_str(),
+                cv::Point(lft.cols / 6, lft.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(rft, (boost::format("rf %.6f") % (loss_result.rf_loss)).str().c_str(),
+                cv::Point(rft.cols / 6, rft.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(lbt, (boost::format("lb %.6f") % (loss_result.lb_loss)).str().c_str(),
+                cv::Point(lbt.cols / 6, lbt.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+    cv::putText(rbt, (boost::format("rb %.6f") % (loss_result.rb_loss)).str().c_str(),
+                cv::Point(rbt.cols / 6, rbt.rows / 4),
+                cv::FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 0));
+
+    cv::Mat total_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, m_map.type());
+    lft.copyTo(total_img(cv::Rect(0, 0, m_map.rows, m_map.cols)));
+    rft.copyTo(total_img(cv::Rect(m_map.rows, 0, m_map.rows, m_map.cols)));
+    lbt.copyTo(total_img(cv::Rect(0, m_map.cols, m_map.rows, m_map.cols)));
+    rbt.copyTo(total_img(cv::Rect(m_map.rows, m_map.cols, m_map.rows, m_map.cols)));
+
+    if(save_img)
+    {
+        cv::resize(total_img,total_img,total_img.size()/2);
+        cv::namedWindow("total img", CV_WINDOW_AUTOSIZE);
+        cv::imshow("total img", total_img);
+        cv::waitKey(2);
+        /*cv::Mat cvted_img = cv::Mat::zeros(m_map.cols * 2, m_map.rows * 2, CV_8U);
+        int total_milli = (std::chrono::time_point_cast<std::chrono::milliseconds>(std::chrono::system_clock::now())).time_since_epoch().count();
+        std::string img_filename="";
+        if(out_img_path.empty()){
+            img_filename = std::string("/home/zx/zzw/catkin_ws/src/feature_extra/debug/img").append(std::to_string(total_milli)).append(".jpg");
+        }
+        else{
+            img_filename = out_img_path;
+        }
+        LOG(INFO) << "write to " << img_filename.c_str();
+        // cv::cvtColor(total_img*255, cvted_img, CV_8U);
+        cv::convertScaleAbs(total_img * 255, cvted_img);
+        cv::imwrite(img_filename, cvted_img);*/
+    }
+}

src/detect_wheel_ceres.h

@@ -0,0 +1,90 @@
+//
+// 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"
+
+class detect_wheel_ceres {
+public:
+    struct Detect_result
+    {
+        public: 
+        double cx;
+        double cy;
+        double theta;
+        double wheel_base;
+        double width;
+        double front_theta; 
+        bool success; 
+        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;
+            return *this;
+        }
+    };
+    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;
+        }
+    };
+    detect_wheel_ceres();
+    ~detect_wheel_ceres();
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_vec, Detect_result &detect_result, std::string &error_info);
+
+protected:
+    void transform_src(Detect_result detect_result);
+    bool update_mat(int rows, int cols);
+    bool Solve(Detect_result &detect_result, double &avg_loss);
+    bool Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info, bool save_img=false);
+    void debug_img(Detect_result detect_result, Loss_result loss_result, bool save_img=false, std::string out_img_path="");
+
+    void create_mark(double x,double y,double theta,pcl::PointCloud<pcl::PointXYZ>& cloud);
+public:
+    pcl::PointCloud<pcl::PointXYZ>          m_left_front_cloud_out;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_front_cloud_out;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>          m_left_rear_cloud_out;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>          m_right_rear_cloud_out;     //右后点云
+
+protected:
+    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;     //右后点云
+    cv::Mat                                 m_map;                  //保存一份地图用作匹配
+
+
+};
+
+
+#endif //CERES_TEST_DETECT_WHEEL_CERES_H

src/error_code/error_code.cpp

@@ -0,0 +1,526 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#include "error_code.h"
+
+/////////////////////////////////////////////
+//构造函数
+Error_manager::Error_manager()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    pm_error_description = 0;
+    m_description_length = 0;
+    return ;
+}
+//拷贝构造
+Error_manager::Error_manager(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level,
+    const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//赋值构造
+Error_manager::Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    pm_error_description = NULL;
+    m_description_length = 0;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//析构函数
+Error_manager::~Error_manager()
+{
+    free_description();
+}
+
+//初始化
+void Error_manager::error_manager_init()
+{
+    error_manager_clear_all();
+    return;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//初始化
+void Error_manager::error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level, const char* p_error_description, int description_length)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string)
+{
+    m_error_code = error_code;
+    m_error_level = error_level;
+    reallocate_memory_and_copy_string(error_aggregate_string);
+    return ;
+}
+//重置
+void Error_manager::error_manager_reset(const Error_manager & error_manager)
+{
+    this->m_error_code = error_manager.m_error_code;
+    this->m_error_level = error_manager.m_error_level;
+    reallocate_memory_and_copy_string(error_manager.pm_error_description, error_manager.m_description_length);
+    return ;
+}
+//清除所有内容
+void Error_manager::error_manager_clear_all()
+{
+    m_error_code = SUCCESS;
+    m_error_level = NORMAL;
+    free_description();
+}
+
+//重载=
+Error_manager& Error_manager::operator=(const Error_manager & error_manager)
+{
+    error_manager_reset(error_manager);
+}
+//重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+Error_manager& Error_manager::operator=(Error_code error_code)
+{
+    error_manager_clear_all();
+    set_error_code(error_code);
+}
+//重载==
+bool Error_manager::operator==(const Error_manager & error_manager)
+{
+    is_equal_error_manager(error_manager);
+}
+//重载==，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator==(Error_code error_code)
+{
+    if(m_error_code == error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//重载!=
+bool Error_manager::operator!=(const Error_manager & error_manager)
+{
+    ! is_equal_error_manager(error_manager);
+}
+//重载!=，支持Error_manager和Error_code的直接比较
+bool Error_manager::operator!=(Error_code error_code)
+{
+    if(m_error_code != error_code)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+//获取错误码
+Error_code Error_manager::get_error_code()
+{
+    return m_error_code;
+}
+//获取错误等级
+Error_level Error_manager::get_error_level()
+{
+    return m_error_level;
+}
+//获取错误描述的指针，（浅拷贝）
+char* Error_manager::get_error_description()
+{
+    return pm_error_description;
+}
+
+//复制错误描述，（深拷贝）
+//output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::copy_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description != NULL && pm_error_description != NULL)
+    {
+        char *pt_source = (char *)p_error_description;
+        char* pt_destination = pm_error_description;
+
+        int t_length_min = m_description_length;
+        if(m_description_length > description_length)
+        {
+            t_length_min = description_length;
+        }
+
+        for(int i=0;i<t_length_min; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+//复制错误描述，（深拷贝）
+//output：error_description_string     错误描述的string
+void Error_manager::copy_error_description(std::string & error_description_string)
+{
+    if( (!error_description_string.empty() ) && pm_error_description != NULL)
+    {
+        error_description_string = pm_error_description;
+    }
+    return;
+}
+
+//设置错误码
+void Error_manager::set_error_code(Error_code error_code)
+{
+    m_error_code = error_code;
+    return;
+}
+//比较错误等级并升级，取高等级的结果
+void Error_manager::set_error_level_up(Error_level error_level)
+{
+    if(m_error_level < error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//比较错误等级并降级，取低等级的结果
+void Error_manager::set_error_level_down(Error_level error_level)
+{
+    if(m_error_level > error_level)
+    {
+        m_error_level = error_level;
+    }
+    return;
+}
+//错误等级，设定到固定值
+void Error_manager::set_error_level_location(Error_level error_level)
+{
+    m_error_level = error_level;
+    return;
+}
+//设置错误描述
+void Error_manager::set_error_description(const char* p_error_description, int description_length)
+{
+    reallocate_memory_and_copy_string(p_error_description, description_length);
+    return ;
+}
+//设置错误描述
+void Error_manager::set_error_description(std::string & error_description_string)
+{
+    reallocate_memory_and_copy_string(error_description_string);
+    return ;
+}
+
+//尾部追加错误描述
+void Error_manager::add_error_description(const char* p_error_description, int description_length)
+{
+    if(p_error_description !=NULL)
+    {
+        char* pt_description_front = pm_error_description;
+        int t_description_front_length = m_description_length;
+        char* pt_description_back = (char *)p_error_description;
+        int t_description_back_length = 0;
+
+        if(description_length == 0)
+        {
+            t_description_back_length = 0;
+            while (*pt_description_back != '\0')
+            {
+                t_description_back_length++;
+                pt_description_back++;
+            }
+            t_description_back_length++;
+            pt_description_back = (char *)p_error_description;
+        }
+        else
+        {
+            t_description_back_length = description_length;
+        }
+
+        int t_description_new_length = t_description_front_length + 5 + t_description_back_length - 1;
+        char* pt_description_new =  (char*) malloc(t_description_new_length );
+
+        sprintf(pt_description_new, "%s ### %s", pt_description_front, pt_description_back);
+        free_description();
+        pm_error_description = pt_description_new;
+        m_description_length = t_description_new_length;
+    }
+    return ;
+}
+//尾部追加错误描述
+void Error_manager::add_error_description(std::string & error_description_string)
+{
+    if( ! error_description_string.empty() )
+    {
+        std::string t_description_string = pm_error_description ;
+        t_description_string += (" ### "+ error_description_string);
+
+        reallocate_memory_and_copy_string(t_description_string);
+    }
+}
+
+//比较错误是否相同，
+// 注：只比较错误码和等级
+bool Error_manager::is_equal_error_manager(const Error_manager & error_manager)
+{
+    if(this->m_error_code == error_manager.m_error_code
+       && this->m_error_level == error_manager.m_error_level)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error(const Error_manager & error_manager)
+{
+    if(this->m_error_code == SUCCESS)
+    {
+        error_manager_reset(error_manager);
+    }
+    else if (error_manager.m_error_code == SUCCESS)
+    {
+		return;
+    }
+    else
+    {
+        Error_manager t_error_manager_new;
+        char* pt_string_inside = NULL;
+        int t_string_inside_length = 0;
+        if(this->m_error_level < error_manager.m_error_level)
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+            translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            error_manager_reset(error_manager);
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+        else
+        {
+            t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + error_manager.m_description_length;
+            pt_string_inside = (char*)malloc(t_string_inside_length);
+			((Error_manager & )error_manager).translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+            add_error_description(pt_string_inside,t_string_inside_length);
+        }
+    }
+}
+//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+//如果错误相同，则保留this的，将输入参数转入描述。
+void Error_manager::compare_and_cover_error( Error_manager * p_error_manager)
+{
+	if(this->m_error_code == SUCCESS)
+	{
+		error_manager_reset(*p_error_manager);
+	}
+	else if (p_error_manager->m_error_code == SUCCESS)
+	{
+		//
+	}
+	else
+	{
+		Error_manager t_error_manager_new;
+		char* pt_string_inside = NULL;
+		int t_string_inside_length = 0;
+		if(this->m_error_level < p_error_manager->m_error_level)
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			error_manager_reset(*p_error_manager);
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+		else
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + p_error_manager->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			p_error_manager->translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+	}
+}
+
+//将所有的错误信息，格式化为字符串，用作日志打印。
+//output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+//output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+void Error_manager::translate_error_to_string(char* p_error_aggregate, int aggregate_length )
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    char* pt_index_front = t_string_array;
+    char* pt_index_back = pm_error_description;
+    char* pt_index = p_error_aggregate;
+
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    int t_length_min = m_description_length + ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1;
+    if(t_length_min > aggregate_length)
+    {
+        t_length_min = aggregate_length;
+    }
+
+    for(int i=0;i<t_length_min; i++)
+    {
+        if(i < ERROR_NAMAGER_TO_STRING_FRONT_LENGTH -1)
+        {
+            *pt_index = *pt_index_front;
+            pt_index++;
+            pt_index_front++;
+        }
+        else
+        {
+            *pt_index = *pt_index_back;
+            pt_index++;
+            pt_index_back++;
+        }
+    }
+}
+//output：error_description_string     错误汇总的string
+void Error_manager::translate_error_to_string(std::string & error_aggregate_string)
+{
+    char t_string_array[ERROR_NAMAGER_TO_STRING_FRONT_LENGTH] = {0};
+    sprintf(t_string_array, "error_code:0x%08x, error_level:%02d, error_description:", m_error_code , m_error_level );
+
+    error_aggregate_string = t_string_array ;
+    if(pm_error_description != NULL)
+    {
+        error_aggregate_string += pm_error_description;
+    }
+    else
+    {
+        //error_aggregate_string += "NULL";
+    }
+}
+//错误码转字符串的简易版，可支持cout<<
+//return     错误汇总的string
+std::string Error_manager::to_string()
+{
+    std::string t_string;
+    translate_error_to_string(t_string);
+    return t_string;
+}
+
+
+
+
+//释放错误描述的内存，
+void Error_manager::free_description()
+{
+    if(pm_error_description != NULL)
+    {
+        free (pm_error_description);
+        pm_error_description = NULL;
+    }
+    m_description_length = 0;
+}
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：p_error_description     错误描述的字符串指针，可以为NULL，
+//input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+void Error_manager::reallocate_memory_and_copy_string(const char* p_error_description, int description_length)
+{
+    free_description();
+
+    if(p_error_description != NULL)
+    {
+        char* pt_source = (char *)p_error_description;
+        char* pt_destination = NULL;
+
+        if(description_length == 0)
+        {
+            m_description_length = 0;
+            while (*pt_source != '\0')
+            {
+                m_description_length++;
+                pt_source++;
+            }
+            m_description_length++;
+            pt_source = (char *)p_error_description;
+        }
+        else
+        {
+            m_description_length = description_length;
+        }
+
+        pm_error_description =  (char*) malloc(m_description_length );
+        pt_destination = pm_error_description;
+
+        for(int i=0;i<m_description_length; i++)
+        {
+            *pt_destination = *pt_source;
+            pt_destination++;
+            pt_source++;
+        }
+    }
+
+    return;
+}
+
+
+//重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+//input：error_aggregate_string     错误描述的string
+void Error_manager::reallocate_memory_and_copy_string(std::string & error_aggregate_string)
+{
+    free_description();
+
+    if( ! error_aggregate_string.empty())
+    {
+        m_description_length = error_aggregate_string.length()+1;
+
+        pm_error_description =  (char*) malloc( m_description_length );
+
+        strcpy(pm_error_description ,   error_aggregate_string.c_str()  );
+    }
+}
+
+
+
+
+

src/error_code/error_code.h

@@ -0,0 +1,388 @@
+
+//Error_code是错误码的底层通用模块，
+//功能：用作故障分析和处理。
+
+//用法：所有的功能接口函数return错误管理类，
+//然后上层判断分析错误码，并进行故障处理。
+
+
+
+#ifndef TEST_ERROR_ERROR_CODE_H
+#define TEST_ERROR_ERROR_CODE_H
+
+#include <string>
+#include <string.h>
+
+//错误管理类转化为字符串 的前缀，固定长度为58
+//这个是由显示格式来确定的，如果要修改格式或者 Error_code长度超过8位，Error_level长度超过2位，折需要重新计算
+#define ERROR_NAMAGER_TO_STRING_FRONT_LENGTH   58
+
+//进程加锁的状态，
+enum Lock_status
+{
+    UNLOCK      = 0,
+    LOCK        = 1,
+};
+
+//设备使能状态，
+enum Able_status
+{
+    UNABLE      = 0,
+    ENABLE      = 1,
+};
+
+//数据是否为空
+enum Empty_status
+{
+    NON_EMPTY   = 0,
+    EMPTY       = 1,
+};
+
+
+//错误码的枚举，用来做故障分析
+enum Error_code
+{
+    //成功，没有错误，默认值0
+    SUCCESS                         = 0x00000000,
+
+
+    //基本错误码，
+    ERROR                           = 0x00000001,//错误
+    PARTIAL_SUCCESS                 = 0x00000002,//部分成功
+    WARNING                         = 0x00000003,//警告
+    FAILED                          = 0x00000004,//失败
+
+    NODATA                          = 0x00000010,//没有数据，传入参数容器内部没有数据时，
+
+    POINTER_IS_NULL                 = 0x00000101,//空指针
+    PARAMETER_ERROR                 = 0x00000102,//参数错误，传入参数不符合规范时，
+    POINTER_MALLOC_FAIL             = 0x00000103,//手动分配内存失败
+
+    CLASS_BASE_FUNCTION_CANNOT_USE  = 0x00000201,//基类函数不允许使用，必须使用子类的
+
+
+//    错误码的规范，
+//    错误码是int型，32位，十六进制。
+//    例如0x12345678
+//    12表示功能模块，例如:laser雷达模块               框架制定
+//    34表示文件名称，例如:laser_livox.cpp             框架制定
+//    56表示具体的类，例如:class laser_livox           个人制定
+//    78表示类的函数，例如:laser_livox::start();       个人制定
+//    注：错误码的制定从1开始，不要从0开始，
+//        0用作错误码的基数，用来位运算，来判断错误码的范围。
+
+//    laser扫描模块
+    LASER_ERROR_BASE                = 0x01000000,
+
+//    laser_base基类
+	LASER_BASE_ERROR_BASE			= 0x01010000,
+    LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达任务输入参数错误
+    LASER_CONNECT_FAILED,
+	LASER_START_FAILED,
+	LASER_CHECK_FAILED				,
+	LASER_STATUS_ERROR,								//雷达状态错误
+
+
+    LASER_LIVOX_SKD_INIT_FAILED,
+
+
+//    laser_livox.cpp的错误码
+    LIVOX_ERROR_BASE                = 0x01020000,
+    LIVOX_START_FAILE               = 0x01020101,
+	LIVOX_TASK_TYPE_ERROR,							//livox任务类型错误
+
+
+     //PLC error code  ...
+    PLC_ERROR_BASE                  = 0x02010000,
+    PLC_UNKNOWN_ERROR,
+    PLC_EMPTY_TASK,
+    PLC_IP_PORT_ERROR,
+    PLC_SLAVE_ID_ERROR,
+    PLC_CONNECTION_FAILED,
+    PLC_READ_FAILED,
+    PLC_WRITE_FAILED,
+    PLC_NOT_ENOUGH_DATA_ERROR,
+
+    //Locater.cpp error from 0x0301000-0x030100FF
+        LOCATER_TASK_INIT_CLOUD_EMPTY=0x03010000,
+    LOCATER_TASK_ERROR,
+    LOCATER_TASK_INPUT_CLOUD_UNINIT,
+    LOCATER_INPUT_CLOUD_EMPTY,
+    LOCATER_YOLO_UNINIT,
+    LOCATER_POINTSIFT_UNINIT,
+    LOCATER_3DCNN_UNINIT,
+    LOCATER_INPUT_YOLO_CLOUD_EMPTY,
+    LOCATER_Y_OUT_RANGE_BY_PLC,
+    LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,
+    LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,
+    LOCATER_INPUT_CLOUD_UNINIT,
+
+
+    //point sift from 0x03010100-0x030101FF
+        LOCATER_SIFT_INIT_FAILED=0x03010100,
+    LOCATER_SIFT_INPUT_CLOUD_UNINIT,
+    LOCATER_SIFT_PREDICT_FAILED,
+    LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
+    LOCATER_SIFT_FILTE_OBS_FAILED,
+    LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,
+    LOCATER_SIFT_INPUT_CLOUD_EMPTY,
+    LOCATER_SIFT_PREDICT_NO_CAR_POINT,
+
+    //yolo from 0x03010200-0x030102FF
+        LOCATER_YOLO_DETECT_FAILED=0x03010200,
+    LOCATER_YOLO_DETECT_NO_TARGET,
+    LOCATER_YOLO_PARAMETER_INVALID,
+    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
+
+    //3dcnn from 0x03010300-0x030103FF
+    LOCATER_3DCNN_INIT_FAILED=0x03010300,
+    LOCATER_3DCNN_INPUT_CLOUD_UNINIT,
+    LOCATER_3DCNN_PREDICT_FAILED,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_3,
+    LOCATER_3DCNN_VERIFY_RECT_FAILED_4,
+    LOCATER_3DCNN_KMEANS_FAILED,
+    LOCATER_3DCNN_IIU_FAILED,
+    LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
+    LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,
+
+    //System_manager error from 0x04010000-0x0401FFFF
+    SYSTEM_READ_PARAMETER_ERROR=0x04010100,
+    SYSTEM_PARAMETER_ERROR,
+    SYSTEM_INPUT_TERMINOR_NO_LASERS,
+
+    //terminor_command_executor.cpp from 0x04010200-0x040102FF
+    TERMINOR_NOT_READY=0x04010200,
+    TERMINOR_INPUT_LASER_NULL,
+    TERMINOR_NOT_CONTAINS_LASER,
+    TERMINOR_INPUT_PLC_NULL,
+    TERMINOR_INPUT_LOCATER_NULL,
+    TERMINOR_CREATE_WORKING_THREAD_FAILED,
+    TERMINOR_FORCE_QUIT,
+    TERMINOR_LASER_TIMEOUT,
+    TERMINOR_POST_PLC_TIMEOUT,
+    TERMINOR_CHECK_RESULTS_ERROR,
+
+    ////Hardware limit from 0x05010000 - 0x0501ffff
+    ///railing.cpp from 0x05010100-0x050101ff
+    HARDWARE_LIMIT_LEFT_RAILING=0x05010100,         //左栏杆限制
+    HARDWARE_LIMIT_RAILING_PARAMETER_ERROR,
+    HARDWARE_LIMIT_RAILING_ERROR,
+    HARDWARE_LIMIT_CENTER_X_LEFT,
+    HARDWARE_LIMIT_CENTER_X_RIGHT,
+    HARDWARE_LIMIT_CENTER_Y_TOP,
+    HARDWARE_LIMIT_CENTER_Y_BOTTOM,
+    HARDWARE_LIMIT_HEIGHT_OUT_RANGE,
+    HARDWARE_LIMIT_ANGLE_OUT_RANGE,
+    //termonal_limit from 0x05010200-0x050102ff
+    HARDWARE_LIMIT_TERMINAL_LEFT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_RIGHT_ERROR,
+    HARDWARE_LIMIT_TERMINAL_LR_ERROR,
+
+
+    //wj_lidar error from 0x06010000-0x0601FFFF
+        WJ_LIDAR_CONNECT_FAILED=0x06010000,
+    WJ_LIDAR_UNINITIALIZED,
+    WJ_LIDAR_READ_FAILED,
+    WJ_LIDAR_WRITE_FAILED,
+    WJ_LIDAR_GET_CLOUD_TIMEOUT,
+
+    //wj lidar protocol error from 0x06020000-0x0602FFFF
+        WJ_PROTOCOL_ERROR_BASE=0x06020000,
+    WJ_PROTOCOL_INTEGRITY_ERROR,
+    WJ_PROTOCOL_PARSE_FAILED,
+    WJ_PROTOCOL_EMPTY_PACKAGE,
+    WJ_PROTOCOL_EXCEED_MAX_SIZE,
+
+    //wj region detect error from 0x06030000-0x0603FFFF
+        WJ_REGION_EMPTY_CLOUD=0x06030000,
+    WJ_REGION_RECTANGLE_ANGLE_ERROR,
+    WJ_REGION_RECTANGLE_SIZE_ERROR,
+    WJ_REGION_RECTANGLE_SYMMETRY_ERROR,
+    WJ_REGION_CLUSTER_SIZE_ERROR,
+    WJ_REGION_CERES_SOLVE_ERROR,
+
+    //wj manager error from 0x06040000-0x0604FFFF
+    WJ_MANAGER_UNINITIALIZED=0x06040000,
+    WJ_MANAGER_LIDAR_DISCONNECTED,
+    WJ_MANAGER_PLC_DISCONNECTED,
+    WJ_MANAGER_EMPTY_CLOUD,
+
+    WJ_LIDAR_TASK_EMPTY_RESULT=0x06050000,
+    WJ_LIDAR_TASK_EMPTY_TASK,
+    WJ_LIDAR_TASK_WRONG_TYPE,
+    WJ_LIDAR_TASK_INVALID_TASK,
+    WJ_LIDAR_TASK_MEASURE_FAILED,
+
+    WJ_FILTER_LACK_OF_RESULT,
+    WJ_FILTER_FLUCTUATING,
+
+};
+
+//错误等级，用来做故障处理
+enum Error_level
+{
+//    正常，没有错误，默认值0
+    NORMAL                = 0,
+
+
+//    可忽略的故障，NEGLIGIBLE_ERROR
+//    提示作用，不做任何处理，不影响代码的流程，
+//    用作一些不重要的事件，一半出错也不会影响到系统功能，
+//    例如：文件保存错误，等
+    NEGLIGIBLE_ERROR      = 1,
+
+
+//    一般故障，MINOR_ERROR
+//    用作底层功能函数的错误返回，表示该功能函数执行失败，
+//    返回给应用层之后，需要做故障分析和处理，
+//    例如：雷达数据传输失败，应用层进行重新扫描，或者重连，或者重置参数等。
+    MINOR_ERROR           = 2,
+
+
+//    严重故障，MAJOR_ERROR
+//    用作应用层的任务事件的结果，表示该功能模块失败。
+//    通常是底层函数返回一般故障之后，应用层无法处理并解决故障，此时就要进行故障升级，
+//    从一般故障升级为严重故障，然后进行回退流程，回退已经执行的操作，最终回到故障待机状态。
+//    需要外部清除故障，并复位至正常待机状态，才能恢复功能的使用。
+//    例如：雷达扫描任务失败，且无法自动恢复。
+    MAJOR_ERROR           = 3,
+
+
+//    致命故障，CRITICAL_ERROR
+//    系统出现致命错误。导致系统无法正常运行，
+//    此时系统应该紧急停机，执行紧急流程，快速停机。
+//    此时不允许再执行任何函数和任务指令，防止系统故障更加严重。
+//    也不需要做任何错误处理了，快速执行紧急流程。
+//    例如：内存错误，进程挂死，关键设备失控，监控设备报警，等
+    CRITICAL_ERROR        = 4,
+};
+
+
+class Error_manager
+{
+public://外部接口函数
+    //构造函数
+    Error_manager();
+    //拷贝构造
+    Error_manager(const Error_manager & error_manager);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level = NORMAL,
+                  const char* p_error_description = NULL, int description_length = 0);
+    //赋值构造
+    Error_manager(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //析构函数
+    ~Error_manager();
+
+    //初始化
+    void error_manager_init();
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level = NORMAL,
+                            const char* p_error_description = NULL, int description_length = 0);
+    //初始化
+    void error_manager_init(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level = NORMAL,
+                             const char* p_error_description = NULL, int description_length = 0);
+    //重置
+    void error_manager_reset(Error_code error_code, Error_level error_level , std::string & error_aggregate_string);
+    //重置
+    void error_manager_reset(const Error_manager & error_manager);
+    //清除所有内容
+    void error_manager_clear_all();
+
+    //重载=
+    Error_manager& operator=(const Error_manager & error_manager);
+    //重载=，支持Error_manager和Error_code的直接转化，会清空错误等级和描述
+    Error_manager& operator=(Error_code error_code);
+    //重载==
+    bool operator==(const Error_manager & error_manager);
+    //重载==，支持Error_manager和Error_code的直接比较
+    bool operator==(Error_code error_code);
+    //重载!=
+    bool operator!=(const Error_manager & error_manager);
+    //重载!=，支持Error_manager和Error_code的直接比较
+    bool operator!=(Error_code error_code);
+
+
+    //获取错误码
+    Error_code get_error_code();
+    //获取错误等级
+    Error_level get_error_level();
+    //获取错误描述的指针，（浅拷贝）
+    char* get_error_description();
+
+    //复制错误描述，（深拷贝）
+    //output：p_error_description     错误描述的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误描述的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void copy_error_description(const char* p_error_description, int description_length);
+    //复制错误描述，（深拷贝）
+    //output：error_description_string     错误描述的string
+    void copy_error_description(std::string & error_description_string);
+
+    //设置错误码
+    void set_error_code(Error_code error_code);
+    //比较错误等级并升级，取高等级的结果
+    void set_error_level_up(Error_level error_level);
+    //比较错误等级并降级，取低等级的结果
+    void set_error_level_down(Error_level error_level);
+    //错误等级，设定到固定值
+    void set_error_level_location(Error_level error_level);
+    //设置错误描述
+    void set_error_description(const char* p_error_description, int description_length = 0);
+    //设置错误描述
+    void set_error_description(std::string & error_description_string);
+
+    //尾部追加错误描述
+    void add_error_description(const char* p_error_description, int description_length = 0);
+    //尾部追加错误描述
+    void add_error_description(std::string & error_description_string);
+
+    //比较错误是否相同，
+    // 注：只比较错误码和等级
+	bool is_equal_error_manager(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error(const Error_manager & error_manager);
+	//比较并覆盖错误，讲低级错误转为字符串存放于描述中，
+	//如果错误相同，则保留this的，将输入参数转入描述。
+	void compare_and_cover_error( Error_manager * p_error_manager);
+
+	//将所有的错误信息，格式化为字符串，用作日志打印。
+    //output：p_error_description     错误汇总的字符串指针，不可以为NULL，必须要有实际的内存
+    //output:description_length      错误汇总的字符串长度，不可以为0，长度最好足够大，一般256即可。
+    void translate_error_to_string(char* p_error_aggregate, int aggregate_length);
+    //output：error_description_string     错误汇总的string
+    void translate_error_to_string(std::string & error_aggregate_string);
+    //错误码转字符串的简易版，可支持cout<<
+    //return     错误汇总的string
+    std::string to_string();
+
+
+
+protected:
+    Error_code              m_error_code;               //错误码
+    Error_level             m_error_level;              //错误等级
+    char*                   pm_error_description;       //错误描述
+    int                     m_description_length;       //错误描述的字符长度
+
+protected://内部功能函数
+public:
+    //释放错误描述的内存，
+    void free_description();
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：p_error_description     错误描述的字符串指针，可以为NULL，
+    //input:description_length      错误描述的字符串长度，如果为0，则从p_error_description里面获取有效的长度
+    void reallocate_memory_and_copy_string(const char* p_error_description, int description_length = 0);
+
+    //重新分配错误描述的内存，并从外部拷贝新的（深拷贝）
+    //input：error_aggregate_string     错误描述的string
+    void reallocate_memory_and_copy_string(std::string & error_aggregate_string);
+};
+
+
+
+
+#endif //TEST_ERROR_ERROR_CODE_H
+
+

src/ground_region.cpp

@@ -0,0 +1,470 @@
+//
+// 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>
+
+//欧式聚类*******************************************************
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> 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.2);
+    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 distance(cv::Point2f p1, cv::Point2f p2)
+{
+    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
+}
+
+
+bool 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_ceres::Detect_result& result)
+{
+    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();
+
+
+}
+Ground_region::Ground_region()
+{
+    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");
+    }
+    return init(m_configure);
+
+}
+Error_manager Ground_region::init(ground_region::Configure configure)
+{
+    //创建雷达
+    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(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double timeout_second)
+{
+    Tick timer;
+    bool sync=false;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr sync_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    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)
+        {
+            *sync_cloud+=(*scans2cloud(sync_frames[i],m_configure.lidar(i).calib()));
+        }
+        return SUCCESS;
+    }
+    return FAILED;
+
+}
+
+void Ground_region::thread_measure_func(Ground_region* p)
+{
+    //保持 10 fps
+    while(false==p->m_exit_condition.wait_for_millisecond(100))
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+        Error_manager code=p->sync_capture(cloud,0.5);
+
+        if(code!=SUCCESS)
+        {
+            std::cout<<code.get_error_description()<<std::endl;
+            continue;
+        }
+
+        detect_wheel_ceres::Detect_result result;
+
+        code=p->detect(cloud,p->m_configure.box(),result);
+        if(code!=SUCCESS)
+        {
+            LOG_EVERY_N(INFO,20)<<code.get_error_description();
+            continue;
+        }
+
+        //  tobe 发布结果
+
+
+
+    }
+}
+
+
+
+Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+        const ground_region::Box& box,detect_wheel_ceres::Detect_result& last_result)
+{
+    //直通滤波
+    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);
+        }
+    }
+
+    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_ceres::Detect_result> results;
+    while(fabs(center_z-last_center_z)>0.01)
+    {
+        center_z=(start_z+max_z)/2.0;
+        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
+        detect_wheel_ceres::Detect_result result;
+        bool ret = classify_ceres_detect(cloud, center_z,result);
+        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++;
+    }
+
+    //
+    if(results.size()==0)
+    {
+        return Error_manager(FAILED,NORMAL,"nor car");
+    }
+    ///  to be
+    last_result=results[0];
+    return SUCCESS;
+}

src/ground_region.h

@@ -0,0 +1,59 @@
+//
+// Created by zx on 2021/5/20.
+//
+
+#ifndef LIDARMEASURE__GROUD_REGION_HPP_
+#define LIDARMEASURE__GROUD_REGION_HPP_
+
+#include "error_code.h"
+#include "detect_wheel_ceres.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 "thread_condition.h"
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+class Ground_region
+{
+ public:
+    Ground_region();
+    ~Ground_region();
+    Error_manager init(std::string configure_file);
+    Error_manager init(ground_region::Configure configure);
+
+ private:
+    bool read_proto_param(std::string file, ::google::protobuf::Message& parameter);
+    Error_manager sync_capture(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);
+
+    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double thresh_z,
+                               detect_wheel_ceres::Detect_result& result);
+    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,const ground_region::Box& box,
+            detect_wheel_ceres::Detect_result& result);
+    static void thread_measure_func(Ground_region* p);
+
+
+ private:
+    std::vector<velodyne::VLP16Capture*>     m_lidars;
+    ground_region::Configure                m_configure;
+    std::thread*                            m_running_thread;
+    Thread_condition                        m_exit_condition;
+
+    detect_wheel_ceres                      m_detector;
+
+
+};
+
+
+#endif //LIDARMEASURE__GROUD_REGION_HPP_

src/tool/StdCondition.cpp

@@ -0,0 +1,35 @@
+#include "StdCondition.h"
+
+StdCondition::StdCondition():m_value(false)
+{
+}
+
+StdCondition::StdCondition(bool init):m_value(init)
+{
+}
+
+StdCondition::~StdCondition()
+{
+}
+bool StdCondition::isTrue(StdCondition* scd)
+{
+	if (scd == 0)return false;
+	return scd->m_value;
+}
+
+void StdCondition::Wait()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_cv.wait(loc,std::bind(isTrue,this));
+}
+bool StdCondition::WaitFor(unsigned int millisecond)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	return m_cv.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(isTrue, this));
+}
+void StdCondition::Notify(bool istrue)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_value = istrue;
+	m_cv.notify_all();
+}

src/tool/StdCondition.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include <thread>  
+#include <mutex>  
+#include <chrono>  
+#include <condition_variable>
+#include <functional>
+
+class StdCondition
+{
+public:
+	StdCondition();
+	StdCondition(bool init);
+	~StdCondition();
+	void Wait();
+	bool WaitFor(unsigned int millisecond);
+	void Notify(bool istrue);
+
+protected:
+	static bool isTrue(StdCondition* scd);
+protected:
+	bool m_value;
+	std::mutex m_mutex;
+	std::condition_variable m_cv;
+};
+

src/tool/binary_buf.cpp

@@ -0,0 +1,341 @@
+
+/*
+ * binary_buf是二进制缓存
+ * 这里用字符串，来存储雷达的通信消息的原始数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
+ *
+ * 例如思科的雷达的消息类型
+ * ready->ready->start->data->data->data->stop->ready->ready
+ *
+ * 提供了 is_equal 系列的函数，来进行判断前面的消息类型
+ * 
+ * 注意了：m_buf是中间可以允许有‘\0’的，不是单纯的字符串格式
+ * 			末尾也不一定是‘\0’
+ */
+
+#include "binary_buf.h"
+
+#include <string>
+#include <string.h>
+
+Binary_buf::Binary_buf()
+{
+	mp_buf = NULL;
+	m_length = 0;
+}
+
+Binary_buf::Binary_buf(const Binary_buf& other)
+{
+	mp_buf = NULL;
+	m_length = 0;
+
+	if ( other.m_length > 0 && other.mp_buf != NULL)
+	{
+		mp_buf = (char*)malloc(other.m_length);
+		memcpy(mp_buf, other.mp_buf, other.m_length);
+		m_length = other.m_length;
+	}
+}
+
+Binary_buf::~Binary_buf()
+{
+	if ( mp_buf )
+	{
+		free(mp_buf);
+		mp_buf = NULL;
+	}
+	m_length = 0;
+
+//	std::cout << "Binary_buf::~Binary_buf()" << std::endl;
+}
+
+
+//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+Binary_buf::Binary_buf(const char* p_buf, int len)
+{
+	mp_buf = NULL;
+	m_length = 0;
+
+	if ( p_buf != NULL)
+	{
+		if (len <= 0)
+		{
+			len = strlen(p_buf);
+		}
+
+		mp_buf = (char*)malloc(len);
+		memcpy(mp_buf, p_buf, len);
+		m_length = len;
+	}
+}
+
+
+//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+Binary_buf::Binary_buf(char* p_buf, int len)
+{
+	mp_buf = NULL;
+	m_length = 0;
+
+	if ( p_buf != NULL)
+	{
+		if (len <= 0)
+		{
+			len = strlen(p_buf);
+		}
+
+		mp_buf = (char*)malloc(len);
+		memcpy(mp_buf, p_buf, len);
+		m_length = len;
+	}
+}
+
+//重载=，深拷贝，
+Binary_buf& Binary_buf::operator=(const Binary_buf& other)
+{
+	clear();
+
+	if ( other.m_length > 0 && other.mp_buf != NULL)
+	{
+		mp_buf = (char*)malloc(other.m_length);
+		memcpy(mp_buf, other.mp_buf, other.m_length);
+		m_length = other.m_length;
+	}
+	return *this;
+}
+
+//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
+Binary_buf& Binary_buf::operator=(const char* p_buf)
+{
+	clear();
+
+	if ( p_buf != NULL)
+	{
+		int len = strlen(p_buf);
+		mp_buf = (char*)malloc(len);
+		memcpy(mp_buf, p_buf, len);
+		m_length = len;
+	}
+	return *this;
+}
+
+//重载+，other追加在this的后面，
+Binary_buf& Binary_buf::operator+(Binary_buf& other)
+{
+	if (other.mp_buf != NULL && other.m_length > 0)
+	{
+		int t_length_total = m_length + other.m_length;
+		char* tp_buf_total = (char*)malloc(t_length_total);
+		memcpy(tp_buf_total, mp_buf, m_length);
+		memcpy(tp_buf_total + m_length, other.mp_buf, other.m_length);
+		free(mp_buf);
+		mp_buf = tp_buf_total;
+		m_length = t_length_total;
+	}
+	return *this;
+}
+
+//重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
+Binary_buf& Binary_buf::operator+(const char* p_buf)
+{
+	if (p_buf != NULL )
+	{
+		int t_length_back = strlen(p_buf);
+		int t_length_total = m_length + t_length_back;
+		char* tp_buf_total = (char*)malloc(t_length_total);
+		memcpy(tp_buf_total, mp_buf, m_length);
+		memcpy(tp_buf_total + m_length, p_buf, t_length_back);
+		free(mp_buf);
+		mp_buf = tp_buf_total;
+		m_length = t_length_total;
+	}
+	return *this;
+}
+
+//重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
+char& Binary_buf::operator[](int n)
+{
+	if (n >= 0 && n < m_length)
+	{
+		return mp_buf[n];
+	}
+	else
+	{
+		throw (n);
+	}
+}
+
+
+//判空
+bool Binary_buf::is_empty()
+{
+	if ( mp_buf != NULL && m_length > 0 )
+	{
+		return false;
+	}
+	else
+	{
+		return true;
+	}
+}
+
+//清空
+void Binary_buf::clear()
+{
+	if ( mp_buf )
+	{
+		free(mp_buf);
+		mp_buf = NULL;
+	}
+	m_length = 0;
+}
+
+
+//比较前面部分的buf是否相等，使用 other.m_length 为标准
+bool Binary_buf::is_equal_front(const Binary_buf& other)
+{
+	if ( other.mp_buf == NULL || other.m_length <= 0 )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			if ( other.m_length > m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, other.mp_buf, other.m_length) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+
+//比较前面部分的buf是否相等，len为0时，使用strlen(buf)为标准，不比较结束符'\0'
+bool Binary_buf::is_equal_front(const char* p_buf, int len)
+{
+	if ( p_buf == NULL )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			if ( len == 0 )
+			{
+				len = strlen(p_buf);
+			}
+			if ( len > m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, p_buf, len) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+
+//比较的buf是否全部相等，
+bool Binary_buf::is_equal_all(const Binary_buf& other)
+{
+	if ( other.mp_buf == NULL || other.m_length <= 0 )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			if ( other.m_length != m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, other.mp_buf, other.m_length) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+//比较的buf是否全部相等，不比较结束符'\0'
+bool Binary_buf::is_equal_all(const char* p_buf)
+{
+	if ( p_buf == NULL )
+	{
+		if ( mp_buf == NULL || m_length <= 0 )
+		{
+			return true;
+		}
+		else
+		{
+			return false;
+		}
+	}
+	else
+	{
+		if ( mp_buf != NULL && m_length > 0 )
+		{
+			int	len = strlen(p_buf);
+			if ( len != m_length )
+			{
+				return false;
+			}
+			return  (strncmp((const char*)mp_buf, p_buf, len) == 0);
+		}
+		else
+		{
+			return false;
+		}
+
+	}
+}
+
+
+
+
+char*	Binary_buf::get_buf()const
+{
+	return mp_buf;
+}
+
+int		Binary_buf::get_length()const
+{
+	return m_length;
+}
+
+
+
+
+

src/tool/binary_buf.h

@@ -0,0 +1,91 @@
+
+/*
+ * binary_buf是二进制缓存
+ * 这里用字符串，来存储雷达的通信消息的原始数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
+ *
+ * 例如思科的雷达的消息类型
+ * ready->ready->start->data->data->data->stop->ready->ready
+ *
+ * 提供了 is_equal 系列的函数，来进行判断前面的消息类型
+ *
+ * 注意了：m_buf是中间可以允许有‘\0’的，不是单纯的字符串格式
+ * 			末尾也不一定是‘\0’
+ */
+
+#ifndef LIDARMEASURE_BINARY_BUF_H
+#define LIDARMEASURE_BINARY_BUF_H
+#include <iostream>
+
+
+//雷达消息的类型
+//在通信消息的前面一部分字符串，表示这条消息的类型。
+//在解析消息的时候，先解析前面的消息类型，来判断这条消息的功用
+enum Buf_type
+{
+	//默认值 BUF_UNKNOW = 0
+	BUF_UNKNOW   		=0,	//未知消息
+	BUF_READY  			=1,	//待机消息
+	BUF_START 			=2,	//开始消息
+	BUF_DATA   			=3,	//数据消息
+	BUF_STOP  			=4,	//结束消息
+	BUF_ERROR   		=5,	//错误消息
+};
+
+
+//二进制缓存，
+class Binary_buf
+{
+public:
+	Binary_buf();
+	Binary_buf(const Binary_buf& other);
+	~Binary_buf();
+
+	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf(const char* p_buf, int len = 0);
+	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf(char* p_buf, int len = 0);
+	//重载=，深拷贝，
+	Binary_buf& operator=(const Binary_buf& other);
+	//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator=(const char* p_buf);
+	//重载+，other追加在this的后面，
+	Binary_buf& operator+(Binary_buf& other);
+	//重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator+(const char* p_buf);
+	//重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
+	char& operator[](int n);
+
+	//判空
+	bool is_empty();
+	//清空
+	void clear();
+
+	//比较前面部分的buf是否相等，使用 other.m_length 为标准
+	bool is_equal_front(const Binary_buf& other);
+	//比较前面部分的buf是否相等，len为0时，使用strlen(buf)为标准，不比较结束符'\0'
+	bool is_equal_front(const char* p_buf, int len = 0);
+
+	//比较的buf是否全部相等，
+	bool is_equal_all(const Binary_buf& other);
+	//比较的buf是否全部相等，不比较结束符'\0'
+	bool is_equal_all(const char* p_buf);
+
+
+
+public:
+	char* get_buf()const;
+	int	get_length()const;
+
+protected:
+	char*		mp_buf;				//二进制缓存指针
+	int			m_length;			//二进制缓存长度
+
+private:
+
+};
+
+
+
+
+#endif //LIDARMEASURE_BINARY_BUF_H

src/tool/binary_buf.puml

@@ -0,0 +1,85 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  binary_buf是二进制缓存
+
+note left of Binary_buf
+/*
+ * binary_buf是二进制缓存
+ * 这里用字符串，来存储雷达的通信消息的原始数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
+ *
+ * 例如思科的雷达的消息类型
+ * ready->ready->start->data->data->data->stop->ready->ready
+ *
+ * 提供了 is_equal 系列的函数，来进行判断前面的消息类型
+ *
+ * 注意了：m_buf是中间可以允许有‘\0’的，不是单纯的字符串格式
+ * 			末尾也不一定是‘\0’
+ */
+end note
+
+
+
+enum Buf_type
+{
+//雷达消息的类型
+//在通信消息的前面一部分字符串，表示这条消息的类型。
+//在解析消息的时候，先解析前面的消息类型，来判断这条消息的功用
+	//默认值 BUF_UNKNOW = 0
+	BUF_UNKNOW   		=0,	//未知消息
+	BUF_READY  			=1,	//待机消息
+	BUF_START 			=2,	//开始消息
+	BUF_DATA   			=3,	//数据消息
+	BUF_STOP  			=4,	//结束消息
+	BUF_ERROR   		=5,	//错误消息
+}
+
+
+
+class Binary_buf
+{
+//二进制缓存，
+==public:==
+	Binary_buf();
+	Binary_buf(const Binary_buf& other);
+	~Binary_buf();
+..
+	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf(const char* p_buf, int len = 0);
+	//重载=，深拷贝，
+	Binary_buf& operator=(const Binary_buf& other);
+	//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator=(const char* p_buf);
+	//重载+，other追加在this的后面，
+	Binary_buf& operator+(Binary_buf& other);
+	//重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf& operator+(const char* p_buf);
+	//重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
+	char& operator[](int n);
+..
+	//判空
+	bool is_empty();
+	//清空
+	void clear();
+..
+	//比较前面部分的buf是否相等，使用 other.m_length 为标准
+	bool is_equal_front(const Binary_buf& other);
+	//比较前面部分的buf是否相等，len为0时，使用strlen(buf)为标准，不比较结束符'\0'
+	bool is_equal_front(const char* p_buf, int len = 0);
+
+	//比较的buf是否全部相等，
+	bool is_equal_all(const Binary_buf& other);
+	//比较的buf是否全部相等，不比较结束符'\0'
+	bool is_equal_all(const char* p_buf);
+==public:==
+	char* get_buf()const;
+	int	get_length()const;
+==protected:==
+	char*		mp_buf;				//二进制缓存指针
+	int			m_length;			//二进制缓存长度
+==private:==
+}
+
+
+@enduml

src/tool/pathcreator.cpp

@@ -0,0 +1,94 @@
+#include "pathcreator.h"
+
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <stdint.h>
+#include <stdio.h>
+
+PathCreator::PathCreator()
+{
+
+}
+
+PathCreator::~PathCreator()
+{
+
+}
+
+std::string PathCreator::GetCurPath()
+{
+    return m_current_path;
+}
+bool PathCreator::Mkdir(std::string dirName)
+{
+    uint32_t beginCmpPath = 0;
+    uint32_t endCmpPath = 0;
+    std::string fullPath = "";
+
+    if('/' != dirName[0])
+    {
+        fullPath = getcwd(nullptr, 0);
+        beginCmpPath = fullPath.size();
+        fullPath = fullPath + "/" + dirName;
+    }
+    else
+    {
+        //Absolute path
+        fullPath = dirName;
+        beginCmpPath = 1;
+    }
+    if (fullPath[fullPath.size() - 1] != '/')
+    {
+        fullPath += "/";
+    }
+    endCmpPath = fullPath.size();
+
+    //create dirs;
+    for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
+    {
+        if('/' == fullPath[i])
+        {
+            std::string curPath = fullPath.substr(0, i);
+            if(access(curPath.c_str(), F_OK) != 0)
+            {
+                if(mkdir(curPath.c_str(), /*S_IRUSR|S_IRGRP|S_IROTH|S_IWUSR|S_IWGRP|S_IWOTH*/0777) == -1)
+                {
+                    printf("mkdir(%s) failed\n", curPath.c_str());
+                    return false;
+                }
+            }
+        }
+    }
+    m_current_path=fullPath;
+    return true;
+
+}
+
+bool PathCreator::CreateDatePath(std::string root, bool add_time)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    if (add_time)
+    {
+        sprintf(buf, "%s/%d%02d%02d-%02d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday,
+                t->tm_hour,
+                t->tm_min,
+                t->tm_sec);
+    }
+    else
+    {
+        sprintf(buf, "%s/%d%02d%02d", root.c_str(),
+                t->tm_year + 1900,
+                t->tm_mon + 1,
+                t->tm_mday);
+    }
+    return Mkdir(buf);
+}

src/tool/pathcreator.h

@@ -0,0 +1,18 @@
+#ifndef PATHCREATOR_H
+#define PATHCREATOR_H
+#include <string>
+
+class PathCreator
+{
+public:
+    PathCreator();
+    ~PathCreator();
+    std::string GetCurPath();
+    bool Mkdir(std::string dir);
+    bool CreateDatePath(std::string root, bool add_time = true);
+protected:
+    std::string m_current_path;
+};
+
+
+#endif // PATHCREATOR_H

src/tool/pcl_cloud_with_lock.h

@@ -0,0 +1,30 @@
+
+
+
+#ifndef PCL_CLOUD_WITH_LOCK_H
+#define PCL_CLOUD_WITH_LOCK_H
+
+
+class Pcl_cloud_with_lock
+{
+public:
+	Pcl_cloud_with_lock();
+	Pcl_cloud_with_lock(const Pcl_cloud_with_lock& other);
+	~Pcl_cloud_with_lock();
+public://API functions
+
+public://get or set member variable
+
+
+protected://member variable
+//	//三维点云的数据保护锁，
+//	std::mutex*                     mp_task_cloud_lock;
+//	//三维点云容器的智能指针，这里不直接分配内存,
+//	pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+
+private:
+
+};
+
+
+#endif //PCL_CLOUD_WITH_LOCK_H

src/tool/proto_tool.cpp

@@ -0,0 +1,42 @@
+
+
+
+#include "proto_tool.h"
+#include <fcntl.h>
+#include<unistd.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+
+//读取protobuf 配置文件，转化为protobuf参数形式
+//input：	prototxt_path ：prototxt文件路径
+//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+bool proto_tool::read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter)
+{
+	int fd = open(prototxt_path.c_str(), O_RDONLY);
+	if (fd == -1) return false;
+	FileInputStream* input = new FileInputStream(fd);
+	bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
+	delete input;
+	close(fd);
+	return success;
+}
+
+
+
+
+
+
+
+
+
+
+
+

src/tool/proto_tool.h

@@ -0,0 +1,56 @@
+
+
+
+
+
+#ifndef __PROTO_TOOL_H
+#define __PROTO_TOOL_H
+
+#include "../tool/singleton.h"
+#include <istream>
+#include <google/protobuf/message.h>
+
+class proto_tool:public Singleton<proto_tool>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<proto_tool>;
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	proto_tool(const proto_tool&)=delete;
+	proto_tool& operator =(const proto_tool&)= delete;
+	~proto_tool()=default;
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	proto_tool()=default;
+
+
+public:
+	//读取protobuf 配置文件，转化为protobuf参数形式
+	//input：	prototxt_path ：prototxt文件路径
+	//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+	static bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter);
+};
+
+
+
+
+#endif //__PROTO_TOOL_H
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

src/tool/singleton.cpp

@@ -0,0 +1,4 @@
+
+#include "singleton.h"
+
+

src/tool/singleton.h

@@ -0,0 +1,81 @@
+
+/* Singleton 是单例类的模板。
+ * https://www.cnblogs.com/sunchaothu/p/10389842.html
+ * 单例 Singleton 是设计模式的一种，其特点是只提供唯一一个类的实例,具有全局变量的特点，在任何位置都可以通过接口获取到那个唯一实例;
+ * 全局只有一个实例：static 特性，同时禁止用户自己声明并定义实例（把构造函数设为 private 或者 protected）
+ * Singleton 模板类对这种方法进行了一层封装。
+ * 单例类需要从Singleton继承。
+ * 子类需要将自己作为模板参数T 传递给 Singleton<T> 模板;
+ * 同时需要将基类声明为友元，这样Singleton才能调用子类的私有构造函数。
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来操作 唯一的实例。
+ * */
+
+#ifndef __SINGLIETON_H
+#define __SINGLIETON_H
+
+//#include <iostream>
+
+template<typename T>
+class Singleton
+{
+public:
+	//获取单例的引用
+	static T& get_instance_references()
+	{
+		static T instance;
+		return instance;
+	}
+	//获取单例的指针
+	static T* get_instance_pointer()
+	{
+		return &(get_instance_references());
+	}
+
+	virtual ~Singleton()
+	{
+//		std::cout<<"destructor called!"<<std::endl;
+	}
+
+	Singleton(const Singleton&)=delete;					//关闭拷贝构造函数
+	Singleton& operator =(const Singleton&)=delete;		//关闭赋值函数
+
+protected:
+	//构造函数需要是 protected，这样子类才能继承；
+	Singleton()
+	{
+//		std::cout<<"constructor called!"<<std::endl;
+	}
+
+};
+
+/*
+// 如下是 使用样例：
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+
+class DerivedSingle:public Singleton<DerivedSingle>
+{
+ // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<DerivedSingle>;
+public:
+ // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	DerivedSingle(const DerivedSingle&)=delete;
+	DerivedSingle& operator =(const DerivedSingle&)= delete;
+ 	~DerivedSingle()=default;
+private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
+	DerivedSingle()=default;
+};
+
+int main(int argc, char* argv[]){
+	DerivedSingle& instance1 = DerivedSingle::get_instance_references();
+	DerivedSingle* p_instance2 = DerivedSingle::get_instance_pointer();
+	return 0;
+}
+
+ */
+
+#endif

src/tool/threadSafeQueue.h

@@ -0,0 +1,101 @@
+//
+// Created by zx on 2019/12/17.
+//
+
+#ifndef SRC_THREADSAFEQUEUE_H
+#define SRC_THREADSAFEQUEUE_H
+
+
+#include <queue>
+#include <memory>
+#include <mutex>
+#include <condition_variable>
+template<typename T>
+class threadsafe_queue
+{
+private:
+    mutable std::mutex mut;
+    std::queue<T> data_queue;
+    std::condition_variable data_cond;
+public:
+    threadsafe_queue() {}
+    threadsafe_queue(threadsafe_queue const& other)
+    {
+        std::lock_guard<std::mutex> lk(other.mut);
+        data_queue = other.data_queue;
+    }
+    ~threadsafe_queue()
+    {
+        while (!empty())
+        {
+            try_pop();
+        }
+    }
+    size_t size()
+    {
+        return data_queue.size();
+    }
+    void push(T new_value)//��Ӳ���
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        data_queue.push(new_value);
+        data_cond.notify_one();
+    }
+    void wait_and_pop(T& value)//ֱ����Ԫ�ؿ���ɾ��Ϊֹ
+    {
+        std::unique_lock<std::mutex> lk(mut);
+        data_cond.wait(lk, [this] {return !data_queue.empty(); });
+        value = data_queue.front();
+        data_queue.pop();
+    }
+    std::shared_ptr<T> wait_and_pop()
+    {
+        std::unique_lock<std::mutex> lk(mut);
+        data_cond.wait(lk, [this] {return !data_queue.empty(); });
+        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+        data_queue.pop();
+        return res;
+    }
+    //ֻ���� �� pop
+    bool front(T& value)
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        if (data_queue.empty())
+            return false;
+        value = data_queue.front();
+        return true;
+    }
+
+    bool try_pop(T& value)//������û�ж���Ԫ��ֱ�ӷ���
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        if (data_queue.empty())
+            return false;
+        value = data_queue.front();
+        data_queue.pop();
+        return true;
+    }
+    std::shared_ptr<T> try_pop()
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        if (data_queue.empty())
+            return std::shared_ptr<T>();
+        std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
+        data_queue.pop();
+        return res;
+    }
+    bool empty() const
+    {
+        std::lock_guard<std::mutex> lk(mut);
+        return data_queue.empty();
+    }
+    void clear()
+    {
+        while (!empty()) {
+            try_pop();
+        }
+    }
+};
+
+
+#endif //SRC_THREADSAFEQUEUE_H

src/tool/thread_condition.cpp

@@ -0,0 +1,175 @@
+
+
+/* Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+ * 线程创建后，一般是循环运行，
+ * 为了防止线程暂满整个cpu，那么需要线程在不工作的是否进入等待状态。
+ * Thread_condition 就可以控制线程的运行状态。
+ *
+	std::atomic<bool> m_pass_ever		//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> m_pass_once		//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+ * 外部调用notify系列的函数，唤醒等待的线程，让线程执行功能函数。
+ * 如果需要线程循环多次执行功能函数，那么就使用 notify_all(true)，后面的线程可以直接通过等待了。
+ * 再使用 notify_all(false) ，即可停止线程，让其继续等待。
+ * 如果只想要线程执行一次，那就使用 notify_all(false, true)
+ * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+ *
+ * m_kill_flag //是否杀死线程，让线程强制退出，
+ * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+
+ 注：notify唤醒线程之后，wait里面的判断函数会重新判断。
+ */
+
+#include "thread_condition.h"
+
+Thread_condition::Thread_condition()
+{
+	m_kill_flag = false;
+	m_pass_ever = false;
+	m_pass_once = false;
+	m_working_flag = false;
+}
+Thread_condition::~Thread_condition()
+{
+	kill_all();
+}
+
+//无限等待，由 is_pass_wait 决定是否阻塞。
+//返回m_pass，
+bool Thread_condition::wait()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_condition_variable.wait(loc,std::bind(is_pass_wait,this));
+	bool t_pass = is_pass_wait(this);
+	m_pass_once = false;
+
+	//只要前面通过了, 那就进入工作状态
+	m_working_flag = true;
+
+	return t_pass;
+}
+//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
+//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+//注意了：线程阻塞期间，是不会return的。
+bool Thread_condition::wait_for_millisecond(unsigned int millisecond)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_condition_variable.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(is_pass_wait, this));
+	bool t_pass = is_pass_wait(this);
+	m_pass_once = false;
+
+	//只要前面通过了, 那就进入工作状态 , 超时通过也算通过
+	m_working_flag = true;
+
+	return t_pass;
+}
+
+
+//唤醒已经阻塞的线程，唤醒一个线程
+//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+void Thread_condition::notify_one(bool pass_ever, bool pass_once)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_pass_ever = pass_ever;
+	m_pass_once = pass_once;
+	m_condition_variable.notify_one();
+}
+//唤醒已经阻塞的线程，唤醒全部线程
+//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+void Thread_condition::notify_all(bool pass_ever, bool pass_once)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_pass_ever = pass_ever;
+	m_pass_once = pass_once;
+	m_condition_variable.notify_all();
+}
+//注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+
+
+//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+void Thread_condition::kill_all()
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_kill_flag = true;
+	m_condition_variable.notify_all();
+}
+
+//判断是否存活，只有活着才能继续支持子线程从功能函数，否则需要强制退出函数并结束子线程
+bool Thread_condition::is_alive()
+{
+	return !m_kill_flag;
+}
+
+
+//判断是否等待, 外部线程通过这个函数来查询this线程的工作状态,
+bool Thread_condition::is_waiting()
+{
+	return !m_working_flag;
+}
+//判断是否工作, 外部线程通过这个函数来查询this线程的工作状态,
+bool Thread_condition::is_working()
+{
+	return m_working_flag;
+}
+
+
+bool Thread_condition::get_kill_flag()
+{
+	return m_kill_flag;
+}
+bool Thread_condition::get_pass_ever()
+{
+	return m_pass_ever;
+}
+bool Thread_condition::get_pass_once()
+{
+	return m_pass_once;
+}
+void Thread_condition::set_kill_flag(bool kill)
+{
+	m_kill_flag = kill;
+}
+void Thread_condition::set_pass_ever(bool pass_ever)
+{
+	m_pass_ever = pass_ever;
+}
+void Thread_condition::set_pass_once(bool pass_once)
+{
+	m_pass_once = pass_once;
+}
+void Thread_condition::reset(bool kill, bool pass_ever, bool pass_once)
+{
+	m_kill_flag = kill;
+	m_pass_ever = pass_ever;
+	m_pass_once = pass_once;
+}
+
+
+//判断线程是否可以通过等待，wait系列函数的判断标志
+//注：m_kill或者m_pass为真时，return true
+bool Thread_condition::is_pass_wait(Thread_condition * other)
+{
+	if ( other == NULL )
+	{
+		throw (other);
+		return false;
+	}
+
+	bool result = (other->m_kill_flag || other->m_pass_ever || other->m_pass_once);
+
+	//如果不能通过等待, 那么线程状态改为等待中,
+	if ( !result )
+	{
+		other->m_working_flag = false;
+	}
+
+
+	return result;
+}
+

src/tool/thread_condition.h

@@ -0,0 +1,183 @@
+
+
+/* Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+ * 线程创建后，一般是循环运行，
+ * 为了防止线程暂满整个cpu，那么需要线程在不工作的是否进入等待状态。
+ * Thread_condition 就可以控制线程的运行状态。
+ *
+	std::atomic<bool> m_pass_ever		//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> m_pass_once		//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+ * 外部调用notify系列的函数，唤醒等待的线程，让线程执行功能函数。
+ * 如果需要线程循环多次执行功能函数，那么就使用 notify_all(true)，后面的线程可以直接通过等待了。
+ * 再使用 notify_all(false) ，即可停止线程，让其继续等待。
+ * 如果只想要线程执行一次，那就使用 notify_all(false, true)
+ * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+ *
+ * m_kill_flag //是否杀死线程，让线程强制退出，
+ * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+
+ 注：notify唤醒线程之后，wait里面的判断函数会重新判断。
+
+
+ 最下面有使用样例,
+
+ */
+
+#ifndef LIDARMEASURE_THREAD_CONDITION_H
+#define LIDARMEASURE_THREAD_CONDITION_H
+
+#include <ratio>
+#include <chrono>
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+#include <functional>
+
+class Thread_condition
+{
+public:
+	Thread_condition();
+	Thread_condition(const Thread_condition& other) = delete;
+	~Thread_condition();
+
+	//无限等待，由 is_pass_wait 决定是否阻塞。
+	//返回m_pass，
+	bool wait();
+	//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	bool wait_for_millisecond(unsigned int millisecond);
+
+	//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	template<typename _Rep, typename _Period>
+	bool wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration);
+
+	//唤醒已经阻塞的线程，唤醒一个线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_one(bool pass_ever, bool pass_once = false);
+	//唤醒已经阻塞的线程，唤醒全部线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_all(bool pass_ever, bool pass_once = false);
+	//注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+	void kill_all();
+
+	//判断是否存活，只有活着才能继续支持子线程从功能函数，否则需要强制退出函数并结束子线程
+	bool is_alive();
+
+	//判断是否等待, 外部线程通过这个函数来查询this线程的工作状态,
+	bool is_waiting();
+	//判断是否工作, 外部线程通过这个函数来查询this线程的工作状态,
+	bool is_working();
+
+public:
+
+	bool get_kill_flag();
+	bool get_pass_ever();
+	bool get_pass_once();
+	void set_kill_flag(bool kill);
+	void set_pass_ever(bool pass_ever);
+	void set_pass_once(bool pass_once);
+	void reset(bool kill = false, bool pass_ever = false, bool pass_once = false);
+
+protected:
+	//判断线程是否可以通过等待，wait系列函数的判断标志
+	//注：m_kill或者m_pass为真时，return true
+	static bool is_pass_wait(Thread_condition * other);
+
+	std::atomic<bool> 		m_kill_flag;			//是否杀死线程，让线程强制退出，
+	std::atomic<bool> 		m_pass_ever;			//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> 		m_pass_once;			//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+
+	std::atomic<bool> 		m_working_flag;			//线程是否进入工作的标志位, false:表示线程进行进入wait等待, true:表示线程仍然在运行中,
+
+	std::mutex 				m_mutex;				//线程的锁
+	std::condition_variable m_condition_variable;	//线程的条件变量
+
+private:
+
+};
+
+//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
+//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+//注意了：线程阻塞期间，是不会return的。
+template<typename _Rep, typename _Period>
+bool Thread_condition::wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_condition_variable.wait_for(loc, std::chrono::duration<_Rep, _Period>(time_duration), std::bind(is_pass_wait, this));
+	bool t_pass = is_pass_wait(this);
+	m_pass_once = false;
+	return t_pass;
+}
+
+#endif //LIDARMEASURE_THREAD_CONDITION_H
+
+
+
+
+
+/*
+//使用样例:
+std::thread*						mp_thread_receive;    		//接受缓存的线程指针
+Thread_condition					m_condition_receive;		//接受缓存的条件变量
+
+void thread_receive()
+{
+	while (m_condition_receive.is_alive())
+	{
+		m_condition_receive.wait();
+		if ( m_condition_receive.is_alive() )
+		{
+			//do everything
+
+		}
+	}
+
+	return;
+}
+
+//main函数的主线程
+int main(int argc,char* argv[])
+{
+ 	//线程创建之后, 默认等待
+	m_condition_receive.reset(false, false, false);
+	mp_thread_receive = new std::thread(& thread_receive );
+
+
+	//唤醒所有线程, 然后线程可以一直通过wait等待, 线程进入无限制的循环工作.
+	m_condition_receive.notify_all(true);
+
+	//暂停所有线程, 然后线程还是继续工作, 直到下一次循环, 进入wait等待
+	m_condition_receive.notify_all(false);
+
+	//如果线程单次循环运行时间较长, 需要等待线程完全停止, 才能读写公共的内存,
+	if ( m_condition_receive.is_waiting() )
+	{
+	    // 读写公共的内存,
+	}
+
+	//唤醒一个线程, 然后线程循环一次, 然后下次循环进入等待
+	m_condition_receive.notify_all(false, true);
+
+
+	//杀死线程,
+	m_condition_receive.kill_all();
+
+	//在线程join结束之后, 就可以可以回收线程内存资源
+	mp_thread_receive->join();
+	delete mp_thread_receive;
+	mp_thread_receive = NULL;
+}
+*/

src/tool/thread_condition.puml

@@ -0,0 +1,96 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+note left of Thread_condition
+/* Thread_condition 是多线程的条件控制类，主要是控制线程的启停和退出
+ * 线程创建后，一般是循环运行，
+ * 为了防止线程暂满整个cpu，那么需要线程在不工作的是否进入等待状态。
+ * Thread_condition 就可以控制线程的运行状态。
+ *
+	std::atomic<bool> m_pass_ever		//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> m_pass_once		//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+ * 外部调用notify系列的函数，唤醒等待的线程，让线程执行功能函数。
+ * 如果需要线程循环多次执行功能函数，那么就使用 notify_all(true)，后面的线程可以直接通过等待了。
+ * 再使用 notify_all(false) ，即可停止线程，让其继续等待。
+ * 如果只想要线程执行一次，那就使用 notify_all(false, true)
+ * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+ *
+ * m_kill_flag //是否杀死线程，让线程强制退出，
+ * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+
+ 注：notify唤醒线程之后，wait里面的判断函数会重新判断。
+ */
+end note
+
+
+
+class Thread_condition
+{
+==public:==
+	Thread_condition();
+	Thread_condition(const Thread_condition& other) = delete;
+	~Thread_condition();
+..
+	//无限等待，由 is_pass_wait 决定是否阻塞。
+	//返回m_pass，
+	bool wait();
+..
+	//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	bool wait_for_millisecond(unsigned int millisecond);
+..
+	//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
+	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+	//注意了：线程阻塞期间，是不会return的。
+	template<typename _Rep, typename _Period>
+	bool wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration);
+..
+	//唤醒已经阻塞的线程，唤醒一个线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_one(bool pass_ever, bool pass_once = false);
+..
+	//唤醒已经阻塞的线程，唤醒全部线程
+	//pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
+	void notify_all(bool pass_ever, bool pass_once = false);
+	//注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
+..
+	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
+	// 注：只是修改m_kill为true，需要线程函数实时检测kill的状态，来return线程。
+	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
+	void kill_all();
+..
+	//判断是否或者，return !m_kill_flag
+	bool is_alive();
+==public:==
+	bool get_kill_flag();
+	bool get_pass_ever();
+	bool get_pass_once();
+	void set_kill_flag(bool kill);
+	void set_pass_ever(bool pass_ever);
+	void set_pass_once(bool pass_once);
+	void reset(bool kill = false, bool pass_ever = false, bool pass_once = false);
+==protected:==
+	//判断线程是否可以通过等待，wait系列函数的判断标志
+	//注：m_kill或者m_pass为真时，return true
+	static bool is_pass_wait(Thread_condition * other);
+
+	std::atomic<bool> 		m_kill_flag;			//是否杀死线程，让线程强制退出，
+	std::atomic<bool> 		m_pass_ever;			//线程能否直接通过等待，对后面的线程也生效。
+	std::atomic<bool> 		m_pass_once;			//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+
+	std::mutex 				m_mutex;				//线程的锁
+	std::condition_variable m_condition_variable;	//线程的条件变量
+
+==private:==
+
+}
+
+
+@enduml

src/tool/thread_safe_list.cpp

@@ -0,0 +1,6 @@
+
+
+
+#include "thread_safe_queue.h"
+
+

src/tool/thread_safe_list.h

@@ -0,0 +1,354 @@
+
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+ 增加了一些功能函数，
+ 补充了注释说明
+
+ termination_list
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
+
+ thread_safe_queue  就是在 Thread_safe_queue 的基础上修改的,
+
+ * */
+
+#ifndef __THREAD_SAFE_LIST_H__
+#define __THREAD_SAFE_LIST_H__
+
+#include <list>
+
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+
+
+template<class T>
+class Thread_safe_list
+{
+public:
+	Thread_safe_list();
+	Thread_safe_list(const Thread_safe_list& other);
+	~Thread_safe_list();
+
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	bool wait_and_pop(T& value);
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	bool try_pop(T& value);
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	std::shared_ptr<T> wait_and_pop();
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	std::shared_ptr<T> try_pop();
+	//插入一项，并唤醒一个线程，
+	//如果成功插入，则返回true，  失败则返回false
+	//注：只能唤醒一个线程，防止多线程误判empty()
+	bool push(T new_value);
+	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+	bool clear();
+	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+	bool clear_and_delete();
+
+public:
+	//判空
+	bool empty();
+	//获取队列大小
+	size_t size();
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+	void termination_list();
+	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_list();
+	//获取退出状态
+	bool get_termination_flag();
+	//判断是否可以直接通过wait，  m_data_list不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+
+public:
+	std::mutex 						m_mutex;				//队列的锁
+	std::list<std::shared_ptr<T>> 	m_data_list;			//队列数据，使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+
+private:
+
+
+};
+
+
+
+
+
+
+
+
+template<class T>
+Thread_safe_list<T>::Thread_safe_list()
+{
+	m_termination_flag = false;
+}
+template<class T>
+Thread_safe_list<T>::Thread_safe_list(const Thread_safe_list& other)
+{
+	std::unique_lock<std::mutex> lock_this(m_mutex);
+	std::unique_lock<std::mutex> lock_other(other.m_mutex);
+	m_data_list = other.data_list;
+	m_termination_flag = other.m_termination_flag;
+}
+template<class T>
+Thread_safe_list<T>::~Thread_safe_list()
+{
+	//析构时，终止队列，让线程通过等待，方便线程推出。
+	termination_list();
+}
+
+//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+bool Thread_safe_list<T>::wait_and_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_list.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return false;
+		}
+		else
+		{
+//			value = move(*m_data_list.front());
+			value = (*m_data_list.front());
+
+			m_data_list.pop_front();
+			return true;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+bool Thread_safe_list<T>::try_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_list.empty())
+		{
+			return false;
+		}
+		else
+		{
+//			value = move(*m_data_list.front());
+			value = (*m_data_list.front());
+
+			m_data_list.pop();
+			return true;
+		}
+	}
+}
+
+
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_list<T>::wait_and_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_list.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_list.front();
+			m_data_list.pop();
+			return res;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_list<T>::try_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_list.empty())
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_list.front();
+			m_data_list.pop();
+			return res;
+		}
+	}
+}
+//插入一项，并唤醒一个线程，
+//如果成功插入，则返回true，  失败则返回false
+//注：只能唤醒一个线程，防止多线程误判empty()
+template<class T>
+bool Thread_safe_list<T>::push(T new_value)
+{
+	if (m_termination_flag)
+	{
+		return false;
+	}
+	else
+	{
+//		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
+		std::shared_ptr<T> data(std::make_shared<T>((new_value)));
+		std::unique_lock<std::mutex> lk(m_mutex);
+		m_data_list.push_back(data);
+		m_data_cond.notify_one();
+		return true;
+	}
+}
+//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+template<class T>
+bool Thread_safe_list<T>::clear()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_list.empty())
+	{
+		m_data_list.pop_front();
+	}
+	return true;
+
+}
+
+//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+template<class T>
+bool Thread_safe_list<T>::clear_and_delete()
+{
+
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_list.empty())
+	{
+		T res = NULL;
+//		res = move(*m_data_list.front());
+		res = (*m_data_list.front());
+
+		m_data_list.pop_front();
+		if(res != NULL)
+		{
+			delete(res);
+
+		}
+	}
+	return true;
+}
+
+
+
+//判空
+template<class T>
+bool Thread_safe_list<T>::empty()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_list.empty();
+}
+//获取队列大小
+template<class T>
+size_t Thread_safe_list<T>::size()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_list.size();
+}
+//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+template<class T>
+void Thread_safe_list<T>::termination_list()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = true;
+	m_data_cond.notify_all();
+}
+//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+template<class T>
+void Thread_safe_list<T>::wake_list()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = false;
+	m_data_cond.notify_all();
+}
+//获取退出状态
+template<class T>
+bool Thread_safe_list<T>::get_termination_flag()
+{
+	return m_termination_flag;
+}
+//判断是否可以直接通过wait，  m_data_list不为空或者m_termination终止时都可以通过等待。
+template<class T>
+bool Thread_safe_list<T>::is_pass()
+{
+	return (!m_data_list.empty() || m_termination_flag);
+}
+
+
+
+
+
+
+#endif //__THREAD_SAFE_LIST_H__

src/tool/thread_safe_map.cpp

@@ -0,0 +1,5 @@
+//
+// Created by zx on 2020/7/3.
+//
+
+#include "thread_safe_map.h"

src/tool/thread_safe_map.h

@@ -0,0 +1,93 @@
+//
+// Created by zx on 2020/7/3.
+//
+
+#ifndef NNXX_TESTS_THREAD_SAFE_MAP_H
+#define NNXX_TESTS_THREAD_SAFE_MAP_H
+
+
+#include <map>
+#include <mutex>
+template<typename Key, typename Val>
+class thread_safe_map
+{
+public:
+    typedef typename std::map<Key, Val>::iterator this_iterator;
+    typedef typename std::map<Key, Val>::const_iterator this_const_iterator;
+    Val& operator [](const Key& key)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_[key];
+    }
+    int erase(const Key& key )
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.erase(key);
+    }
+
+    bool find_update(const Key& key,const Val& val)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        if(dataMap_.find(key)!=dataMap_.end()) {
+            dataMap_[key]=val;
+            return true;
+        }
+        return false;
+    }
+
+    bool find(const Key& key)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        if(dataMap_.find(key)!=dataMap_.end()) {
+            return true;
+        }
+        return false;
+    }
+
+    bool find(const Key& key,Val& val)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        if(dataMap_.find(key)!=dataMap_.end()) {
+            val=dataMap_[key];
+            return true;
+        }
+        return false;
+    }
+
+    /*this_iterator find( const Key& key )
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.find(key);
+    }
+    this_const_iterator find( const Key& key ) const
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.find(key);
+    }*/
+
+    this_iterator begin()
+    {
+        return dataMap_.begin();
+    }
+
+    this_iterator end()
+    {
+        return dataMap_.end();
+    }
+
+    this_const_iterator end() const
+    {
+        return dataMap_.end();
+    }
+
+    unsigned int size()
+    {
+        return dataMap_.size();
+    }
+private:
+    std::map<Key, Val> dataMap_;
+    std::mutex mtx_;
+};
+
+
+#endif //NNXX_TESTS_THREAD_SAFE_MAP_H

src/tool/thread_safe_queue.cpp

@@ -0,0 +1,34 @@
+
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic std::mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+引用了他的思路，增加了一些功能函数， 补充了注释说明
+
+ termination_queue
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif
+ 然后在调用方include包含cpp文件，但是这样不好。
+ * */
+
+#include "thread_safe_queue.h"
+
+

src/tool/thread_safe_queue.h

@@ -0,0 +1,334 @@
+
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+ 增加了一些功能函数，
+ 补充了注释说明
+
+ termination_queue
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
+
+ * */
+
+#ifndef LIDARMEASURE_THREAD_SAFE_QUEUE_H
+#define LIDARMEASURE_THREAD_SAFE_QUEUE_H
+
+#include <queue>
+
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+
+
+template<class T>
+class Thread_safe_queue
+{
+public:
+	Thread_safe_queue();
+	Thread_safe_queue(const Thread_safe_queue& other);
+	~Thread_safe_queue();
+
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	bool wait_and_pop(T& value);
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	bool try_pop(T& value);
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	std::shared_ptr<T> wait_and_pop();
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	std::shared_ptr<T> try_pop();
+	//插入一项，并唤醒一个线程，
+	//如果成功插入，则返回true，  失败则返回false
+	//注：只能唤醒一个线程，防止多线程误判empty()
+	bool push(T new_value);
+	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+	bool clear();
+	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+	bool clear_and_delete();
+
+public:
+	//判空
+	bool empty();
+	//获取队列大小
+	size_t size();
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+	void termination_queue();
+	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_queue();
+	//获取退出状态
+	bool get_termination_flag();
+	//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+
+protected:
+	std::mutex 						m_mutex;				//队列的锁
+	std::queue<std::shared_ptr<T>> 	m_data_queue;			//队列数据，使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+
+private:
+
+
+};
+
+
+
+template<class T>
+Thread_safe_queue<T>::Thread_safe_queue()
+{
+	m_termination_flag = false;
+}
+template<class T>
+Thread_safe_queue<T>::Thread_safe_queue(const Thread_safe_queue& other)
+{
+	std::unique_lock<std::mutex> lock_this(m_mutex);
+	std::unique_lock<std::mutex> lock_other(other.m_mutex);
+	m_data_queue = other.data_queue;
+	m_termination_flag = other.m_termination_flag;
+}
+template<class T>
+Thread_safe_queue<T>::~Thread_safe_queue()
+{
+	//析构时，终止队列，让线程通过等待，方便线程推出。
+	termination_queue();
+}
+
+//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+bool Thread_safe_queue<T>::wait_and_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_queue.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return false;
+		}
+		else
+		{
+			value = move(*m_data_queue.front());
+			m_data_queue.pop();
+			return true;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+bool Thread_safe_queue<T>::try_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_queue.empty())
+		{
+			return false;
+		}
+		else
+		{
+			value = move(*m_data_queue.front());
+			m_data_queue.pop();
+			return true;
+		}
+	}
+}
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_queue<T>::wait_and_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_queue.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_queue.front();
+			m_data_queue.pop();
+			return res;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_queue<T>::try_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_queue.empty())
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_queue.front();
+			m_data_queue.pop();
+			return res;
+		}
+	}
+}
+//插入一项，并唤醒一个线程，
+//如果成功插入，则返回true，  失败则返回false
+//注：只能唤醒一个线程，防止多线程误判empty()
+template<class T>
+bool Thread_safe_queue<T>::push(T new_value)
+{
+	if (m_termination_flag)
+	{
+		return false;
+	}
+	else
+	{
+		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
+		std::unique_lock<std::mutex> lk(m_mutex);
+		m_data_queue.push(data);
+		m_data_cond.notify_one();
+		return true;
+	}
+}
+//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+template<class T>
+bool Thread_safe_queue<T>::clear()
+{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		while (!m_data_queue.empty())
+		{
+			m_data_queue.pop();
+		}
+		return true;
+
+}
+//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+template<class T>
+bool Thread_safe_queue<T>::clear_and_delete()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_queue.empty())
+	{
+		T res = NULL;
+		res = move(*m_data_queue.front());
+		m_data_queue.pop();
+		if(res != NULL)
+		{
+			delete(res);
+
+		}
+	}
+	return true;
+
+}
+
+//判空
+template<class T>
+bool Thread_safe_queue<T>::empty()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_queue.empty();
+}
+//获取队列大小
+template<class T>
+size_t Thread_safe_queue<T>::size()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_queue.size();
+}
+//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+template<class T>
+void Thread_safe_queue<T>::termination_queue()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = true;
+	m_data_cond.notify_all();
+}
+//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+template<class T>
+void Thread_safe_queue<T>::wake_queue()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = false;
+	m_data_cond.notify_all();
+}
+//获取退出状态
+template<class T>
+bool Thread_safe_queue<T>::get_termination_flag()
+{
+	return m_termination_flag;
+}
+//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
+template<class T>
+bool Thread_safe_queue<T>::is_pass()
+{
+	return (!m_data_queue.empty() || m_termination_flag);
+}
+
+
+
+
+
+
+#endif //LIDARMEASURE_THREAD_SAFE_QUEUE_H

src/tool/thread_safe_queue.puml

@@ -0,0 +1,108 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  Thread_safe_queue 安全线程队列
+
+note left of Thread_safe_queue
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+ 增加了一些功能函数，
+ 补充了注释说明
+
+ termination_queue
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
+
+ * */
+end note
+
+
+
+class Thread_safe_queue <<    template<class T>    >>
+{
+==public:==
+	Thread_safe_queue();
+	Thread_safe_queue(const Thread_safe_queue& other);
+	~Thread_safe_queue();
+..
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	bool wait_and_pop(T& value);
+..
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	bool try_pop(T& value);
+..
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	std::shared_ptr<T> wait_and_pop();
+..
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	std::shared_ptr<T> try_pop();
+..
+	//插入一项，并唤醒一个线程，
+	//如果成功插入，则返回true，  失败则返回false
+	//注：只能唤醒一个线程，防止多线程误判empty()
+	bool push(T new_value);
+..
+	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+	bool clear();
+..
+	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+	bool clear_and_delete();
+==public:==
+	//判空
+	bool empty();
+..
+	//获取队列大小
+	size_t size();
+..
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+	void termination_queue();
+..
+	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_queue();
+..
+	//获取退出状态
+	bool get_termination_flag();
+..
+	//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+==protected:==
+	std::mutex 						m_mutex;				//队列的锁
+	std::queue<std::shared_ptr<T>> 	m_data_queue;			//队列数据，使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+==private:==
+}
+
+
+@enduml

src/tool/tick.hpp

@@ -0,0 +1,46 @@
+//
+// Created by zx on 2021/5/19.
+//
+
+#ifndef ROBOT_CONTROL_FEATURE_EXTRA_SRC_TICK_HPP_
+#define ROBOT_CONTROL_FEATURE_EXTRA_SRC_TICK_HPP_
+
+#include <chrono>
+
+class Tick
+{
+ public:
+    Tick()
+    {
+        m_start=std::chrono::steady_clock::now();
+        m_last_tick=m_start;
+    }
+    Tick(const Tick& tick)= default;
+    Tick& operator=(const Tick& tick)= default;
+    double tic()
+    {
+        auto tic=std::chrono::steady_clock::now();
+        m_last_tick=tic;
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(tic - m_start);
+        return double(duration.count()) * std::chrono::microseconds::period::num /
+                std::chrono::microseconds::period::den;
+    }
+    double tic_relatively()
+    {
+        auto tic=std::chrono::steady_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(tic - m_last_tick);
+        m_last_tick=tic;
+        return double(duration.count()) * std::chrono::microseconds::period::num /
+                std::chrono::microseconds::period::den;
+    }
+
+ private:
+    std::chrono::steady_clock::time_point     m_start;
+    std::chrono::steady_clock::time_point     m_last_tick;
+};
+
+
+
+
+
+#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_TICK_HPP_

src/vlp16.hpp

@@ -0,0 +1,563 @@
+//
+// Created by zx on 2021/5/20.
+//
+
+#ifndef LIDARMEASURE_WJ_LIDAR_VLP16_HPP_
+#define LIDARMEASURE_WJ_LIDAR_VLP16_HPP_
+
+
+// VelodyneCapture
+//
+// VelodyneCapture is the capture class to retrieve the laser data from Velodyne sensors using Boost.Asio and PCAP.
+// VelodyneCapture will be able to retrieve lasers infomation about azimuth, vertical and distance that capture at one rotation.
+// This class supports direct capture form Velodyne sensors, or capture from PCAP files.
+// ( This class only supports VLP-16 and HDL-32E sensor, and not supports Dual Return mode. )
+//
+// If direct capture from sensors, VelodyneCapture are requires Boost.Asio and its dependent libraries ( Boost.System, Boost.Date_Time, Boost.Regex ).
+// Please define HAVE_BOOST in preprocessor.
+//
+// If capture from PCAP files, VelodyneCapture are requires PCAP.
+// Please define HAVE_PCAP in preprocessor.
+//
+// This source code is licensed under the MIT license. Please see the License in License.txt.
+// Copyright (c) 2017 Tsukasa SUGIURA
+// t.sugiura0204@gmail.com
+
+#define HAVE_BOOST 1
+
+#include <string>
+#include <sstream>
+#include <thread>
+#include <atomic>
+#include <mutex>
+#include <queue>
+#include <vector>
+#include <cassert>
+#include <cstdint>
+#include <chrono>
+#include <iomanip>
+#include <algorithm>
+#include <functional>
+#ifdef HAVE_BOOST
+#include <boost/asio.hpp>
+#include <boost/property_tree/ptree.hpp>
+#include <boost/property_tree/xml_parser.hpp>
+#endif
+#ifdef HAVE_PCAP
+#include <pcap.h>
+#endif
+#define EPSILON 0.001
+#include "tick.hpp"
+namespace velodyne
+{
+struct Laser
+{
+    double azimuth;
+    double vertical;
+    float distance;
+    unsigned char intensity;
+    unsigned char id;
+    long long time;
+
+    const bool operator < ( const struct Laser& laser ){
+        if( azimuth == laser.azimuth ){
+            return id < laser.id;
+        }
+        else{
+            return azimuth < laser.azimuth;
+        }
+    }
+};
+struct Frame
+{
+    std::vector<Laser>      scans;
+    Tick                    tic;
+};
+
+class VelodyneCapture
+{
+ protected:
+#ifdef HAVE_BOOST
+    boost::asio::io_service ioservice;
+            boost::asio::ip::udp::socket* socket = nullptr;
+            boost::asio::ip::address address;
+            unsigned short port = 2368;
+#endif
+
+#ifdef HAVE_PCAP
+    pcap_t* pcap = nullptr;
+            std::string filename = "";
+#endif
+
+    std::thread* thread = nullptr;
+    std::atomic_bool run = { false };
+    std::mutex mutex;
+    Frame       m_frame;
+
+    int max_num_lasers;
+    std::vector<double> lut;
+    double time_between_firings;
+    double time_half_idle;
+    double time_total_cycle;
+
+    static const int LASER_PER_FIRING = 32;
+    static const int FIRING_PER_PKT = 12;
+
+#pragma pack(push, 1)
+    typedef struct LaserReturn
+    {
+        uint16_t distance;
+        uint8_t intensity;
+    } LaserReturn;
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+    struct FiringData
+    {
+        uint16_t blockIdentifier;
+        uint16_t rotationalPosition;
+        LaserReturn laserReturns[LASER_PER_FIRING];
+    };
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+    struct DataPacket
+    {
+        FiringData firingData[FIRING_PER_PKT];
+        uint32_t gpsTimestamp;
+        uint8_t mode;
+        uint8_t sensorType;
+    };
+#pragma pack(pop)
+
+ public:
+    // Constructor
+    VelodyneCapture()
+    {
+    };
+
+#ifdef HAVE_BOOST
+    // Constructor ( direct capture from Sensor )
+            VelodyneCapture( const boost::asio::ip::address& address, const unsigned short port = 2368 )
+            {
+                open( address, port );
+            };
+#endif
+
+#ifdef HAVE_PCAP
+    // Constructor ( capture from PCAP )
+            VelodyneCapture( const std::string& filename )
+            {
+                open( filename );
+            };
+#endif
+
+    // Destructor
+    ~VelodyneCapture()
+    {
+        close();
+    };
+
+#ifdef HAVE_BOOST
+    // Open Direct Capture from Sensor
+            const bool open( const boost::asio::ip::address& address, const unsigned short port = 2368 )
+            {
+                // Check Running
+                if( isRun() ){
+                    close();
+                }
+
+                // Set IP-Address and Port
+                this->address = ( !address.is_unspecified() ) ? address : boost::asio::ip::address::from_string( "255.255.255.255" );
+                this->port = port;
+
+                // Create Socket
+                try{
+                    socket = new boost::asio::ip::udp::socket( ioservice, boost::asio::ip::udp::endpoint( this->address, this->port ) );
+                }
+                catch( ... ){
+                    delete socket;
+                    socket = new boost::asio::ip::udp::socket( ioservice, boost::asio::ip::udp::endpoint( boost::asio::ip::address_v4::any(), this->port ) );
+                }
+
+                // Start IO-Service
+                try{
+                    ioservice.run();
+                }
+                catch( const std::exception& e ){
+                    std::cerr << e.what() << std::endl;
+                    return false;
+                }
+
+                // Start Capture Thread
+                run = true;
+                thread = new std::thread( std::bind( &VelodyneCapture::captureSensor, this ) );
+
+                return true;
+            };
+#endif
+
+#ifdef HAVE_PCAP
+    // Open Capture from PCAP
+            const bool open( const std::string& filename )
+            {
+                // Check Running
+                if( isRun() ){
+                    close();
+                }
+
+                // Open PCAP File
+                char error[PCAP_ERRBUF_SIZE];
+                pcap_t* pcap = pcap_open_offline( filename.c_str(), error );
+                if( !pcap ){
+                    throw std::runtime_error( error );
+                    return false;
+                }
+
+                // Convert PCAP_NETMASK_UNKNOWN to 0xffffffff
+                struct bpf_program filter;
+                std::ostringstream oss;
+                if( pcap_compile( pcap, &filter, oss.str().c_str(), 0, 0xffffffff ) == -1 ){
+                    throw std::runtime_error( pcap_geterr( pcap ) );
+                    return false;
+                }
+
+                if( pcap_setfilter( pcap, &filter ) == -1 ){
+                    throw std::runtime_error( pcap_geterr( pcap ) );
+                    return false;
+                }
+
+                this->pcap = pcap;
+                this->filename = filename;
+
+                // Start Capture Thread
+                run = true;
+                thread = new std::thread( std::bind( &VelodyneCapture::capturePCAP, this ) );
+
+                return true;
+            };
+#endif
+
+    // Check Open
+    const bool isOpen()
+    {
+        std::lock_guard<std::mutex> lock( mutex );
+        return (
+#if defined( HAVE_BOOST ) || defined( HAVE_PCAP )
+        #ifdef HAVE_BOOST
+                    ( socket && socket->is_open() )
+                    #endif
+                    #if defined( HAVE_BOOST ) && defined( HAVE_PCAP )
+                    ||
+                    #endif
+                    #ifdef HAVE_PCAP
+                    pcap != nullptr
+                    #endif
+#else
+                false
+#endif
+        );
+    };
+
+    // Check Run
+    const bool isRun()
+    {
+        // Returns True when Thread is Running or Queue is Not Empty
+        std::lock_guard<std::mutex> lock( mutex );
+        return ( run );
+    }
+
+    // Close Capture
+    void close()
+    {
+        run = false;
+        // Close Capturte Thread
+        if( thread && thread->joinable() ){
+            thread->join();
+            thread->~thread();
+            delete thread;
+            thread = nullptr;
+        }
+
+        std::lock_guard<std::mutex> lock( mutex );
+#ifdef HAVE_BOOST
+        // Close Socket
+                if( socket && socket->is_open() ){
+                    socket->close();
+                    delete socket;
+                    socket = nullptr;
+                }
+
+                // Stop IO-Service
+                if( ioservice.stopped() ){
+                    ioservice.stop();
+                    ioservice.reset();
+                }
+#endif
+
+#ifdef HAVE_PCAP
+        // Close PCAP
+                if( pcap ){
+                    pcap_close( pcap );
+                    pcap = nullptr;
+                    filename = "";
+                }
+#endif
+    };
+
+    // Retrieve Capture Data
+    void get_frame( struct Frame& frame )
+    {
+        // Pop One Rotation Data from Queue
+        if( mutex.try_lock() ){
+            frame=m_frame;
+            mutex.unlock();
+        }
+    };
+
+ private:
+    void parseDataPacket( const DataPacket* packet, std::vector<Laser>& lasers, double& last_azimuth )
+    {
+        if( packet->sensorType != 0x21 && packet->sensorType != 0x22 ){
+            throw( std::runtime_error( "This sensor is not supported" ) );
+        }
+        if( packet->mode != 0x37 && packet->mode != 0x38){
+            throw( std::runtime_error( "Sensor can't be set in dual return mode" ) );
+        }
+
+        // Retrieve Unix Time ( microseconds )
+        const std::chrono::time_point<std::chrono::system_clock> now = std::chrono::system_clock::now();
+        const std::chrono::microseconds epoch = std::chrono::duration_cast<std::chrono::microseconds>( now.time_since_epoch() );
+        const long long unixtime = epoch.count();
+
+        // Azimuth delta is the angle from one firing sequence to the next one
+        double azimuth_delta = 0.0;
+        if( packet->firingData[1].rotationalPosition < packet->firingData[0].rotationalPosition ){
+            azimuth_delta = ( ( packet->firingData[1].rotationalPosition + 36000 ) - packet->firingData[0].rotationalPosition );
+        }
+        else{
+            azimuth_delta = ( packet->firingData[1].rotationalPosition - packet->firingData[0].rotationalPosition );
+        }
+
+        // Processing Packet
+        for( int firing_index = 0; firing_index < FIRING_PER_PKT; firing_index++ ){
+            // Retrieve Firing Data
+            const FiringData firing_data = packet->firingData[firing_index];
+            for( int laser_index = 0; laser_index < LASER_PER_FIRING; laser_index++ ){
+                // Retrieve Rotation Azimuth
+                double azimuth = static_cast<double>( firing_data.rotationalPosition );
+                double laser_relative_time = laser_index * time_between_firings + time_half_idle* (laser_index / max_num_lasers);
+
+                azimuth += azimuth_delta * laser_relative_time / time_total_cycle;
+
+                // Reset Rotation Azimuth
+                if( azimuth >= 36000 )
+                {
+                    azimuth -= 36000;
+                }
+
+                // Complete Retrieve Capture One Rotation Data
+#ifndef PUSH_SINGLE_PACKETS
+                if( last_azimuth > azimuth ){
+                    // Push One Rotation Data to Queue
+                    mutex.lock();
+                    m_frame.scans=lasers;
+                    m_frame.tic=Tick();
+                    mutex.unlock();
+                    lasers.clear();
+                }
+#endif
+#ifdef NO_EMPTY_RETURNS
+                if( firing_data.laserReturns[laser_index].distance < EPSILON ){
+                          continue;
+                      }
+#endif
+                Laser laser;
+                laser.azimuth = azimuth / 100.0f;
+                laser.vertical = lut[laser_index % max_num_lasers];
+#ifdef USE_MILLIMETERS
+                laser.distance = static_cast<float>( firing_data.laserReturns[laser_index].distance ) * 2.0f;
+#else
+                laser.distance = static_cast<float>( firing_data.laserReturns[laser_index].distance ) * 2.0f / 10.0f;
+#endif
+                laser.intensity = firing_data.laserReturns[laser_index].intensity;
+                laser.id = static_cast<unsigned char>( laser_index % max_num_lasers );
+#ifdef HAVE_GPSTIME
+                laser.time = packet->gpsTimestamp + static_cast<long long>( laser_relative_time );
+#else
+                laser.time = unixtime + static_cast<long long>( laser_relative_time );
+#endif
+                lasers.push_back( laser );
+                // Update Last Rotation Azimuth
+                last_azimuth = azimuth;
+            }
+        }
+#ifdef PUSH_SINGLE_PACKETS
+        // Push packet after processing
+              mutex.lock();
+              queue.push( std::move( lasers ) );
+              mutex.unlock();
+              lasers.clear();
+#endif
+
+    };
+#ifdef HAVE_BOOST
+    // Capture Thread from Sensor
+            void captureSensor()
+            {
+                double last_azimuth = 0.0;
+                std::vector<Laser> lasers;
+                unsigned char data[1500];
+                boost::asio::ip::udp::endpoint sender;
+
+                while( socket->is_open() && ioservice.stopped() && run )
+                {
+
+                    // Receive Packet
+                    boost::system::error_code error;
+                    const size_t length = socket->receive_from( boost::asio::buffer( data, sizeof( data ) ), sender, 0, error );
+                    if( error == boost::asio::error::eof ){
+                        break;
+                    }
+
+                    // Check IP-Address and Port
+                    if( sender.address() != address && sender.port() != port ){
+                        continue;
+                    }
+
+                    // Check Packet Data Size
+                    // Data Blocks ( 100 bytes * 12 blocks ) + Time Stamp ( 4 bytes ) + Factory ( 2 bytes )
+                    if( length != 1206 ){
+                        continue;
+                    }
+
+                    // Convert to DataPacket Structure
+                    // Sensor Type 0x21 is HDL-32E, 0x22 is VLP-16
+                    const DataPacket* packet = reinterpret_cast<const DataPacket*>( data );
+                    parseDataPacket(  packet, lasers, last_azimuth );
+
+                }
+                run = false;
+            };
+#endif
+
+#ifdef HAVE_PCAP
+    // Capture Thread from PCAP
+            void capturePCAP()
+            {
+                double last_azimuth = 0.0;
+                std::vector<Laser> lasers;
+
+                while( run ){
+                    // Retrieve Header and Data from PCAP
+                    struct pcap_pkthdr* header;
+                    const unsigned char* data;
+                    const int ret = pcap_next_ex( pcap, &header, &data );
+                    if( ret <= 0 ){
+                        break;
+                    }
+
+                    // Check Packet Data Size
+                    // Data Blocks ( 100 bytes * 12 blocks ) + Time Stamp ( 4 bytes ) + Factory ( 2 bytes )
+                    if( ( header->len - 42 ) != 1206 ){
+                        continue;
+                    }
+
+                    // Retrieve Unix Time ( microseconds )
+                    std::stringstream ss;
+                    ss << header->ts.tv_sec << std::setw( 6 ) << std::right << std::setfill( '0' ) << header->ts.tv_usec;
+                    const long long unixtime = std::stoll( ss.str() );
+
+                    // Convert to DataPacket Structure ( Cut Header 42 bytes )
+                    // Sensor Type 0x21 is HDL-32E, 0x22 is VLP-16
+                    const DataPacket* packet = reinterpret_cast<const DataPacket*>( data + 42 );
+                    parseDataPacket(packet, lasers, last_azimuth);
+                }
+                run = false;
+            };
+#endif
+};
+
+class VLP16Capture : public VelodyneCapture
+{
+ public:
+    VLP16Capture() : VelodyneCapture()
+    {
+        initialize();
+    };
+
+#ifdef HAVE_BOOST
+    VLP16Capture( const boost::asio::ip::address& address, const unsigned short port = 2368 ) : VelodyneCapture()
+            {
+                initialize();
+                open( address, port );
+            };
+#endif
+
+#ifdef HAVE_PCAP
+    VLP16Capture( const std::string& filename ) : VelodyneCapture()
+            {
+                initialize();
+                open( filename );
+            };
+#endif
+
+    ~VLP16Capture()
+    {
+    };
+
+ private:
+    void initialize()
+    {
+        max_num_lasers       = 16;
+        lut                  = { -15.0, 1.0, -13.0, 3.0, -11.0, 5.0, -9.0, 7.0, -7.0, 9.0, -5.0, 11.0, -3.0, 13.0, -1.0, 15.0 };
+        time_between_firings = 2.304;
+        time_half_idle       = 18.432;
+        time_total_cycle     = 110.592; // 55.296*2;
+    };
+};
+
+class HDL32ECapture : public VelodyneCapture
+{
+ public:
+    HDL32ECapture() : VelodyneCapture()
+    {
+        initialize();
+    };
+
+#ifdef HAVE_BOOST
+    HDL32ECapture( const boost::asio::ip::address& address, const unsigned short port = 2368 ) : VelodyneCapture()
+            {
+                initialize();
+                open( address, port );
+            };
+#endif
+
+#ifdef HAVE_PCAP
+    HDL32ECapture( const std::string& filename ) : VelodyneCapture()
+            {
+                initialize();
+                open( filename );
+            };
+#endif
+
+    ~HDL32ECapture()
+    {
+    };
+
+ private:
+    void initialize()
+    {
+        max_num_lasers       = 32;
+        lut                  = { -30.67, -9.3299999, -29.33, -8.0, -28, -6.6700001, -26.67, -5.3299999, -25.33, -4.0, -24.0, -2.6700001, -22.67, -1.33, -21.33,
+                0.0, -20.0, 1.33, -18.67, 2.6700001, -17.33, 4.0, -16, 5.3299999, -14.67, 6.6700001, -13.33, 8.0, -12.0, 9.3299999, -10.67, 10.67 };
+        time_between_firings = 1.152;
+        time_half_idle       = 0.0;
+        time_total_cycle     = 46.080;
+    };
+};
+}
+
+
+
+#endif //LIDARMEASURE_WJ_LIDAR_VLP16_HPP_

src/vlp16_sample.cpp

@@ -0,0 +1,27 @@
+//
+// Created by zx on 2021/5/20.
+//
+# define HAVE_BOOST 1
+#include <iostream>
+#include <boost/asio.hpp>
+#include "ground_region.h"
+
+
+int main(int argc,char* argv[])
+{
+    if(argc<2)
+    {
+        printf(" argv must  > 1 \n");
+        return -1;
+    }
+    Ground_region ground;
+    Error_manager code=ground.init(argv[1]);
+    if(code!=SUCCESS)
+    {
+        std::cout<<code.get_error_description()<<std::endl;
+        return -2;
+    }
+
+    getchar();
+    return 0;
+}