添加内容

CMakeLists.txt

@@ -0,0 +1,125 @@
+# 定义cmake的最低版本
+cmake_minimum_required(VERSION 3.5)
+
+# 定义工程名称
+project(Project)
+
+option(TEST "Enable test" ON)
+option(GFLAGS "Enable Google Flags." ON)
+option(DataToCloud "Enable collect data to mqtt server." ON)
+option(AlgWheelDetect "Enable detect wheel data." ON)
+option(AlgAddTest "Enable test alg module." ON)
+option(ClampSafety "Enable clamp safety." ON)
+option(WatchDog "Enable watch dog." ON)
+option(MeasureNode "Enable Measure node." ON)
+option(LidarICP "Enable LidarICP" ON)
+
+# 指定安装目录
+if (CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64")
+    set(CMAKE_INSTALL_PREFIX
+            /out
+            )
+else ()
+    set(CMAKE_INSTALL_PREFIX
+            ${PROJECT_SOURCE_DIR}/build/out
+            )
+endif ()
+if (TEST)
+    set(CMAKE_INSTALL_PREFIX
+            ${PROJECT_SOURCE_DIR}/build/out
+            )
+else ()
+    set(CMAKE_INSTALL_PREFIX
+            /out
+            )
+    add_definitions(-DETC_PATH="${CMAKE_INSTALL_PREFIX}")
+endif ()
+
+SET (CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+set (CMAKE_BUILD_TYPE "RELEASE")
+
+# 打印工程定级目录（非必要）
+message("project path: " ${PROJECT_SOURCE_DIR})
+
+execute_process(COMMAND bash ${PROJECT_SOURCE_DIR}/etc/proto.sh ${PROJECT_SOURCE_DIR})
+# 第三方库
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(nanomsg REQUIRED nanomsg)
+pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
+find_path(YAML_CPP_INCLUDE_DIR
+        NAMES yaml_cpp.h
+        PATHS ${YAML_CPP_INCLUDE_DIRS})
+find_library(YAML_CPP_LIBRARY
+        NAMES YAML_CPP
+        PATHS ${YAML_CPP_LIBRARY_DIRS})
+if (NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
+    add_definitions(-DHAVE_NEW_YAMLCPP)
+endif (NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
+add_subdirectory(thirdpart/rs_driver)
+
+# Don't search with REQUIRED as we can continue without gflags.
+find_package(gflags 2.2.0)
+if (gflags_FOUND)
+    if (TARGET gflags)
+        message("-- Found Google Flags (gflags) version ${gflags_VERSION}: ${gflags_DIR}")
+        message("-- Found Google Flags (gflags) version ${gflags_VERSION}: ${GFLAGS_LIBRARIES}")
+    else()
+        message("-- Detected version of gflags: ${gflags_VERSION} does not define "
+                "expected gflags CMake target which should be exported by gflags 2.2+. "
+                "Building without gflags.")
+        update_cache_variable(GFLAGS OFF)
+    endif()
+else (gflags_FOUND)
+    message("-- Did not find Google Flags (gflags), Building without gflags.")
+    update_cache_variable(GFLAGS OFF)
+endif (gflags_FOUND)
+
+find_package(PCL REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(Ceres REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(rs_driver REQUIRED)
+find_package(Protobuf REQUIRED)
+
+include_directories(
+        ${rs_driver_INCLUDE_DIRS}
+        ${OpenCV_INCLUDE_DIRS}
+        ${EIGEN3_INCLUDE_DIRS}
+        ${CERES_INCLUDE_DIR}
+        ${PCL_INCLUDE_DIRS}
+        ${PROJECT_SOURCE_DIR}/include
+        ${PROJECT_SOURCE_DIR}/Modules
+)
+
+# 添加子目录CMAKE
+add_subdirectory(include)
+if(DataToCloud)
+    add_subdirectory(Modules/DataToCloud)
+endif ()
+if(AlgWheelDetect)
+    add_subdirectory(Modules/AlgWheelDetect)
+endif ()
+if(AlgAddTest)
+    add_subdirectory(Modules/AlgAddTest)
+endif ()
+if(ClampSafety)
+    add_subdirectory(Modules/ClampSafety)
+endif ()
+if(WatchDog)
+    add_subdirectory(Modules/WatchDog)
+endif ()
+if(MeasureNode)
+    include_directories(${PROJECT_SOURCE_DIR}/Modules/MeasureNode)
+    add_subdirectory(Modules/MeasureNode)
+endif ()
+
+if(LidarICP)
+    include_directories(${PROJECT_SOURCE_DIR}/Modules/LidarICP)
+    add_subdirectory(Modules/LidarICP)
+endif ()
+
+install(DIRECTORY ${PROJECT_SOURCE_DIR}/etc
+        DESTINATION ${CMAKE_INSTALL_PREFIX}
+        )
+

Modules/AlgAddTest/CMakeLists.txt

@@ -0,0 +1,39 @@
+# 测试相关代码
+if(TEST)
+    add_definitions(-DETC_PATH="${CMAKE_CURRENT_LIST_DIR}")
+endif ()
+
+# 添加文件
+set(AlgTestFile
+        algTest.cpp
+        fit_wheel_ret.h
+        fit_wheel_ret.cpp
+        define.pb.h
+        define.pb.cc
+)
+
+set(AlgTestLib
+        zx
+
+        )
+
+add_executable(AlgTest ${AlgTestFile})
+# 添加链接静态库
+target_link_libraries(AlgTest
+        ${AlgTestLib}
+        )
+
+# 将库文件，可执行文件，头文件安装到指定目录
+#GuiYangTest GuiYangReceive
+install(TARGETS AlgTest
+        LIBRARY DESTINATION lib  # 动态库安装路径
+        ARCHIVE DESTINATION lib  # 静态库安装路径
+        RUNTIME DESTINATION bin  # 可执行文件安装路径
+        PUBLIC_HEADER DESTINATION include  # 头文件安装路径
+        )
+
+install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/etc
+        DESTINATION ${CMAKE_INSTALL_PREFIX}
+        )
+
+

Modules/AlgAddTest/algTest.cpp

@@ -0,0 +1,18 @@
+/**
+  * @project Project
+  * @brief   Use to test alg.
+  * @author  lz
+  * @data    2023/6/16
+ **/
+
+#include <iostream>
+#include "defines.hpp"
+#include "pahoc/mqtt_async.h"
+
+const char *ETC_FILE = ETC_PATH "/etc/test.json";
+
+int main(int argc, char **agrv) {
+
+    printf("%s", ETC_FILE);
+    return EXIT_SUCCESS;
+}

Modules/AlgAddTest/define.proto

@@ -0,0 +1,111 @@
+syntax = "proto2";
+
+// 算法测量结果内容
+message LossResult {
+  required float lf_loss = 1;
+  required float rf_loss = 2;
+  required float lb_loss = 3;
+  required float rb_loss = 4;
+  required float total_avg_loss = 5;
+};
+message DetectResult {
+  required float cx = 1;
+  required float cy = 2;
+  required float theta = 3;
+  required float wheel_base = 4;
+  required float width = 5;
+  required float body_width = 6;
+  required float front_theta = 7;
+  required float single_wheel_width = 8;
+  required float single_wheel_length = 9;
+  required bool success = 10;
+  required LossResult loss = 11;
+};
+
+// 区域配置
+message RegionEtc {
+  required double maxx = 1;
+  required double minx = 2;
+  required double maxy = 3;
+  required double miny = 4;
+  required double maxz = 5;
+  required double minz = 6;
+};
+
+message Region
+{
+  required RegionEtc cut_region = 1;
+  required float turnplate_cx = 2;
+  required float turnplate_cy = 3;
+  required float border_minx = 4; // 最小边界x，左超界提示
+  required float border_maxx = 5; // 最大边界x，右超界提示
+  required float plc_offsetx = 6; // plc偏移x
+  required float plc_offsety = 7; // plc偏移y
+  required float plc_offset_degree = 8; // plc偏移角度
+  required float plc_border_miny = 9;// plc后夹持y方向极限值
+  required float plc_border_maxy = 10;// plc后夹持y方向极限值
+  required float car_min_width = 11; // 最小车宽
+  required float car_max_width = 12; // 最大车宽
+  required float car_min_wheelbase = 13; // 最小轴距
+  required float car_max_wheelbase = 14; // 最大轴距
+  required float turnplate_angle_min = 15;
+  required float turnplate_angle_max = 16;
+  required float turnplate_angle_limit_anti_clockwise = 17; // 转盘逆时针角度极限
+  required float turnplate_angle_limit_clockwise = 18; // 转盘顺时针角度极限
+}
+
+// 测量节点与雷达关系, 一个测量节点可以配置多个雷达
+message MeasureLidar {
+  required  string measureTopic = 1;          // 测量节点将测量结果发送的话题
+  required  Region region = 2;             // 测量节点裁剪雷达数据的配置
+  repeated string lidarTopic = 5;   // 测量节点雷达数据来源话题
+}
+
+// 测量节点有关配置
+message MeasureConfig {
+  required string sendTopics = 1;                // 测量节点发送的所有消息都在这个话题下
+  required string lidarTopics = 2;               // 雷达的监听节点
+  repeated MeasureLidar measures = 5;   // 测量节点所有的雷达关系
+}
+
+// 测量节点状态
+message measure_info {
+  required float cx = 1 [default = 0];                 // 车身中心x坐标
+  required float cy = 2 [default = 0];                 // 车身中心y坐标
+  required float theta = 3 [default = 0];              // 车身偏转角
+  required float length = 4 [default = 0];             // 车长
+  required float width = 5 [default = 0];              // 车宽
+  required float height = 6 [default = 0];             // 车高
+  required float wheelbase = 7 [default = 0];          // 轴距
+  required float front_theta = 8 [default = 0];        // 前轮角
+  required int32 border_statu = 9 [default = 0];       // 超界信息: 0x01:后超界, 0x02:左超界, 0x04:右超界, 0x08:前超界, 0x10:底盘超界, 0x20:超宽, 0x40:轴距超界, 0x80:车身角度偏左, 0x100:车身角度偏右, 0x200:方向盘未回正
+  required int32 ground_status = 10 [default = 1];     // 0:ok, 1:nothing, 2:noise, 3:超界
+  required int32 is_stop = 11 [default = 2];           // 是否可停1为可停，2为不可停
+}
+message measure_statu{
+  required measure_info info = 1;
+}
+
+enum Range_status {
+  Range_correct             = 0x0000; // 未超界
+  Range_back                = 0x0001; // 后超界
+  Range_left                = 0x0002; // 左超界
+  Range_right               = 0x0004; // 右超界
+  Range_front               = 0x0008; // 前超界
+  Range_bottom              = 0x0010; // 底盘超界
+  Range_car_width           = 0x0020; // 车宽超界
+  Range_car_wheelbase       = 0x0040; // 轴距超界
+  Range_angle_anti_clock    = 0x0080; // 左(逆时针)旋转超界
+  Range_angle_clock         = 0x0100; // 右(顺时针)旋转超界
+  Range_steering_wheel_nozero = 0x0200; // 方向盘未回正
+  Range_top                 = 0x0400; // 车顶超界
+  Range_car_moving          = 0x0800; // 车辆移动为1，静止为0
+};
+
+enum Ground_statu
+{
+  ok = 0;
+  Noise = 1;
+  Nothing = 2;
+  Car_border_reached = 3;
+}

Modules/AlgAddTest/etc/test.json

@@ -0,0 +1,6 @@
+{
+    "name": "etc",
+    "version": "1.0.0",
+    "dependencies": {
+    }
+}

Modules/AlgAddTest/fit_wheel_ret.cpp

@@ -0,0 +1,7 @@
+/**
+  * @project Project
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/6/16
+ **/
+#include "fit_wheel_ret.h"

Modules/AlgAddTest/fit_wheel_ret.h

@@ -0,0 +1,117 @@
+/**
+  * @project Project
+  * @brief   测试算法: 对算法优化结果进行优化,由于算法数据来源存在错误概率,该算法用于评估算法数据.
+  * @author  lz
+  * @data    2023/6/13
+ **/
+#pragma once
+
+#include "defines.hpp"
+#include "define.pb.h"
+#include "error_code/error_code.hpp"
+
+#include <deque>
+
+class TrajxtoryRegression {
+public:
+    /**
+      * @brief   更新全部的车辆轨迹数据, 更新后会自动根据当前轨迹队列进行回归.
+      * @param   deque 完整的轨迹数据, 更新后当前的轨迹数据就是这个输入的轨迹数据.
+      * @return  数据回归结果
+      */
+    Error_manager upTrajxtoryQueue(std::deque<measure_statu> &deque) {
+        m_trajxtory_deque = deque;
+
+        return fitTrajxtory();
+    }
+
+    /**
+      * @brief   添加一组的轨迹数据, 添加后会更新轨迹回归参数.
+      * @param   trqjxtory 轨迹数据
+      * @return  数据回归结果
+      */
+    Error_manager upTrajxtoryQueue(measure_statu &trqjxtory) {
+        m_trajxtory_deque.pop_front();
+        m_trajxtory_deque.emplace_back(trqjxtory);
+
+        return fitTrajxtory();
+    }
+
+    /**
+      * @brief   刷新轨迹数据, 再次进行回归, 默认情况下会释放一组轨迹数据
+      * @param   erase 是否释放队列第一个轨迹数据
+      * @return  数据回归结果
+      */
+    Error_manager upTrajxtoryQueue(bool erase = true) {
+        if (erase) {
+            m_trajxtory_deque.pop_front();
+        }
+
+        return fitTrajxtory();
+    }
+
+    /**
+      * @brief   计算输入轨迹数据相对于当前回归数据的损失值.
+      * @param   trqjxtory 轨迹数据
+      * @return  损失值
+      */
+    double lossTrajxtory(measure_statu &trqjxtory) {
+        // TODO
+        return -1;
+    }
+
+    /**
+      * @brief   获取目前存储的轨迹数据
+      * @param   deque 传出轨迹数据
+      * @return  NULL
+      */
+    Error_manager getTrajxtoryQueue(std::deque<measure_statu> &deque) {
+        deque = m_trajxtory_deque;
+
+        return Error_code::SUCCESS;
+    }
+
+    /**
+      * @brief   获取轨迹回归参数
+      * @param   TODO
+      * @return  NULL
+      */
+    Error_manager getRegressionParam() {
+        return Error_code::FAILED;
+    }
+
+
+
+
+private:
+    Error_manager fitTrajxtory() {
+        // TODO
+        pcl::PointCloud<pcl::PointXY>::Ptr clouds;
+        for (auto &tra: m_trajxtory_deque) {
+            float x = tra.info().cx();
+            float y = tra.info().cy();
+            float theta = tra.info().theta();
+            float l = tra.info().wheelbase();
+            pcl::PointXY point{};
+            point.x = (float)(x - l * sin((double)theta));
+            point.y = (float)(y - l * cos((double)theta));
+            clouds->emplace_back(point);
+        }
+
+
+
+        return Error_code::FAILED;
+    }
+
+private:
+    std::deque<measure_statu> m_trajxtory_deque;
+
+    // 回归参数 TODO
+    struct TrajxtoryParam {
+        pcl::PointXY location;
+        float v;
+        float omega;
+        std::chrono::steady_clock::time_point time_stamp;
+
+    };
+};

Modules/AlgWheelDetect/CMakeLists.txt

@@ -0,0 +1,30 @@
+if(TEST)
+    add_definitions(-DETC_PATH="${CMAKE_CURRENT_LIST_DIR}")
+endif ()
+
+# 添加文件
+set(target_name wheel_detect)
+
+aux_source_directory(. WheelData)
+
+# 生成可执行程序
+add_executable(${target_name} ${WheelData})
+
+# 添加链接静态库
+target_link_libraries(${target_name}
+        zx
+        ${CERES_LIBRARIES}
+        -lpthread
+        )
+
+# 安装
+install(TARGETS ${target_name}
+        LIBRARY DESTINATION lib  # 动态库安装路径
+        ARCHIVE DESTINATION lib  # 静态库安装路径
+        RUNTIME DESTINATION bin  # 可执行文件安装路径
+        PUBLIC_HEADER DESTINATION include  # 头文件安装路径
+        )
+
+install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/etc
+        DESTINATION ${CMAKE_INSTALL_PREFIX}
+        )

Modules/AlgWheelDetect/alg_manager.cpp

@@ -0,0 +1,166 @@
+/**
+  * @project Project
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/4/28
+ **/
+#include "alg_manager.h"
+
+bool VelodyneManager::run_measure_form(const std::string &path) {
+    // 初始化glog日志
+    initGlog();
+
+    // 初始化配置信息
+    if (!initMeasureConfig(path)) {
+        LOG(ERROR) << "Init measure config form " << path << " error!";
+        return false;
+    }
+
+    // 初始化mqtt配置
+    m_mqtt_client = new CloudDataMqttAsync;
+    m_mqtt_client->init();
+
+    // 初始化rabbitmq
+    m_detect_result_rabbitmq = new DetectResultRabbitmq;
+    Error_manager ret;
+    if ((ret = m_detect_result_rabbitmq->rabbitmq_init()) != SUCCESS) {
+        LOG(ERROR) << ret.to_string();
+        exit(EXIT_FAILURE);
+    }
+
+    // 根据配置信息启动所有入口的线程
+    for (auto &measure: m_measure_config->measures()) {
+        std::thread t(&VelodyneManager::thread_func, this, std::ref(measure), std::ref(*m_mqtt_client));
+        t.detach();
+    }
+
+    return true;
+}
+
+bool VelodyneManager::initGlog() {
+    FLAGS_colorlogtostderr = true;   // log信息区分颜色
+    FLAGS_logtostderr = true;           // 允许log信息打印到屏幕
+    google::SetStderrLogging(google::GLOG_INFO);  // 输出log的最低等级是 INFO (可以设置为WARNING或者更高)
+    google::InitGoogleLogging("");  // 用当前可执行程序初始化glog
+    return true;
+}
+
+bool VelodyneManager::initMeasureConfig(const std::string &path) {
+    m_measure_config = new MeasureConfig;
+    Json::Value config;
+    ReadJsonFile(path, config);
+    google::protobuf::util::Status ret = google::protobuf::util::JsonStringToMessage(config.toStyledString(),
+                                                                                     m_measure_config);
+    LOG(INFO) << m_measure_config->DebugString();
+    if (!ret.ok()) {
+        LOG(ERROR) << "init config from proto error";
+        return false;
+    }
+    return true;
+}
+
+void VelodyneManager::thread_func(const MeasureLidar &config, CloudDataMqttAsync &mqtt_client) {
+    DetectWheel detect;
+    detect.init(config.region());
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    LOG(INFO) << "------------------------------";
+    while (true) {
+        // 采集该雷达监听的雷达下的所有数据并合并在一个点云数据中
+
+#if NO_MQTT_GET_CLOUD
+        if (!getLocalPointCloud("../etc/data/cloud_cut.txt", cloud)) {
+            LOG(WARNING) << "get local point cloud error, please Re-check the ../etc/test_car.txt file's data again.";
+        }
+#else
+        getTopicsLidarData(config, cloud);
+#endif
+
+        // 对点云数据进行检测，获取车辆信息
+        switch (detect.detect(cloud).get_error_code()) {
+            case VELODYNE_REGION_EMPTY_CLOUD:
+                continue;
+            default:
+                break;
+        }
+
+        // 将车辆检测结果进行转换
+        measure_statu statu;
+        detect.upStatus(cloud);
+        packMeasureInfo(detect, statu);
+
+        if (statu.info().border_statu()) {
+            LOG(INFO) << statu.DebugString();
+        } else {
+            LOG(INFO) << "detect okk.";
+            LOG(INFO) << statu.DebugString();
+        }
+        // send mqtt message
+        mqtt_client.SendMessage(m_measure_config->sendtopics() + config.measuretopic(),
+                                (void *) statu.DebugString().c_str(),
+                                statu.DebugString().length());
+
+        // send rabbitmq message
+        m_detect_result_rabbitmq->encapsulate_msg(statu.DebugString(), atoi(config.measuretopic().c_str()), "statu_ex",
+                                                  "measure_" + config.measuretopic() + "_statu_port", 10000);
+    }
+}
+
+bool VelodyneManager::getTopicsLidarData(const MeasureLidar &config, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
+    for (auto &topic: config.lidartopic()) {
+        m_mqtt_client->getCloudData(m_measure_config->lidartopics() + topic, cloud);
+    }
+
+    if (cloud->empty()) {
+        LOG(WARNING) << "Can't get any cloud data.";
+        return false;
+    }
+
+    return true;
+}
+
+bool VelodyneManager::packMeasureInfo(DetectWheel &detect, measure_statu &measure) {
+    DetectResult result;
+    if (Error_code::FAILED == detect.getDetectResult(result).get_error_code()) {
+        return false;
+    }
+
+    // 直接参数
+    auto info = measure.mutable_info();
+    info->set_theta(result.theta());
+    info->set_length(0.0f);  // not need
+    info->set_width(result.width());
+    info->set_height(0.0f);  // not need
+    info->set_wheelbase(result.wheel_base());
+    info->set_front_theta(result.front_theta());
+
+    // TODO:中心点需要转换
+    info->set_cx(result.cx());
+    info->set_cy(result.cy());
+
+    info->set_ground_status(detect.groundStatus());  // 地面雷达状态
+    info->set_border_statu(detect.borderStatus());    // 超界状态
+
+    // is stop 暂未使用
+    if (info->border_statu() == 0) {
+        info->set_is_stop(1);    // 暂时未使用
+    } else {
+        info->set_is_stop(2);    // 暂时未使用
+    }
+
+    return true;
+}
+
+bool VelodyneManager::getLocalPointCloud(const std::string &path, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
+    cloud->clear();
+    ReadTxtCloud(path, cloud);
+    Eigen::Rotation2D<double> rotation(180 * M_PI / 180);
+    Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+    for (auto &pt: cloud->points) {
+        const Eigen::Matrix<double, 2, 1> point(double(pt.x), double(pt.y));
+        const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * (point);
+        pt.x = tanslate_point[0];
+        pt.y = tanslate_point[1];
+    }
+    return !cloud->empty();
+}

Modules/AlgWheelDetect/alg_manager.h

@@ -0,0 +1,73 @@
+/**
+  * @project ALG_WHeelData
+  * @brief   $BRIEF$
+  * @author  lz
+  * @data    2023/4/21
+ **/
+#pragma once
+
+#include <iostream>
+#include <thread>
+#include "json/json.h"
+#include "AlgWheelDetect/cloud_data.h"
+#include "AlgWheelDetect/detect_wheel.h"
+#include "define.pb.h"
+#include "rabbitmq.h"
+#include "message/measure_message.pb.h"
+#include "trajxtory_regression.hpp"
+
+#define NO_MQTT_GET_CLOUD 1
+
+class VelodyneManager {
+public:
+    /**
+      * @brief   根据指定的配置文件启动测量功能。
+      * @param   path 配置文件路径，json文件格式且符合proto配置的内容
+      */
+    bool run_measure_form(const std::string &path);
+
+private:
+    /**
+      * @brief   初始化glog日志系统
+      * @return  是否成功
+      */
+    bool initGlog();
+
+    /**
+      * @brief   初始化测量配置
+      * @注      配置初始化失败很有可能是配置文件错了，配置文件不能多出不相关字段
+      * @return  是否成功
+      */
+    bool initMeasureConfig(const std::string &path);
+
+    /**
+      * @brief   线程函数，每个入口一个线程，线程中会根据入口雷达数据进行算法检测，测出入口车辆信息
+      * @param   config 该线程对应的雷达信息的配置以及结果发送到哪个key（rabbitmq）或者topic（mqtt）
+      * @param   mqtt_client mqtt类，通过这个向mqtt发送测量结果;
+      */
+    void thread_func(const MeasureLidar &config, CloudDataMqttAsync &mqtt_client);
+
+    /**
+      * @brief   将监听的话题的雷达数据全部集合在一起
+      * @param   MeasureLidar 该线程的测量雷达相关配置
+      * @param   cloud 点云数据
+      * @return  NULL
+      */
+    bool getTopicsLidarData(const MeasureLidar &config, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    /**
+      * @brief   对检测结果进行对比分析，并将最终信息按照约定协议存储
+      * @param   NULL
+      * @return  NULL
+      */
+    bool packMeasureInfo(DetectWheel &detect, measure_statu &measure);
+
+
+    // 测试用代码，有时候数据需要旋转
+    bool getLocalPointCloud(const std::string &path, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+private:
+    MeasureConfig *m_measure_config;
+    CloudDataMqttAsync *m_mqtt_client;
+    DetectResultRabbitmq *m_detect_result_rabbitmq;
+};

Modules/AlgWheelDetect/car_pose_detector.cpp

@@ -0,0 +1,471 @@
+/*
+ * @Description: 
+ * @Author: yct
+ * @Date: 2021-09-16 15:18:34
+ * @LastEditTime: 2021-11-17 15:37:25
+ * @LastEditors: yct
+ */
+
+#include "car_pose_detector.h"
+
+// 变换点云, 反向平移后旋转
+void
+Car_pose_detector::inv_trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta) {
+    for (int i = 0; i < cloud_ptr->size(); i++) {
+        Eigen::Matrix<double, 2, 1> t_point(double(cloud_ptr->points[i].x) - x, double(cloud_ptr->points[i].y) - y);
+        Eigen::Rotation2D<double> rotation(theta);
+        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        Eigen::Matrix<double, 2, 1> trans_point = rotation_matrix * t_point;
+        cloud_ptr->points[i].x = trans_point.x();
+        cloud_ptr->points[i].y = trans_point.y();
+    }
+}
+
+// 变换点云,旋转后平移
+void Car_pose_detector::trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta) {
+    for (int i = 0; i < cloud_ptr->size(); i++) {
+        Eigen::Matrix<double, 2, 1> t_point(double(cloud_ptr->points[i].x), double(cloud_ptr->points[i].y));
+        Eigen::Rotation2D<double> rotation(theta);
+        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        Eigen::Matrix<double, 2, 1> trans_point = rotation_matrix * t_point;
+        cloud_ptr->points[i].x = trans_point.x() + x;
+        cloud_ptr->points[i].y = trans_point.y() + y;
+    }
+}
+
+// 创建两轴具体曲线
+void Car_pose_detector::create_curve_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double width) {
+    for (double x = -2.5; x < 2.5; x += 0.02) {
+        double left_value = 1.0 / (1.0 + exp(30 * (x + width / 2.0)));
+        double right_value = 1.0 / (1.0 + exp(30 * (-x + width / 2.0)));
+        double front_value = 1.0 / (1.0 + exp(15 * (x + 2.2)));
+        double back_value = 1.0 / (1.0 + exp(15 * (-x + 2.2)));
+        cloud_ptr->push_back(pcl::PointXYZ(x, std::max(left_value, right_value) - 3.0, 0.0));
+        cloud_ptr->push_back(pcl::PointXYZ(std::max(front_value, back_value) - 2.0, x, 0.0));
+    }
+}
+
+// 代价函数的计算模型
+struct Car_pose_cost {
+    Car_pose_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr) : m_cloud_ptr(cloud_ptr) {}
+
+    // 残差的计算
+    template<typename T>
+    bool operator()(
+            const T *const vars, // 模型参数，x y theta w
+            T *residual) const {
+        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0) {
+            std::cout << "error occured" << std::endl;
+            return false;
+        }
+        const T norm_scale = T(0.002);
+        //residual[0] = T(0);
+        // residual[1] = T(0);
+        // residual[m_cloud_ptr->size()] = T(0.0);
+        // 点云loss
+        const T width = T(2.0); //vars[3];
+
+        char buf[40960] = {0};
+        // sprintf(buf, "----");
+        for (int i = 0; i < m_cloud_ptr->size(); i++) {
+            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0],
+                                           T(m_cloud_ptr->points[i].y) - vars[1]);
+            Eigen::Rotation2D<T> rotation(vars[2]);
+            Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;
+
+            T left_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (t_trans_point.x() + width / T(2.0))));
+            T right_loss = T(1.0) / (T(1.0) + ceres::exp(T(30.0) * (-t_trans_point.x() + width / T(2.0))));
+            T front_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (t_trans_point.y() + T(2.2))));
+            T back_loss = T(1.0) / (T(1.0) + ceres::exp(T(15.0) * (-t_trans_point.y() + T(2.2))));
+            residual[i] = left_loss + right_loss + front_loss +
+                          back_loss; // + norm_scale * ((left_loss - T(0.5)) + (right_loss - T(0.5)));
+            // residual[m_cloud_ptr->size()] += (left_loss - T(0.5)) + (right_loss - T(0.5));
+            // if(left_loss > T(0.01))
+            //   std::cout << "index l r: " << i << ", " << left_loss << ", " << right_loss << ", " << trans_point.x() << std::endl;
+            // sprintf(buf + strlen(buf), "%.4f ", residual[i]);
+            // if(i%20==8)
+            // {
+            //     sprintf(buf + strlen(buf), "\n");
+            // }
+        }
+        // printf(buf);
+
+        // // 参数L2正则化loss
+        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
+        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
+        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
+        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
+        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
+        return true;
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
+};
+
+// 公式转图片优化
+#include <ceres/cubic_interpolation.h>
+
+constexpr float kMinProbability = 0.01f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+constexpr float resolutionx = 0.01f;
+constexpr float resolutiony = 0.01f;
+constexpr float min_x = -2.0f;
+constexpr float min_y = -3.0f;
+constexpr float max_x = 2.0f;
+constexpr float max_y = 3.0f;
+
+cv::Mat Car_pose_detector::m_model = Car_pose_detector::create_mat(min_x, max_x, min_y, max_y, resolutionx,
+                                                                   resolutiony);
+
+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<float>(row - kPadding, column - kPadding));
+            // if (row - kPadding > 100 && row - kPadding < 300 && column - kPadding > 50 && column - kPadding < 150)
+            //     printf("row:%d, col:%d, val%.3f\n", row-kPadding, column-kPadding, grid_.at<float>(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_;
+};
+
+// 代价函数的计算模型
+struct Trans_mat_cost {
+    Trans_mat_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat &mat) : m_cloud_ptr(cloud_ptr), m_mat(mat) {
+        // cv::namedWindow("img", CV_WINDOW_FREERATIO);
+        // cv::imshow("img", m_mat);
+        // cv::waitKey();
+        // debug_img(cloud_ptr, mat);
+    }
+
+    void debug_img(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, cv::Mat &mat) {
+        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0) {
+            std::cout << "error occured" << std::endl;
+            return;
+        }
+        cv::Mat img_show = mat.clone();//cv::Mat::zeros(mat.rows * 1, mat.cols * 1, CV_32FC1);
+        const GridArrayAdapter adapter(mat);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        // 点云loss
+        // char buf[150960]={0};
+        // sprintf(buf, "----");
+        for (int i = 0; i < m_cloud_ptr->size(); i++) {
+            Eigen::Matrix<double, 2, 1> t_point(double(m_cloud_ptr->points[i].x), double(m_cloud_ptr->points[i].y));
+            Eigen::Rotation2D<double> rotation(0);
+            Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+            Eigen::Matrix<double, 2, 1> t_trans_point = rotation_matrix * t_point;
+
+            int col_index = int((t_trans_point.x() - min_x) / resolutionx);
+            int row_index = int((t_trans_point.y() - min_y) / resolutiony);
+            double val = 0.0;
+            interpolator.Evaluate(row_index + 0.5 + kPadding, col_index + 0.5 + kPadding, &val);
+            cv::circle(img_show, cv::Point(col_index, row_index), 1, cv::Scalar(0.25), 1);
+            // sprintf(buf + strlen(buf), "(%.2f,%.2f)---%.3f ", t_trans_point.x(), t_trans_point.y(), val);
+            // if(i%15==8)
+            // {
+            //     sprintf(buf + strlen(buf), "\n");
+            // }
+        }
+        // printf(buf);
+        // for (size_t i = -mat.rows; i < mat.rows*2; i++)
+        // {
+        //     for (size_t j = -mat.cols; j < mat.cols*2; j++)
+        //     {
+        //         double val;
+        //         // adapter.GetValue(i + kPadding, j + kPadding, &val);
+        //         interpolator.Evaluate(i+kPadding, j+kPadding, &val);
+        //         img_show.at<float>(i, j) = val;
+        //     }
+        // }
+        // printf("r:%d c:%d\n", img_show.rows, img_show.cols);
+        cv::namedWindow("img", cv::WINDOW_FREERATIO);
+        // cv::imshow("origin", mat);
+        cv::imshow("img", img_show);
+        cv::waitKey();
+    }
+
+    // 残差的计算
+    template<typename T>
+    bool operator()(
+            const T *const vars, // 模型参数，x y theta w
+            T *residual) const {
+        if (m_cloud_ptr == nullptr || m_cloud_ptr->size() <= 0) {
+            std::cout << "error occured" << std::endl;
+            return false;
+        }
+        const GridArrayAdapter adapter(m_mat);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        // 点云loss
+        Eigen::Rotation2D<T> rotation(vars[2]);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+        for (int i = 0; i < m_cloud_ptr->size(); i++) {
+            Eigen::Matrix<T, 2, 1> t_point(T(m_cloud_ptr->points[i].x) - vars[0],
+                                           T(m_cloud_ptr->points[i].y) - vars[1]);
+            Eigen::Matrix<T, 2, 1> t_trans_point = rotation_matrix * t_point;
+
+            T col_index = (t_trans_point.x() - T(min_x)) / T(resolutionx) + T(0.5 + kPadding);
+            T row_index = (t_trans_point.y() - T(min_y)) / T(resolutiony) + T(0.5 + kPadding);
+            interpolator.Evaluate(row_index, col_index, &residual[i]);
+        }
+
+        // // 参数L2正则化loss
+        // residual[m_cloud_ptr->size()] = T(m_cloud_ptr->size()) * width * norm_scale;
+        // residual[1] += ceres::pow(vars[1],2) * norm_scale;
+        // residual[1] += ceres::pow(vars[2],2) * norm_scale;
+        // residual[1] += ceres::pow((vars[3]-T(1.8)),2) * norm_scale;
+        // ((i != 3) ? norm_scale : norm_scale * T(m_cloud_ptr->size()));
+        return true;
+    }
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud_ptr; // x,y数据
+    cv::Mat m_mat;
+};
+
+// 生成优化图像，降低优化耗时
+cv::Mat Car_pose_detector::create_mat(double min_x, double max_x, double min_y, double max_y, double resolution_x,
+                                      double resolution_y) {
+    if (max_x < min_x || max_y < min_y || resolution_x <= 0 || resolution_y <= 0) {
+        return cv::Mat();
+    }
+    int cols = (max_x - min_x) / resolution_x + 1;
+    int rows = (max_y - min_y) / resolution_y + 1;
+    if (rows <= 1 || cols <= 1) {
+        return cv::Mat();
+    }
+    cv::Mat t_mat(rows, cols, CV_32FC1);
+
+    for (size_t i = 0; i < rows; i++) {
+        for (size_t j = 0; j < cols; j++) {
+            double x = j * resolution_x + min_x;
+            double y = i * resolution_y + min_y;
+            double left_value = 1.0 / (1.0 + exp(30 * (x + 1.0)));
+            double right_value = 1.0 / (1.0 + exp(30 * (-x + 1.0)));
+            double front_value = 1.0 / (1.0 + exp(15 * (y + 2.2)));
+            double back_value = 1.0 / (1.0 + exp(15 * (-y + 2.2)));
+            t_mat.at<float>(i, j) = float((left_value + right_value + front_value + back_value) / 1.0f);
+        }
+    }
+    // std::cout << "r,c " << t_mat.rows << ", " << t_mat.cols << std::endl;
+    // cv::imshow("img", t_mat);
+    // cv::waitKey();
+
+    return t_mat;
+}
+
+// 检测底盘z方向值，去中心，使用mat加速
+bool Car_pose_detector::detect_pose_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr,
+                                        pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y,
+                                        double &theta, bool debug_cloud) {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+    t_cloud->operator+=(*cloud_ptr);
+    double t_vars[4] = {0, 0, 0, 1.9};
+    // 去中心化
+    pcl::PointXYZ t_center(0, 0, 0);
+    for (size_t i = 0; i < t_cloud->size(); i++) {
+        t_center.x += t_cloud->points[i].x;
+        t_center.y += t_cloud->points[i].y;
+    }
+    t_center.x /= t_cloud->size();
+    t_center.y /= t_cloud->size();
+    for (size_t i = 0; i < t_cloud->size(); i++) {
+        t_cloud->points[i].x -= t_center.x;
+        t_cloud->points[i].y -= t_center.y;
+    }
+    // // write_pointcloud(t_cloud, std::to_string(count)+"_ori_");
+
+    // 构建最小二乘问题
+    ceres::Problem problem;
+    Trans_mat_cost *tp_trans_mat_cost = new Trans_mat_cost(t_cloud, m_model);
+    // tp_trans_mat_cost->debug_img(t_cloud, m_model);
+    problem.AddResidualBlock( // 向问题中添加误差项
+            // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
+            new ceres::AutoDiffCostFunction<Trans_mat_cost, ceres::DYNAMIC, 3>(
+                    tp_trans_mat_cost, t_cloud->size()),
+            new ceres::HuberLoss(1.0), // 核函数，这里不使用，为空
+            t_vars   // 待估计参数
+    );
+
+    // 配置求解器
+    ceres::Solver::Options options;                            // 这里有很多配置项可以填
+    options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
+    options.minimizer_progress_to_stdout = false;              // 输出到cout
+    options.max_num_iterations = 100;
+
+    ceres::Solver::Summary summary; // 优化信息
+    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+    ceres::Solve(options, &problem, &summary); // 开始优化
+    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
+    // std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;
+
+    // 输出结果
+    // std::cout << summary.BriefReport() << std::endl;
+    // t_vars[3] -= 0.4;
+    // 保存结果，将去除的中心补上
+    x = t_vars[0] + t_center.x;
+    y = t_vars[1] + t_center.y;
+    theta = t_vars[2];
+    // printf("x:%.3f y:%.3f th:%.3f\n", x, y, theta);
+    if (fabs(theta) > 10 * M_PI / 180.0) {
+        //std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
+        return false;
+    }
+    return true;
+}
+
+// 检测底盘z方向值，原始点云去中心，并切除底盘z以上部分
+bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr,
+                                    pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr, double &x, double &y,
+                                    double &theta, double &width, double &z_value, bool debug_cloud) {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+    t_cloud->operator+=(*cloud_ptr);
+    double t_vars[4] = {0, 0, 0, 1.9};
+    // 去中心化
+    pcl::PointXYZ t_center(0, 0, 0);
+    for (size_t i = 0; i < t_cloud->size(); i++) {
+        // t_cloud->points[i].x /= 1000.0;
+        // t_cloud->points[i].y /= 1000.0;
+        // t_cloud->points[i].z /= 1000.0;
+        t_center.x += t_cloud->points[i].x;
+        t_center.y += t_cloud->points[i].y;
+        // t_center.z += t_cloud->points[i].z;
+    }
+    t_center.x /= t_cloud->size();
+    t_center.y /= t_cloud->size();
+    // t_center.z /= t_cloud->size();
+    for (size_t i = 0; i < t_cloud->size(); i++) {
+        t_cloud->points[i].x -= t_center.x;
+        t_cloud->points[i].y -= t_center.y;
+        // t_cloud->points[i].z -= t_center.z;
+    }
+    // // write_pointcloud(t_cloud, std::to_string(count)+"_ori_");
+
+    // 构建最小二乘问题
+    ceres::Problem problem;
+    problem.AddResidualBlock( // 向问题中添加误差项
+            // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
+            new ceres::AutoDiffCostFunction<Car_pose_cost, ceres::DYNAMIC, 3>(
+                    new Car_pose_cost(t_cloud), t_cloud->size()),
+            new ceres::HuberLoss(1.0), // 核函数，这里不使用，为空
+            t_vars   // 待估计参数
+    );
+
+    // 配置求解器
+    ceres::Solver::Options options;                            // 这里有很多配置项可以填
+    options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解
+    options.minimizer_progress_to_stdout = false;              // 输出到cout
+    options.max_num_iterations = 100;
+
+    ceres::Solver::Summary summary; // 优化信息
+    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
+    ceres::Solve(options, &problem, &summary); // 开始优化
+    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
+    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
+    std::cout << "solve time cost = " << time_used.count() << " seconds. " << std::endl;
+
+    // 输出结果
+    // std::cout << summary.BriefReport() << std::endl;
+    // t_vars[3] -= 0.4;
+    // 保存结果，将去除的中心补上
+    x = t_vars[0] + t_center.x;
+    y = t_vars[1] + t_center.y;
+    theta = t_vars[2];
+    // printf("x:%.3f y:%.3f th:%.3f\n", x, y, theta);
+    if (fabs(theta) > 10 * M_PI / 180.0) {
+        //std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
+        return false;
+    }
+
+    inv_trans_cloud(t_cloud, x, y, theta);
+    // viewer.showCloud(cloud_ptr);
+    // write_pointcloud(cloud_ptr, "uniform_");
+
+    // //离群点过滤
+    // pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+    // sor.setInputCloud(t_cloud);
+    // sor.setMeanK(15);            //K近邻搜索点个数
+    // sor.setStddevMulThresh(3.0); //标准差倍数
+    // sor.setNegative(false);      //保留未滤波点（内点）
+    // sor.filter(*t_cloud);      //保存滤波结果到cloud_filter
+
+    // 判断x方向边界，若不关于中心对称则错误
+    pcl::PointXYZ total_min_p, total_max_p;
+    pcl::getMinMax3D(*t_cloud, total_min_p, total_max_p);
+    double x_diff = fabs(total_max_p.x + total_min_p.x) / 2.0;
+    if (x_diff > 2.0) {
+        // std::cout << "left, right not mirroring----"<<x_diff << std::endl;
+    } else {
+        // std::cout << "x diff " << x_diff << std::endl;
+        width = total_max_p.x - total_min_p.x;
+        // 最大轮宽0.28再加上前轮极限旋转角35度
+        double wheel_width = 0.28 * (1 + sin(35 * M_PI / 180.0));
+        // std::cout << "car width: " << width << std::endl;
+        // 切出底盘点，找最低即为z值
+        pcl::PointCloud<pcl::PointXYZ>::Ptr inside_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+                new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PassThrough<pcl::PointXYZ> pass;
+        pass.setInputCloud(t_cloud);
+        pass.setFilterFieldName("x");
+        pass.setFilterLimits(-width / 2.0 + wheel_width, width / 2.0 - wheel_width);
+        pass.setFilterLimitsNegative(false);
+        pass.filter(*inside_cloud);
+        // 找最低为z值
+        pcl::PointXYZ min_p, max_p;
+        pcl::getMinMax3D(*inside_cloud, min_p, max_p);
+        z_value = min_p.z - 0.01;
+        // 根据z值切原始点云
+        pass.setInputCloud(t_cloud);
+        pass.setFilterFieldName("z");
+        pass.setFilterLimits(total_min_p.z, z_value);
+        pass.setFilterLimitsNegative(false);
+        pass.filter(*t_cloud);
+        // std::cout << "\n--------------------------- chassis z0: " << min_p.z << std::endl;
+
+        if (debug_cloud) {
+            create_curve_cloud(t_cloud, t_vars[3]);
+
+            for (size_t i = 0; i < 60; i++) {
+                t_cloud->push_back(pcl::PointXYZ(-t_vars[3] / 2.0, -3.0 + i * 0.1, 0.0));
+                t_cloud->push_back(pcl::PointXYZ(t_vars[3] / 2.0, -3.0 + i * 0.1, 0.0));
+                t_cloud->push_back(pcl::PointXYZ(-width / 2.0, -3.0 + i * 0.1, 0.0));
+                t_cloud->push_back(pcl::PointXYZ(width / 2.0, -3.0 + i * 0.1, 0.0));
+                t_cloud->push_back(pcl::PointXYZ(-width / 2.0 + wheel_width, -3.0 + i * 0.1, 0.0));
+                t_cloud->push_back(pcl::PointXYZ(width / 2.0 - wheel_width, -3.0 + i * 0.1, 0.0));
+            }
+            out_cloud_ptr->clear();
+            out_cloud_ptr->operator+=(*t_cloud);
+        } else {
+            out_cloud_ptr->clear();
+            out_cloud_ptr->operator+=(*t_cloud);
+        }
+    }
+    // std::cout << "estimated x,y,theta = " << x << ", " << y << ", " << theta*180.0/M_PI << std::endl;
+    // std::cout << "----------------------------------" << std::endl;
+    return true;
+}

Modules/AlgWheelDetect/car_pose_detector.h

@@ -0,0 +1,64 @@
+/*
+ * @Description: 底盘z高度检测类,准确识别出平移,旋转,车宽,底盘高度，供3D车轮算法优化出车辆准确信息
+ * @Author: yct
+ * @Date: 2021-09-16 15:02:49
+ * @LastEditTime: 2021-11-16 15:45:02
+ * @LastEditors: yct
+ */
+
+#ifndef Z_DETECTOR_HH
+#define Z_DETECTOR_HH
+
+#include <iostream>
+#include <chrono>
+#include <ceres/ceres.h>
+#include <pcl/common/common.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+
+#include <opencv2/opencv.hpp>
+
+#include "tool/singleton.h"
+
+class Car_pose_detector : public Singleton<Car_pose_detector> {
+    friend class Singleton<Car_pose_detector>;
+
+public:
+    Car_pose_detector(const Car_pose_detector &) = delete;
+
+    Car_pose_detector &operator=(const Car_pose_detector &) = delete;
+
+    ~Car_pose_detector() = default;
+
+    // 变换点云
+    void inv_trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta);
+
+    // 变换点云
+    void trans_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double x, double y, double theta);
+
+    // 创建两轴具体曲线
+    void create_curve_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_ptr, double width);
+
+    // 检测底盘z方向值，去中心，
+    bool detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+                     double &x, double &y, double &theta, double &width, double &z_value, bool debug_cloud = false);
+
+    // 检测底盘z方向值，去中心，使用mat加速
+    bool
+    detect_pose_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr out_cloud_ptr,
+                    double &x, double &y, double &theta, bool debug_cloud = false);
+
+    // 生成优化图像，降低优化耗时
+    static cv::Mat
+    create_mat(double min_x, double max_x, double min_y, double max_y, double resolution_x, double resolution_y);
+
+private:
+    // 父类的构造函数必须保护，子类的构造函数必须私有。
+    Car_pose_detector() = default;
+
+    static cv::Mat m_model;
+};
+
+#endif // !Z_DETECTOR_HH

Modules/AlgWheelDetect/chassis_ceres_solver.cpp

@@ -0,0 +1,288 @@
+//
+// Created by zx on 2021/9/17.
+//
+
+#include "chassis_ceres_solver.h"
+
+const float CERES_PA = (100.);
+const float CERES_PB = (300.);
+const float CERES_PC = (300.);
+
+class chassis_ceres_cost {
+private:
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;     //点云
+
+public:
+    chassis_ceres_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
+            : m_cloud(cloud) {
+    }
+
+    template<typename T>
+    bool operator()(const T *const variable, T *residual) const {
+        T cx = variable[0];
+        T cy = variable[1];
+        T w = variable[2];
+        T h = variable[3];
+
+        const T PA = T(CERES_PA);
+        const T PB = T(CERES_PB);
+        const T PC = T(CERES_PC);
+        //
+        for (int i = 0; i < m_cloud->size(); ++i) {
+            //先平移后旋转
+            const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x)),
+                                               (T(m_cloud->points[i].z)));
+            T kx = point(0, 0);
+            T ky = point(1, 0);
+            T fx = 1.0 / (1.0 + ceres::exp(-PA * (kx - cx + w / 2.0))) -
+                   1.0 / (1.0 + ceres::exp(-PA * (kx - cx - w / 2.0)));
+            T fy = 1.0 / (1.0 + ceres::exp(-PB * (ky - cy + h / 2.0))) -
+                   1.0 / (1.0 + ceres::exp(-PC * ((ky - cy) - h / 2.0)));
+
+            residual[i] = fx * fy; //+(50./m_cloud->size())/(w*h+1e-16);
+        }
+        residual[m_cloud->size()] = T(1.0) / (w * h + 1e-16);
+        return true;
+    }
+
+};
+
+// 公式转图片优化
+#include <ceres/cubic_interpolation.h>
+
+constexpr float kMinProbability = 0.01f;
+constexpr float kMaxProbability = 1.f - kMinProbability;
+constexpr float kMaxCorrespondenceCost = 1.f - kMinProbability;
+constexpr int kPadding = INT_MAX / 4;
+constexpr float resolutionx = 0.01;
+constexpr float resolutiony = 0.005;
+constexpr float inner_width = 1.2;
+constexpr float chassis_height = 0.14;
+constexpr float startx = -2.0;
+constexpr float starty = -0.1;
+constexpr float endx = 2.0;
+constexpr float endy = 0.3;
+
+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<float>(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 chassis_mat_cost {
+private:
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;     //点云
+    cv::Mat m_mat;
+
+public:
+    chassis_mat_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Mat mat)
+            : m_cloud(cloud), m_mat(mat) {
+    }
+
+    template<typename T>
+    bool operator()(const T *const variable, T *residual) const {
+        T cx = variable[0];
+        T cy = variable[1];
+
+        const GridArrayAdapter adapter(m_mat);
+        ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+
+        for (int i = 0; i < m_cloud->size(); ++i) {
+            //先平移后旋转
+            const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x) - cx),
+                                               (T(m_cloud->points[i].z) - cy));
+            T kx = (point(0, 0) - T(startx)) / T(resolutionx) + T(0.5 + kPadding);
+            T ky = (point(1, 0) - T(starty)) / T(resolutiony) + T(0.5 + kPadding);
+            interpolator.Evaluate(kx, ky, &residual[i]);
+        }
+
+        return true;
+    }
+
+    // 生成debug图片
+    void generate_mat(cv::Mat &mat, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double *variables) {
+        mat = m_mat.clone();
+        // double w_ratio = variables[2] / inner_width;
+        // double h_ratio = variables[3] / chassis_height;
+        for (size_t i = 0; i < cloud->size(); i++) {
+            const Eigen::Vector2d point(
+                    ((cloud->points[i].x - variables[0]) - startx) / resolutionx,
+                    ((cloud->points[i].z - variables[1]) - starty) / resolutiony);
+            cv::circle(mat, cv::Point2d(point.y(), point.x()), 2, cv::Scalar(0.4), 1);
+            // std::cout << point.y() << ", " << point.x() << std::endl;
+        }
+    }
+
+};
+
+cv::Mat chassis_ceres_solver::m_model = chassis_ceres_solver::create_mat();
+
+chassis_ceres_solver::chassis_ceres_solver() {}
+
+chassis_ceres_solver::~chassis_ceres_solver() {}
+
+
+/**
+ * @description: 使用sigmoid函数寻找xoz平面平移以及内宽、底盘高度
+ */
+Error_manager
+chassis_ceres_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &y, double &w, double &h) {
+    double variable[] = {x, y, w, h};
+    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    chassis_ceres_cost *cost_func = new chassis_ceres_cost(cloud);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction *cost_function = new
+            ceres::AutoDiffCostFunction<chassis_ceres_cost, ceres::DYNAMIC, 4>(
+            cost_func, cloud->size() + 1);
+    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.use_nonmonotonic_steps = false;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    options.max_num_iterations = 500;
+    options.num_threads = 1;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    x = variable[0];
+    y = variable[1];
+    w = variable[2];
+    h = variable[3];
+    return SUCCESS;
+}
+
+/**
+ * @description: 图片代替函数十倍提升优化速度
+ */
+Error_manager
+chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &y, double &w, double &h,
+                                bool update_debug_img) {
+    bool logTime = true;
+    std::chrono::steady_clock::time_point t = std::chrono::steady_clock::now();
+    std::chrono::duration<double> diff;
+
+    double variable[] = {x, y, w, h};
+    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    chassis_mat_cost *cost_func = new chassis_mat_cost(cloud, m_model);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction *cost_function = new
+            ceres::AutoDiffCostFunction<chassis_mat_cost, ceres::DYNAMIC, 2>(
+            cost_func, cloud->size());
+    problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1.0), variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+    if (logTime) {
+        diff = std::chrono::steady_clock::now() - t;
+        LOG(INFO) << "cost time : " << diff.count() * 1000 << "ms.";
+    }
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.use_nonmonotonic_steps = false;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    //options.logging_type = ceres::LoggingType::SILENT;
+    options.max_num_iterations = 500;
+    options.num_threads = 1;
+    options.minimizer_progress_to_stdout = true;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    if (logTime) {
+        diff = std::chrono::steady_clock::now() - t;
+        LOG(INFO) << "cost time : " << diff.count() * 1000 << "ms.";
+    }
+    DLOG(INFO) << "after solve: " << x << " " << y << " " << w << " " << h;
+    DLOG(INFO) << "after solve: " << variable[0] << " " << variable[1] << " " << variable[2] << " " << variable[3];
+
+    x = variable[0];
+    y = variable[1];
+
+    if (update_debug_img) {
+        // printf("midx:%.3f, midz:%.3f\n", x, y);
+        cost_func->generate_mat(m_projected_mat, cloud, variable);
+        cv::namedWindow("win", cv::WINDOW_FREERATIO);
+        cv::imshow("win", m_projected_mat);
+        cv::waitKey();
+    }
+
+    return SUCCESS;
+}
+
+cv::Mat chassis_ceres_solver::create_mat() {
+    int rows = (endx - startx) / resolutionx + 1;
+    int cols = (endy - starty) / resolutiony + 1;
+    if (rows <= 1 || cols <= 1)
+        return cv::Mat();
+    cv::Mat t_mat(rows, cols, CV_32FC1);
+
+    for (size_t i = 0; i < rows; i++) {
+        for (size_t j = 0; j < cols; j++) {
+            double x = i * resolutionx + startx;
+            double y = j * resolutiony + starty;
+            double fx = 1.0 / (1.0 + std::exp(-CERES_PA * (x + inner_width / 2.0))) -
+                        1.0 / (1.0 + std::exp(-CERES_PA * (x - inner_width / 2.0)));
+            double fy = 1.0 / (1.0 + std::exp(-CERES_PB * (y + chassis_height / 2.0))) -
+                        1.0 / (1.0 + std::exp(-CERES_PC * (y - chassis_height / 2.0)));
+            t_mat.at<float>(i, j) = MAX(fx, fy);
+            // std::cout << "x,y: " << x << ", " << y << "-----value: " << 1.0 / (1.0 + std::exp(-CERES_PA * (x + default_width / 2.0)))<<", "<<
+            //   1.0 / (1.0 + std::exp(-CERES_PA * (x - default_width / 2.0))) << std::endl;
+            // if(fx*fy>0)
+            //     std::cout << "x,y: " << x << "," << y << ", fx,fy: " << fx << ", " << fy << ", " << fx * fy << std::endl;
+        }
+    }
+    cv::normalize(t_mat, t_mat, 1.0, 0.1, cv::NORM_MINMAX);
+    // std::cout << t_mat << std::endl;
+
+    // cv::namedWindow("win", cv::WINDOW_FREERATIO);
+    // cv::imshow("win", t_mat);
+    // cv::waitKey(0);
+
+    return t_mat;
+}
+
+
+// 返回投影地图转换的点云, 便于投影到pcl视野中
+pcl::PointCloud<pcl::PointXYZ>::Ptr chassis_ceres_solver::get_projected_cloud() {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+    if (m_projected_mat.empty())
+        return t_cloud;
+    for (size_t i = 0; i < m_projected_mat.rows; i++) {
+        for (size_t j = 0; j < m_projected_mat.cols; j++) {
+            double x0 = (i * resolutionx + startx);
+            t_cloud->push_back(pcl::PointXYZ());
+        }
+    }
+    return t_cloud;
+}

Modules/AlgWheelDetect/chassis_ceres_solver.h

@@ -0,0 +1,46 @@
+//
+// Created by zx on 2021/9/17.
+//
+
+#ifndef FIND_CHASSIS__CHASSIS_CERES_SOLVER_H_
+#define FIND_CHASSIS__CHASSIS_CERES_SOLVER_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 "error_code/error_code.hpp"
+
+class chassis_ceres_solver {
+public:
+    chassis_ceres_solver();
+
+    ~chassis_ceres_solver();
+
+    Error_manager solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &z, double &w, double &h);
+
+    // 利用图像优化，计算速度提升
+    Error_manager solve_mat(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double &x, double &z, double &w, double &h,
+                            bool update_debug_img = false);
+
+    // 创建优化地图
+    static cv::Mat create_mat();
+
+    // 返回更新投影地图
+    cv::Mat get_projected_mat() { return m_projected_mat; }
+
+    // 返回投影地图转换的点云, 便于投影到pcl视野中
+    pcl::PointCloud<pcl::PointXYZ>::Ptr get_projected_cloud();
+
+private:
+    static cv::Mat m_model;
+    cv::Mat m_projected_mat;
+};
+
+#endif //FIND_CHASSIS__CHASSIS_CERES_SOLVER_H_

Modules/AlgWheelDetect/cloud_data.cpp

@@ -0,0 +1,107 @@
+/**
+  * @project ALG_WHeelData
+  * @brief   从mqtt服务器获取点云数据并进行处理，仅给外部一个获取数据的接口。
+  * @author  lz
+  * @data    2023/4/21
+ **/
+#include "cloud_data.h"
+
+void CloudDataMqttAsync::init() {
+    m_message_arrived_callback_ = CloudDataArrived;
+
+    MqttAsyncClient::init_from_proto("../etc/alg_mqtt_async.json");
+}
+
+int CloudDataMqttAsync::CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen,
+                                         MQTTAsync_message *message) {
+    std::string topic = topicName;
+//    DLOG(INFO) << "received message from topic : " << topic << ".";
+
+    auto *m_client = dynamic_cast<CloudDataMqttAsync *>(client);
+    m_client->insertCloudData(topic, (unsigned char *) message->payload, message->payloadlen);
+
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        DLOG(WARNING) << "find topic " << topic << " in m_cloud_map error.";
+        return false;
+    }
+
+    for (auto &point: iter->second->points) {
+        cloud->emplace_back(point.x, point.y, point.z);
+    }
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        LOG(WARNING) << "find topic " << topic << " in m_cloud_map error.";
+        return false;
+    }
+
+    pcl::PointXYZI pt;
+    for (auto &point: iter->second->points) {
+        pt.x = point.x;
+        pt.y = point.y;
+        pt.z = point.z;
+        pt.intensity = point.intensity;
+        cloud->emplace_back(pt);
+    }
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointRPRY>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        DLOG(WARNING) << "find topic " << topic << " in m_cloud_map error.";
+        return false;
+    }
+
+    pcl::PointRPRY pt;
+    for (auto &point: iter->second->points) {
+        pt.pitch = point.pitch;
+        pt.yaw = point.yaw;
+        pt.roll = point.roll;
+        pt.r = point.r;
+        cloud->emplace_back(pt);
+    }
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        LOG(WARNING) << "find topic " << topic << " in m_cloud_map error.";
+        return false;
+    }
+
+    cloud->insert(cloud->end(), iter->second->points.begin(), iter->second->points.end());
+    return true;
+}
+
+bool CloudDataMqttAsync::insertCloudData(std::string &topic, unsigned char *cloud_data, int size) {
+    pcl::PointCloud<pcl::PointALL>::Ptr clouds(new pcl::PointCloud<pcl::PointALL>);
+
+    // 解压数据，获得点云
+    m_unpacket->unpacket((unsigned char *) cloud_data, size, clouds);
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter_full_cloud = m_full_clouds_map.find(topic);
+    if (iter_full_cloud == m_full_clouds_map.end()) {
+        m_full_clouds_map.insert(
+                std::pair<std::string, pcl::PointCloud<pcl::PointALL>::Ptr>(topic, new pcl::PointCloud<pcl::PointALL>));
+        iter_full_cloud = m_full_clouds_map.find(topic);
+    }
+
+    iter_full_cloud->second->swap(*clouds);
+
+    return true;
+}
+

Modules/AlgWheelDetect/cloud_data.h

@@ -0,0 +1,44 @@
+/**
+  * @project ALG_WHeelData
+  * @brief   从mqtt服务器获取点云数据并进行处理，仅给外部一个获取数据的接口。
+  * @author  lz
+  * @data    2023/4/21
+ **/
+#pragma once
+
+#include "pahoc/mqtt_async.h"
+#include "message/unpacket.hpp"
+#include "defines.hpp"
+#include <vector>
+#include <memory>
+#include <typeinfo>
+
+// TODO: 完善功能
+class CloudDataMqttAsync : public MqttAsyncClient {
+public:
+    CloudDataMqttAsync() = default;
+
+    ~CloudDataMqttAsync() = default;
+
+    void init();
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointRPRY>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud);
+
+private:
+    static int CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen, MQTTAsync_message *message);
+
+    bool insertCloudData(std::string &topic, unsigned char *cloud_data, int size);
+
+private:
+    CloudPointUnpacket *m_unpacket;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr m_clouds;
+
+    std::mutex m_cloud_mutex;
+    std::map<std::string, pcl::PointCloud<pcl::PointALL>::Ptr> m_full_clouds_map;
+};

Modules/AlgWheelDetect/define.proto

@@ -0,0 +1,111 @@
+syntax = "proto2";
+
+// 算法测量结果内容
+message LossResult {
+  required float lf_loss = 1;
+  required float rf_loss = 2;
+  required float lb_loss = 3;
+  required float rb_loss = 4;
+  required float total_avg_loss = 5;
+};
+message DetectResult {
+  required float cx = 1;
+  required float cy = 2;
+  required float theta = 3;
+  required float wheel_base = 4;
+  required float width = 5;
+  required float body_width = 6;
+  required float front_theta = 7;
+  required float single_wheel_width = 8;
+  required float single_wheel_length = 9;
+  required bool success = 10;
+  required LossResult loss = 11;
+};
+
+// 区域配置
+message RegionEtc {
+  required double maxx = 1;
+  required double minx = 2;
+  required double maxy = 3;
+  required double miny = 4;
+  required double maxz = 5;
+  required double minz = 6;
+};
+
+message Region
+{
+  required RegionEtc cut_region = 1;
+  required float turnplate_cx = 2;
+  required float turnplate_cy = 3;
+  required float border_minx = 4; // 最小边界x，左超界提示
+  required float border_maxx = 5; // 最大边界x，右超界提示
+  required float plc_offsetx = 6; // plc偏移x
+  required float plc_offsety = 7; // plc偏移y
+  required float plc_offset_degree = 8; // plc偏移角度
+  required float plc_border_miny = 9;// plc后夹持y方向极限值
+  required float plc_border_maxy = 10;// plc后夹持y方向极限值
+  required float car_min_width = 11; // 最小车宽
+  required float car_max_width = 12; // 最大车宽
+  required float car_min_wheelbase = 13; // 最小轴距
+  required float car_max_wheelbase = 14; // 最大轴距
+  required float turnplate_angle_min = 15;
+  required float turnplate_angle_max = 16;
+  required float turnplate_angle_limit_anti_clockwise = 17; // 转盘逆时针角度极限
+  required float turnplate_angle_limit_clockwise = 18; // 转盘顺时针角度极限
+}
+
+// 测量节点与雷达关系, 一个测量节点可以配置多个雷达
+message MeasureLidar {
+  required  string measureTopic = 1;          // 测量节点将测量结果发送的话题
+  required  Region region = 2;             // 测量节点裁剪雷达数据的配置
+  repeated string lidarTopic = 5;   // 测量节点雷达数据来源话题
+}
+
+// 测量节点有关配置
+message MeasureConfig {
+  required string sendTopics = 1;                // 测量节点发送的所有消息都在这个话题下
+  required string lidarTopics = 2;               // 雷达的监听节点
+  repeated MeasureLidar measures = 5;   // 测量节点所有的雷达关系
+}
+
+// 测量节点状态
+message measure_info {
+  required float cx = 1 [default = 0];                 // 车身中心x坐标
+  required float cy = 2 [default = 0];                 // 车身中心y坐标
+  required float theta = 3 [default = 0];              // 车身偏转角
+  required float length = 4 [default = 0];             // 车长
+  required float width = 5 [default = 0];              // 车宽
+  required float height = 6 [default = 0];             // 车高
+  required float wheelbase = 7 [default = 0];          // 轴距
+  required float front_theta = 8 [default = 0];        // 前轮角
+  required int32 border_statu = 9 [default = 0];       // 超界信息: 0x01:后超界, 0x02:左超界, 0x04:右超界, 0x08:前超界, 0x10:底盘超界, 0x20:超宽, 0x40:轴距超界, 0x80:车身角度偏左, 0x100:车身角度偏右, 0x200:方向盘未回正
+  required int32 ground_status = 10 [default = 1];     // 0:ok, 1:nothing, 2:noise, 3:超界
+  required int32 is_stop = 11 [default = 2];           // 是否可停1为可停，2为不可停
+}
+message measure_statu{
+  required measure_info info = 1;
+}
+
+enum Range_status {
+  Range_correct             = 0x0000; // 未超界
+  Range_back                = 0x0001; // 后超界
+  Range_left                = 0x0002; // 左超界
+  Range_right               = 0x0004; // 右超界
+  Range_front               = 0x0008; // 前超界
+  Range_bottom              = 0x0010; // 底盘超界
+  Range_car_width           = 0x0020; // 车宽超界
+  Range_car_wheelbase       = 0x0040; // 轴距超界
+  Range_angle_anti_clock    = 0x0080; // 左(逆时针)旋转超界
+  Range_angle_clock         = 0x0100; // 右(顺时针)旋转超界
+  Range_steering_wheel_nozero = 0x0200; // 方向盘未回正
+  Range_top                 = 0x0400; // 车顶超界
+  Range_car_moving          = 0x0800; // 车辆移动为1，静止为0
+};
+
+enum Ground_statu
+{
+  ok = 0;
+  Noise = 1;
+  Nothing = 2;
+  Car_border_reached = 3;
+}

Modules/AlgWheelDetect/detect_wheel.cpp

@@ -0,0 +1,668 @@
+/**
+  * @project ALG_WHeelData
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/4/21
+ **/
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/common/transforms.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include "detect_wheel.h"
+
+DetectWheel::DetectWheel() {
+    mp_cloud_filtered = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    mp_cloud_detect_z = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    m_detector = new WheelCeres3D();
+}
+
+Error_manager DetectWheel::init(const Region &region) {
+    std::map<std::string, float> m_modules;
+    m_modules.insert(std::pair<std::string, float>("../etc/model/left_model.txt", 1.0f));
+    m_modules.insert(std::pair<std::string, float>("../etc/model/right_model.txt", 1.0f));
+    m_detector->init(m_modules);
+    m_region = region;
+    LOG(INFO) << "\n" << m_region.DebugString();
+    return Error_code::SUCCESS;
+}
+
+Error_manager DetectWheel::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
+    auto t = std::chrono::steady_clock::now();
+    std::chrono::duration<double> diff{};
+
+    // 1.*********点云合法性检验*********
+    if (cloud->empty()) {
+        return Error_code::VELODYNE_REGION_EMPTY_CLOUD;
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud.txt", cloud);
+#endif
+
+    // 2.*********点云预处理*********
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cut(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clean(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
+
+    // 过滤大部分无效点
+    if (!cutFilter(cloud, cloud_cut)) {
+        LOG(WARNING) << "cloud_cut cloud falture.";
+        return Error_code::FAILED;
+    }
+
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud_cut.txt", cloud_cut);
+#endif
+
+    // 下采样
+    if (!voxeFilter(cloud_cut, cloud_clean, pcl::PointXYZ(0.02, 0.02, 0.02))) {
+        LOG(WARNING) << "cloud_clean cloud falture.";
+        return Error_code::FAILED;
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud_clean.txt", cloud_clean);
+#endif
+
+    // 离群点过滤
+    if (!outlierFilter(cloud_clean, cloud_filtered)) {
+        LOG(WARNING) << "cloud_filtered cloud falture.";
+        return Error_code::FAILED;
+    }
+    if (cloud_filtered->size() <= 10) {
+        LOG(WARNING) << "cloud_filtered cloud number too small.";
+        return Error_code::FAILED;
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud_filtered.txt", cloud_filtered);
+#endif
+
+    // 更新过滤点
+    mp_cloud_filtered->clear();
+    mp_cloud_filtered->operator+=(*cloud_filtered);
+
+    // 3.*********位姿优化，获取中心xy与角度*********
+    double car_pose_x, car_pose_y, car_pose_theta;
+    if (!Car_pose_detector::get_instance_references().detect_pose_mat(cloud_filtered, cloud_detect_z, car_pose_x,
+                                                                      car_pose_y, car_pose_theta, false)) {
+        LOG(WARNING) << "detect pose mat falture.";
+        return Error_code::FAILED;
+    }
+
+    if (fabs(car_pose_theta * 180.0 / M_PI) > 8) {
+        LOG(WARNING) << "Pose theta > 8, update detect result and return false.";
+        m_detect_result.set_cx((float) car_pose_x);
+        m_detect_result.set_cy((float) car_pose_y);
+        m_detect_result.set_theta((float) (-car_pose_theta * 180.0f / M_PI));
+        m_t = std::chrono::steady_clock::now();
+        return Error_code::FAILED;
+    }
+
+    // 4.*********xoz优化获取底盘高度*********
+    // 重新取包含地面点的点云,用于底盘优化
+    double z_solver_x = car_pose_x;
+    double z_solver_y = car_pose_y;
+    double z_solver_theta = car_pose_theta;
+    chassis_ceres_solver t_solver;
+    // ！！！重新筛选地面点并滤波，融入第一步位姿使用的滤波后点云，传入第二步获取底盘高度
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_z_solver = cloud_cut;
+
+    //离群点过滤
+    if (!outlierFilter(cloud_z_solver, cloud_z_solver)) {
+        LOG(WARNING) << "outlierFilter falture.";
+        return Error_code::FAILED;
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud_z_solver.txt", cloud_z_solver);
+#endif
+
+    cloud_z_solver->operator+=(*mp_cloud_filtered);
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud_z_solver2.txt", cloud_z_solver);
+#endif
+
+    // 去中心，角度调正
+    Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, z_solver_x, z_solver_y,
+                                                                 z_solver_theta);
+    if (cloud_z_solver->empty()) {
+        LOG(WARNING) << "inv trans cloud falture.";
+        return Error_code::FAILED;
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud_z_solver3.txt", cloud_z_solver);
+#endif
+
+    double mid_z = 0.24 + m_region.cut_region().minz();
+    pcl::PassThrough<pcl::PointXYZ> pass;
+    pass.setInputCloud(cloud_z_solver);
+    pass.setFilterFieldName("z");
+    pass.setFilterLimits((float) m_region.cut_region().minz(), (float) mid_z);
+    pass.setFilterLimitsNegative(false);
+    pass.filter(*cloud_z_solver);
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/cloud_z_solver4.txt", cloud_z_solver);
+#endif
+
+    mp_cloud_detect_z->clear();
+    mp_cloud_detect_z->operator+=(*cloud_filtered);
+
+    std::vector<DetectResult> results;
+    DetectResult result;
+    double chassis_z = mid_z;
+    if (chassis_z > m_region.cut_region().maxz() || chassis_z < m_region.cut_region().minz()) {
+        LOG(WARNING) << "chassis_z valure error.";
+        return Error_code::FAILED;
+    }
+    bool ret;
+
+    while (chassis_z > m_region.cut_region().minz()) {
+        // 初值中x使用第一步优化的值
+        result.set_cx((float) car_pose_x);
+        // 传入整车角度，用于准确判断轮宽，此处需要的是点云的角度，通常逆时针90度到x轴
+        result.set_theta((float) (car_pose_theta - M_PI_2));
+        ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
+        // changed by yct, 根据底盘z识别，失败则向下移动直至成功或超过底盘内空中点
+        if (ret) {
+            results.push_back(result);
+            break;
+        } else {
+            chassis_z -= 0.02;
+        }
+    }
+    if (results.empty()) {
+        LOG(WARNING) << "results is empty.";
+        return Error_code::FAILED;
+    }
+
+    ///  to be
+    float min_mean_loss = 1.0;
+    for (int i = 0; i < results.size(); ++i) {
+        DetectResult result_loss = results[i];
+        std::vector<double> loss;
+        loss.push_back(result_loss.loss().lf_loss());
+        loss.push_back(result_loss.loss().rf_loss());
+        loss.push_back(result_loss.loss().lb_loss());
+        loss.push_back(result_loss.loss().rb_loss());
+        double mean =
+                (result_loss.loss().lf_loss() + result.loss().rf_loss() + result.loss().lb_loss() +
+                 result.loss().rb_loss()) / 4.0;
+        double var = -1.;
+        computer_var(loss, var);
+        if (mean < min_mean_loss) {
+            m_detect_result = result_loss;
+            min_mean_loss = (float) mean;
+            m_t = std::chrono::steady_clock::now();
+        }
+    }
+
+    // 角度校验，理论上car pose检测使用车身点云，应与车轮点云识别得到的车身 x偏移(0.035)与角度(<1°)一致
+    double car_pose_theta_deg = 90 - car_pose_theta * 180.0 / M_PI;
+    if (fabs(car_pose_x - m_detect_result.cx()) > 0.035 || fabs(car_pose_theta_deg - m_detect_result.theta()) > 1) {
+        char valid_info[255];
+        sprintf(valid_info, "validation failed for x and theta, carpose: (%.3f, %.3f, result: (%.3f, %.3f)",
+                car_pose_x, car_pose_theta_deg, m_detect_result.cx(), m_detect_result.theta());
+        LOG(ERROR) << valid_info;
+        return Error_code::FAILED;
+    }
+
+    // 车宽精度优化
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_rough_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+                new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_width_extract_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+                new pcl::PointCloud<pcl::PointXYZ>);
+        t_width_rough_cloud->operator+=(*mp_cloud_filtered);
+        t_width_extract_cloud->operator+=(m_detector->m_left_front_cloud);
+        t_width_extract_cloud->operator+=(m_detector->m_right_front_cloud);
+        t_width_extract_cloud->operator+=(m_detector->m_left_rear_cloud);
+        t_width_extract_cloud->operator+=(m_detector->m_right_rear_cloud);
+
+        pcl::PointXYZ t_rough_min_p, t_rough_max_p;
+        //add by zzw
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_width_upwheel(new pcl::PointCloud<pcl::PointXYZ>);
+        for (int i = 0; i < t_width_rough_cloud->size(); ++i) {
+            pcl::PointXYZ pt = t_width_rough_cloud->points[i];
+            if (pt.z > chassis_z) {
+                cloud_for_width_upwheel->push_back(pt);
+            }
+        }
+
+        // rough width for human detect
+        {
+            //离群点过滤
+            if (!outlierFilter(cloud_for_width_upwheel, cloud_for_width_upwheel)) {
+                LOG(WARNING) << "outlierFilter error.";
+                return Error_code::FAILED;
+            }
+
+            Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
+            t_affine.prerotate(
+                    Eigen::AngleAxisd((90 - m_detect_result.theta()) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
+            pcl::transformPointCloud(*cloud_for_width_upwheel, *cloud_for_width_upwheel, t_affine.matrix());
+
+            // 车宽重计算，并赋值到当前车宽
+            pcl::getMinMax3D(*cloud_for_width_upwheel, t_rough_min_p, t_rough_max_p);
+        }
+        double rough_width = t_rough_max_p.x - t_rough_min_p.x;
+
+        //离群点过滤
+        if (!outlierFilter(t_width_extract_cloud, t_width_extract_cloud)) {
+            LOG(WARNING) << "outlierFilter error.";
+            return Error_code::FAILED;
+        }
+
+        Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
+        t_affine.prerotate(Eigen::AngleAxisd((90 - m_detect_result.theta()) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
+        pcl::transformPointCloud(*t_width_extract_cloud, *t_width_extract_cloud, t_affine.matrix());
+
+        // 车宽重计算，并赋值到当前车宽
+        pcl::PointXYZ t_min_p, t_max_p;
+        pcl::getMinMax3D(*t_width_extract_cloud, t_min_p, t_max_p);
+
+        double accurate_width = t_max_p.x - t_min_p.x;
+        m_detect_result.set_width(accurate_width);
+        m_t = std::chrono::steady_clock::now();
+
+        if (fabs(rough_width - accurate_width) > 0.15) {
+            std::string disp_str =
+                    std::string("width difference too large, assume noise situation, ") + std::to_string(rough_width) +
+                    ", " + std::to_string(accurate_width);
+            LOG(WARNING) << disp_str;
+            return Error_code::FAILED;
+        }
+    }
+
+//    diff = std::chrono::steady_clock::now() - t;
+//    LOG(INFO) << "cost time : " << diff.count() * 1000 << "ms.";
+
+    return Error_code::SUCCESS;
+}
+
+Error_manager DetectWheel::getDetectResult(DetectResult &result) {
+    std::chrono::duration<double> t = std::chrono::steady_clock::now() - m_t;
+
+    if (t.count() * 1000 > 1000) {
+        return Error_code::FAILED;
+    }
+
+    result = m_detect_result;
+
+    return Error_code::SUCCESS;
+}
+
+int DetectWheel::borderStatus() {
+    return m_border_status;
+}
+
+int DetectWheel::groundStatus() {
+    return m_ground_status;
+}
+
+bool
+DetectWheel::cutFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud) {
+    for (auto &pt: cloud->points) {
+        if (pt.x > m_region.cut_region().minx() && pt.x < m_region.cut_region().maxx() && pt.y > m_region.cut_region().miny() && pt.y < m_region.cut_region().maxy() &&
+            pt.z > m_region.cut_region().minz() && pt.z < m_region.cut_region().maxz()) {
+            out_cloud->push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
+        }
+    }
+    if (out_cloud->empty()) {
+        DLOG(WARNING) << "Filter all points, please check region or cloud data.";
+        return false;
+    }
+    return true;
+}
+
+bool DetectWheel::voxeFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
+                             pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud, pcl::PointXYZ leaf) {
+    pcl::VoxelGrid<pcl::PointXYZ> vox;             //创建滤波对象
+    vox.setInputCloud(input_cloud);          //设置需要过滤的点云给滤波对象
+    vox.setLeafSize((float) leaf.x, (float) leaf.y, (float) leaf.z); //设置滤波时创建的体素体积为1cm的立方体
+    vox.filter(*out_cloud);                     //执行滤波处理，存储输出
+    if (out_cloud->empty()) {
+        DLOG(WARNING) << "Voxe Filter all points, please check LeafSize or cloud data.";
+        return false;
+    }
+    return true;
+}
+
+bool DetectWheel::outlierFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
+                                pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud) {
+    //离群点过滤
+    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
+    sor.setInputCloud(input_cloud);
+    sor.setMeanK(5);                           //K近邻搜索点个数
+    sor.setStddevMulThresh(3.0);         //标准差倍数
+    sor.setNegative(false);                        //保留未滤波点（内点）
+    sor.filter(*out_cloud);                     //保存滤波结果到cloud_filter
+    if (out_cloud->empty()) {
+        DLOG(WARNING) << "Outlier filter all points, please check K、Stddev、Negative or cloud data.";
+        return false;
+    }
+    return true;
+}
+
+//欧式聚类*******************************************************
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>
+DetectWheel::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud) {
+    std::vector<pcl::PointIndices> ece_inlier;
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
+    ece.setInputCloud(sor_cloud);
+    ece.setClusterTolerance(0.07);
+    ece.setMinClusterSize(20);
+    ece.setMaxClusterSize(20000);
+    ece.setSearchMethod(tree);
+    ece.extract(ece_inlier);
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation_clouds;
+    for (int i = 0; i < ece_inlier.size(); i++) {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
+        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
+        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy); //按照索引提取点云数据
+        segmentation_clouds.push_back(cloud_copy);
+    }
+    return segmentation_clouds;
+}
+
+bool DetectWheel::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
+                                        DetectResult &result) {
+    if (m_detector == nullptr)
+        return false;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
+    for (int i = 0; i < cloud->size(); ++i) {
+        if (cloud->points[i].z < thresh_z) {
+            cloud_filtered->push_back(cloud->points[i]);
+        }
+    }
+    static int i = 0;
+    //下采样
+    if (!voxeFilter(cloud_filtered, cloud_filtered, pcl::PointXYZ(0.01, 0.01, 0.01))) {
+        return false;
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/classify_voxe_cloud_filtered_" + std::to_string(i) + ".txt", cloud_filtered);
+#endif
+
+    // 离群滤波
+    if (!outlierFilter(cloud_filtered, cloud_filtered)) {
+        return false;
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/classify_outlier_cloud_filtered_" + std::to_string(i) + ".txt", cloud_filtered);
+#endif
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> seg_clouds;
+    seg_clouds = segmentation(cloud_filtered);
+
+    if (!(seg_clouds.size() == 4 || seg_clouds.size() == 3)) {
+        DLOG(INFO) << seg_clouds.size();
+        return false;
+    }
+    std::vector<cv::Point2f> centers;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr record(new pcl::PointCloud<pcl::PointXYZ>);
+    cv::Point2f temp_centers(0, 0);
+    for (const auto &i: seg_clouds) {
+        record->operator+=(*i);
+    }
+    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]));
+        temp_centers += cv::Point2f(centroid[0], centroid[1]);
+    }
+#if RECORD_DETECT_CLOUD_DATA
+    WriteTxtCloud("../etc/test/segmentation_cloud_filtered_" + std::to_string(i) + ".txt", record);
+#endif
+    temp_centers /= 4.0f;
+    bool ret = isRect(centers);
+    DLOG(INFO) << "isRect ret :" << ret;
+    if (ret) {
+        std::string error_str;
+        ret = m_detector->detect(seg_clouds, result, error_str);
+#if RECORD_DETECT_CLOUD_DATA
+        record->clear();
+        record->operator+=(m_detector->m_left_front_transformed_cloud);
+        WriteTxtCloud("../etc/test/m_left_front_transformed_cloud" + std::to_string(i) + ".txt", record);
+        record->clear();
+        record->operator+=(m_detector->m_right_front_transformed_cloud);
+        WriteTxtCloud("../etc/test/m_right_front_transformed_cloud" + std::to_string(i) + ".txt", record);
+        record->clear();
+        record->operator+=(m_detector->m_left_rear_transformed_cloud);
+        WriteTxtCloud("../etc/test/m_left_rear_transformed_cloud" + std::to_string(i) + ".txt", record);
+        record->clear();
+        record->operator+=(m_detector->m_right_rear_transformed_cloud);
+        WriteTxtCloud("../etc/test/m_right_rear_transformed_cloud" + std::to_string(i) + ".txt", record);
+#endif
+        if (!ret) {
+            LOG(INFO) << "error_str :" << error_str;
+        }
+        return ret;
+    }
+    return ret;
+}
+
+bool DetectWheel::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))) {
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            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) {
+            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) {
+            return false;
+        }
+
+        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))) {
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            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);
+            return true;
+        } else {
+            return false;
+        }
+    }
+
+    return false;
+}
+
+Error_manager DetectWheel::upStatus(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
+    upBorderStatus();
+    if (!m_detect_result.success())
+    {
+        if (cloud->empty())
+            m_ground_status = Ground_statu::Nothing;
+        else
+            m_ground_status = Ground_statu::Noise;
+    }
+    else if (m_border_status == int(Range_status::Range_correct))
+    {
+        m_ground_status = Ground_statu::ok;
+    }
+    else {
+        m_ground_status = Ground_statu::Car_border_reached;
+    }
+    return Error_code::SUCCESS;
+}
+
+Error_manager DetectWheel::upBorderStatus() {
+    m_border_status = Range_status::Range_correct;
+    isRangeXY();
+    isRangeCarWidth();
+    isRangeCarWheelbase();
+    isAngleClock();
+
+    return Error_code::SUCCESS;
+}
+
+Error_manager DetectWheel::isRangeXY() {
+    Eigen::Vector2d car_center(m_detect_result.cx(), m_detect_result.cy());
+    Eigen::Vector2d car_uniform_center;
+    double theta_rad = m_detect_result.theta() * M_PI / 180.0;
+    car_uniform_center.x() = car_center.x() * cos(theta_rad) - car_center.y() * sin(theta_rad);
+    car_uniform_center.y() = car_center.x() * sin(theta_rad) + car_center.y() * cos(theta_rad);
+
+    double t_center_y = 0.0 - (car_uniform_center.y() + m_region.plc_offsety()); // 汽车中心y， 参照plc坐标，并且进行偏移， 绝对值。结果大约为 5～6左右
+    if( t_center_y - 4.88 + 0.5*(m_detect_result.wheel_base() - 2.7) < m_region.plc_border_maxy()-0.000    &&
+        t_center_y - 4.88 - 0.5*(m_detect_result.wheel_base() - 2.7) < m_region.plc_border_miny()-0.000 )
+    {
+        // y未超界
+        m_border_status &= (~Range_status::Range_back);
+    }
+    else if( t_center_y - 4.88 + 0.5*(m_detect_result.wheel_base() - 2.7) > m_region.plc_border_maxy()+0.010    ||
+             t_center_y - 4.88 - 0.5*(m_detect_result.wheel_base() - 2.7) > m_region.plc_border_miny()+0.010 )
+    {
+        // y超界
+        m_border_status |= (Range_status::Range_back);
+    }
+
+    float t_center_x = car_uniform_center.y() + m_region.plc_offsetx();
+    if(t_center_x < m_region.border_minx()-0.000)
+    {
+        m_border_status |= Range_status::Range_left;
+    }
+    else if(t_center_x > m_region.border_minx()+0.010)
+    {
+        m_border_status &= (~Range_status::Range_left);
+    }
+
+    if(t_center_x > m_region.border_maxx()+0.000)
+    {
+        m_border_status |= Range_status::Range_right;
+    }
+    else if(t_center_x < m_region.border_maxx()-0.010)
+    {
+        m_border_status &= (~Range_status::Range_right);
+    }
+
+    return Error_code::SUCCESS;
+}
+
+Error_manager DetectWheel::isRangeCarWidth() {    // 判断车辆宽度超界情况
+    if (m_detect_result.width() < m_region.car_min_width()-0.000 ||
+            m_detect_result.width() > m_region.car_max_width()+0.000)
+    {
+        m_border_status |= Range_status::Range_car_width;
+    }
+    else if (m_detect_result.width() > m_region.car_min_width()+0.010 &&
+            m_detect_result.width() < m_region.car_max_width()-0.010)
+    {
+        m_border_status &= (~Range_status::Range_car_width);
+    }
+    return Error_code::SUCCESS;
+}
+
+Error_manager DetectWheel::isRangeCarWheelbase() {    // 判断车辆轴距超界情况
+    if (m_detect_result.wheel_base() < m_region.car_min_wheelbase()-0.000 ||
+            m_detect_result.wheel_base() > m_region.car_max_wheelbase()+0.000)
+    {
+        m_border_status |= Range_status::Range_car_wheelbase;
+    }
+    else if (m_detect_result.wheel_base() > m_region.car_min_wheelbase()+0.010 ||
+            m_detect_result.wheel_base() < m_region.car_max_wheelbase()-0.010)
+    {
+        m_border_status &= (~Range_status::Range_car_wheelbase);
+    }
+    return Error_code::SUCCESS;
+}
+
+Error_manager DetectWheel::isAngleClock() {
+    double t_dtheta = m_detect_result.theta() + m_region.plc_offset_degree();
+    if (t_dtheta < m_region.turnplate_angle_limit_anti_clockwise()+0.00)
+    {
+        m_border_status |= Range_status::Range_angle_anti_clock;
+    }
+    else if (t_dtheta > m_region.turnplate_angle_limit_anti_clockwise() + 0.10)
+    {
+        m_border_status &= (~Range_status::Range_angle_anti_clock);
+    }
+
+    if (t_dtheta > m_region.turnplate_angle_limit_clockwise()-0.00)
+    {
+        m_border_status |= Range_status::Range_angle_clock;
+    }
+    else if (t_dtheta < m_region.turnplate_angle_limit_clockwise() - 0.10)
+    {
+        m_border_status &= (~Range_status::Range_angle_clock);
+    }
+
+    return Error_code::SUCCESS;
+}
+

Modules/AlgWheelDetect/detect_wheel.h

@@ -0,0 +1,83 @@
+/**
+  * @project ALG_WHeelData
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/4/21
+ **/
+#pragma once
+
+#include "define.pb.h"
+#include "defines.hpp"
+#include "wheel_ceres_3d.hpp"
+
+// 测试用代码
+#include "tool/static_tool.hpp"
+#include "error_code/error_code.hpp"
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/common/impl/common.hpp>
+#include <json/json.h>
+#include "google/protobuf/util/json_util.h"
+#include "car_pose_detector.h"
+#include "chassis_ceres_solver.h"
+
+#define RECORD_DETECT_CLOUD_DATA 0
+
+class DetectWheel {
+public:
+    DetectWheel();
+
+    ~DetectWheel() = default;
+
+    Error_manager init(const Region &region);
+
+    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    Error_manager getDetectResult(DetectResult &result);
+
+    Error_manager upStatus(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    int borderStatus();
+
+    int groundStatus();
+
+protected:
+    bool cutFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud);
+
+    bool voxeFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud,
+                    pcl::PointXYZ leaf);
+
+    bool outlierFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &out_cloud);
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);
+
+private:
+
+    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
+                               DetectResult &result);
+
+    bool isRect(std::vector<cv::Point2f> &points);
+
+    Error_manager upBorderStatus();
+
+    Error_manager isRangeXY();
+
+    Error_manager isRangeCarWidth();
+
+    Error_manager isRangeCarWheelbase();
+
+    Error_manager isAngleClock();
+
+private:
+    int m_border_status;
+    int m_ground_status;
+    DetectResult m_detect_result;
+    std::chrono::steady_clock::time_point m_t = std::chrono::steady_clock::now();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_filtered;      // 区域切除后的点
+    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_cloud_detect_z;      // 底盘切除后的点
+
+    Region m_region;
+    WheelCeres3D *m_detector; // 检测器
+
+};
+

Modules/AlgWheelDetect/main.cpp

@@ -0,0 +1,71 @@
+/**
+  * @project Project
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/4/28
+ **/
+#include <iostream>
+#include "json/json.h"
+#include "AlgWheelDetect/cloud_data.h"
+#include "AlgWheelDetect/detect_wheel.h"
+#include "AlgWheelDetect/alg_manager.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <termios.h>
+#include <unistd.h>
+
+// 定义函数，还原终端设置
+void restore_terminal_mode()
+{
+    struct termios t;
+    tcgetattr(STDIN_FILENO, &t);
+    t.c_lflag |= ICANON | ECHO;
+    tcsetattr(STDIN_FILENO, TCSANOW, &t);
+}
+
+// 定义信号处理函数，在接收到SIGINT信号时退出程序
+void sigint_handler(int signum)
+{
+    restore_terminal_mode();
+    std::cout << std::endl << "Exit the program." << std::endl;
+    exit(0);
+}
+
+// 定义函数，设置终端为无缓冲输入模式
+void set_terminal_mode()
+{
+    struct termios t;
+    tcgetattr(STDIN_FILENO, &t);
+    t.c_lflag &= ~(ICANON | ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &t);
+}
+
+void checkKey() {
+    char c = getchar();  // 获取单个字符
+
+    if (c == 'Q' || c == 'q')
+    {
+        // 如果按下了q或Q键，则退出程序
+        pid_t pid = getpid();
+        kill(pid, SIGTERM);
+    }
+}
+
+int main(int argc, char **argv) {
+    signal(SIGTERM, sigint_handler); // 注册SIGINT信号处理函数
+    set_terminal_mode();            // 进入无缓冲输入模式
+
+    VelodyneManager velodyne;
+    if (!velodyne.run_measure_form("../etc/measure_config.json")) {
+        return EXIT_FAILURE;
+    }
+
+    // 死循环
+    while (true) {
+        checkKey();
+    }
+
+    return EXIT_SUCCESS;
+}

Modules/AlgWheelDetect/rabbitmq.cpp

@@ -0,0 +1,7 @@
+/**
+  * @project Project
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/5/5
+ **/
+#include "rabbitmq.h"

Modules/AlgWheelDetect/rabbitmq.h

@@ -0,0 +1,13 @@
+/**
+  * @project Project
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/5/5
+ **/
+#pragma once
+
+#include "rabbitmq/rabbitmq_base.h"
+
+class DetectResultRabbitmq : public Rabbitmq_base {
+
+};

Modules/AlgWheelDetect/third_src/common/math.h

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

Modules/AlgWheelDetect/third_src/common/port.h

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

Modules/AlgWheelDetect/third_src/hybrid_grid.hpp

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

Modules/AlgWheelDetect/third_src/interpolated_grid.hpp

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

Modules/AlgWheelDetect/trajxtory_regression.hpp

@@ -0,0 +1,95 @@
+/**
+  * @project Project
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/6/13
+ **/
+#pragma once
+
+#include "defines.hpp"
+#include "define.pb.h"
+#include "error_code/error_code.hpp"
+
+#include <deque>
+
+class TrajxtoryRegression {
+public:
+    /**
+      * @brief   更新全部的车辆轨迹数据, 更新后会自动根据当前轨迹队列进行回归.
+      * @param   deque 完整的轨迹数据, 更新后当前的轨迹数据就是这个输入的轨迹数据.
+      * @return  数据回归结果
+      */
+    Error_manager upTrajxtoryQueue(std::deque<measure_statu> &deque) {
+        m_trajxtory_deque = deque;
+
+        return fitTrajxtory();
+    }
+
+    /**
+      * @brief   添加一组的轨迹数据, 添加后会更新轨迹回归参数.
+      * @param   trqjxtory 轨迹数据
+      * @return  数据回归结果
+      */
+    Error_manager upTrajxtoryQueue(measure_statu &trqjxtory) {
+        m_trajxtory_deque.pop_front();
+        m_trajxtory_deque.emplace_back(trqjxtory);
+
+        return fitTrajxtory();
+    }
+
+    /**
+      * @brief   刷新轨迹数据, 再次进行回归, 默认情况下会释放一组轨迹数据
+      * @param   erase 是否释放队列第一个轨迹数据
+      * @return  数据回归结果
+      */
+    Error_manager upTrajxtoryQueue(bool erase = true) {
+        if (erase) {
+            m_trajxtory_deque.pop_front();
+        }
+
+        return fitTrajxtory();
+    }
+
+    /**
+      * @brief   计算输入轨迹数据相对于当前回归数据的损失值.
+      * @param   trqjxtory 轨迹数据
+      * @return  损失值
+      */
+    double lossTrajxtory(measure_statu &trqjxtory) {
+        // TODO
+        return -1;
+    }
+
+    /**
+      * @brief   获取目前存储的轨迹数据
+      * @param   deque 传出轨迹数据
+      * @return  NULL
+      */
+    Error_manager getTrajxtoryQueue(std::deque<measure_statu> &deque) {
+        deque = m_trajxtory_deque;
+
+        return Error_code::SUCCESS;
+    }
+
+    /**
+      * @brief   获取轨迹回归参数
+      * @param   TODO
+      * @return  NULL
+      */
+    Error_manager getRegressionParam();
+
+
+
+
+private:
+    Error_manager fitTrajxtory() {
+        // TODO
+        return Error_code::FAILED;
+    }
+
+private:
+    std::deque<measure_statu> m_trajxtory_deque;
+
+    // 回归参数 TODO
+
+};

Modules/AlgWheelDetect/wheel_ceres_3d.hpp

@@ -0,0 +1,593 @@
+//
+// Created by lz on 2023/4/3.
+//
+#pragma once
+
+#include <random>
+#include <ceres/ceres.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/centroid.h>
+#include <pcl/common/transforms.h>
+#include <opencv4/opencv2/opencv.hpp>
+#include "define.pb.h"
+#include "defines.hpp"
+#include "tool/static_tool.hpp"
+#include "third_src/interpolated_grid.hpp"
+
+class CostFunctor3d {
+private:
+    const InterpolatedProbabilityGrid m_left_interpolated_grid;
+    const InterpolatedProbabilityGrid m_right_interpolated_grid;
+    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;     //右后点云
+    // 20220118 added by yct
+    float m_ratio; // 模型变换比例，用于贴合模型，cloud / ratio --> model
+
+public:
+    CostFunctor3d(pcl::PointCloud<pcl::PointXYZ> &left_front, pcl::PointCloud<pcl::PointXYZ> &right_front,
+                  pcl::PointCloud<pcl::PointXYZ> &left_rear, pcl::PointCloud<pcl::PointXYZ> &right_rear,
+                  const HybridGrid &left_interpolated_grid,
+                  const HybridGrid &right_interpolated_grid, float ratio)
+            : m_left_interpolated_grid(left_interpolated_grid), m_right_interpolated_grid(right_interpolated_grid) {
+        m_left_front_cloud = left_front;
+        m_right_front_cloud = right_front;
+        m_left_rear_cloud = left_rear;
+        m_right_rear_cloud = right_rear;
+        m_ratio = ratio;
+    }
+
+    template<typename T>
+    bool operator()(const T *const variable, T *residual) const {
+        T cx = variable[0];
+        T cy = variable[1];
+        T theta = variable[2];
+        T length = variable[3];
+        T width = variable[4];
+        T theta_front = variable[5];
+
+        //整车旋转
+        Eigen::Rotation2D<T> rotation(theta);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //左前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+        //右前偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+        //左后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+        //右后偏移
+        Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+
+        //前轮旋转
+        Eigen::Rotation2D<T> rotation_front(theta_front);
+        Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+
+        //左前轮
+        size_t 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 * T(m_ratio));
+            residual[i] = T(1.0) - m_left_interpolated_grid.GetInterpolatedValue(point_rotation[0], point_rotation[1],
+                                                                                 T(m_left_front_cloud[i].z));
+        }
+        //右前轮
+        size_t 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 * T(m_ratio));
+
+            residual[left_front_num + i] =
+                    T(1.0) - m_right_interpolated_grid.GetInterpolatedValue(point_rotation[0], point_rotation[1],
+                                                                            T(m_right_front_cloud[i].z));
+        }
+        //左后轮
+        size_t 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) * T(m_ratio);
+
+            residual[left_front_num + right_front_num + i] = T(1.0) -
+                                                             m_left_interpolated_grid.GetInterpolatedValue(
+                                                                     tanslate_point[0], tanslate_point[1],
+                                                                     T(m_left_rear_cloud[i].z));
+        }
+
+        //右后轮
+        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) * T(m_ratio);
+
+            residual[left_front_num + right_front_num + left_rear_num + i] = T(1.0) -
+                                                                             m_right_interpolated_grid.GetInterpolatedValue(
+                                                                                     tanslate_point[0],
+                                                                                     tanslate_point[1],
+                                                                                     T(m_right_rear_cloud[i].z));
+        }
+
+        return true;
+    }
+
+};
+
+class WheelCeres3D {
+public:
+    // public函数
+    WheelCeres3D() = default;
+
+    ~WheelCeres3D() = default;
+
+    bool init(const std::map<std::string, float> &files) {
+        if (!loadModule(files, grids)) {
+            return false;
+        }
+
+        DLOG(INFO) << "cloud module grid is well, test (0,0,0) :"
+                   << grids[0].GetProbability(grids[0].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
+        DLOG(INFO) << "cloud module grid is well, test (0,0,0) :"
+                   << grids[1].GetProbability(grids[1].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
+        return true;
+    }
+
+    bool detect(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &cloud_vec, DetectResult &detect_result,
+                std::string &error_info) {
+        error_info = "";
+        //清理点云W
+        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 (const auto &i: cloud_vec) {
+            cloud_all += (*i);
+        }
+
+        if (cloud_all.size() < 20) {
+            LOG(WARNING) << "The point cloud data has " << cloud_all.size()
+                         << " remaining points after Euclidean clustering.";
+            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 (auto &i: cloud_all) {
+            points_cv.emplace_back(i.x, 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.0f / M_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+
+        //第二步, 将每份点云旋转回去,计算点云重心所在象限
+        pcl::PointCloud<pcl::PointXYZ> t_cloud_left, t_cloud_right, t_cloud_front, t_cloud_rear;
+        for (const auto &i: cloud_vec) {
+            pcl::PointCloud<pcl::PointXYZ> cloud = (*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);
+
+            //计算重心
+            pcl::compute3DCentroid(transformed_cloud, centroid);
+            double x = centroid[0];
+            double y = centroid[1];
+            //计算象限
+            if (x > 0 && y > 0) {
+                //第一象限
+                m_left_front_cloud = cloud;
+                t_cloud_left += (m_left_front_cloud);
+                t_cloud_front += (m_left_front_cloud);
+            }
+            if (x > 0 && y < 0) {
+                //第四象限
+                m_right_front_cloud = cloud;
+                t_cloud_right += (m_right_front_cloud);
+                t_cloud_front += (m_right_front_cloud);
+            }
+            if (x < 0 && y > 0) {
+                //第二象限
+                m_left_rear_cloud = cloud;
+                t_cloud_left += (m_left_rear_cloud);
+                t_cloud_rear += (m_left_rear_cloud);
+            }
+            if (x < 0 && y < 0) {
+                //第三象限
+                m_right_rear_cloud = cloud;
+                t_cloud_right += (m_right_rear_cloud);
+                t_cloud_rear += (m_right_rear_cloud);
+            }
+        }
+
+        Eigen::Vector4f centroid_left, centroid_right, centroid_front, centroid_rear;
+        // 计算x
+        pcl::compute3DCentroid(t_cloud_left, centroid_left);
+        pcl::compute3DCentroid(t_cloud_right, centroid_right);
+        // 计算y
+        pcl::compute3DCentroid(t_cloud_front, centroid_front);
+        pcl::compute3DCentroid(t_cloud_rear, centroid_rear);
+        // LOG(WARNING)<<"x,y: "<<(centroid_left.x()+centroid_right.x())/2.0f<<", "<<(centroid_front.y()+centroid_rear.y())/2.0f;
+
+        // 使用外部第一步优化产生的x，以及当前计算后的y作为初值
+        // 多次优化，获取最佳优化结果
+        // detect_result.cx = center_x;
+        detect_result.set_cx(detect_result.cx());
+        detect_result.set_cy((centroid_front.y() + centroid_rear.y()) / 2.0f);
+        detect_result.set_wheel_base(centroid_front.y() - centroid_rear.y() + 0.12);
+        // 优化结果
+        bool solve_result = false;
+
+        DetectResult t_detect_result = detect_result;
+
+        LossResult t_loss_result;
+        t_loss_result.set_total_avg_loss(1000);
+        std::string t_error_info;
+        bool current_result = Solve(t_detect_result, t_loss_result, t_error_info);
+        error_info = t_error_info;
+
+        if (current_result) {
+            detect_result = t_detect_result;
+//            detect_result.set_allocated_loss(&t_loss_result);
+            detect_result.mutable_loss()->CopyFrom(t_loss_result);
+            solve_result = current_result;
+        }
+        detect_result.set_success(solve_result);
+
+        return solve_result;
+    }
+
+protected:
+    // protected函数
+
+private:
+    // private函数
+    bool loadModule(const std::map<std::string, float> &files, std::vector<HybridGrid> &grids) {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr module(new pcl::PointCloud<pcl::PointXYZ>);
+        for (const auto &file: files) {
+            DLOG(INFO) << "Load module:" << file.first << ", raido:" << file.second;
+            module->clear();
+//            HybridGrid grid(0.01);
+            grids.emplace_back(0.01);
+            if (!ReadTxtCloud(file.first, module)) {
+                return false;
+            }
+            update_model(module, grids.back(), file.second);
+        }
+        return true;
+    }
+
+
+// 根据点云与模型比例参数更新模型
+    bool update_model(pcl::PointCloud<pcl::PointXYZ>::Ptr &points, HybridGrid &grid, float ratio) {
+        float del_x = 0.05;
+        float del_y = 0.03;
+        float del_z = 0.05;
+        Eigen::Matrix3f delta = Eigen::MatrixXf::Identity(3, 3);
+        delta(0, 0) = del_x * del_x;
+        delta(1, 1) = del_y * del_y;
+        delta(2, 2) = del_z * del_z;
+        Eigen::Matrix3f delta_inv = delta.inverse();
+
+
+        Eigen::Vector3f d(0, 0, 0.02);
+        Eigen::MatrixXf ce = -0.5 * ((d).transpose() * delta_inv * (d));
+        float cut_p = exp(ce(0, 0));
+
+        DLOG(INFO) << "grid.resolution() = " << grid.resolution();
+
+        for (auto &pt: points->points) {
+            Eigen::Vector3f u(pt.x, pt.y, pt.z);
+            for (float x = pt.x - 0.1; x < pt.x + 0.1; x += grid.resolution()) {
+                for (float y = pt.y - 0.1; y < pt.y + 0.1; y += grid.resolution()) {
+                    for (float z = pt.z - 0.1; z < pt.z + 0.1; z += grid.resolution()) {
+                        Eigen::Vector3f p(x, y, z);
+                        Eigen::MatrixXf B = -0.5 * ((p - u).transpose() * delta_inv * (p - u));
+                        float prob = exp(B(0, 0));
+                        if (prob > cut_p)
+                            prob = cut_p;
+                        Eigen::Array3i index = grid.GetCellIndex(p);
+                        if (grid.GetProbability(index) < prob) {
+                            grid.SetProbability(index, prob);
+                        }
+                    }
+                }
+            }
+        }
+        DLOG(INFO) << "0, 0, 0 :" << grid.GetProbability(grid.GetCellIndex(Eigen::Vector3f(0, 0, 0)));
+
+        return true;
+    }
+
+    bool Solve(DetectResult &detect_result, LossResult &loss_result, std::string &error_info) {
+        if (grids.size() != 2) {
+            LOG(ERROR) << "Cloud module grid is nullptr, please check it's initialization process.";
+            return false;
+        }
+//        LOG(INFO) << "cloud module grid is well, test (0,0,0) :" << grids[0].GetProbability(grids[0].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
+//        LOG(INFO) << "cloud module grid is well, test (0,0,0) :" << grids[1].GetProbability(grids[1].GetCellIndex(Eigen::Vector3f(0, 0, 0)));
+        float model_ratio = 1.0f;
+        double cx = detect_result.cx();
+        double cy = detect_result.cy();
+        double init_theta = detect_result.theta();
+        double init_wheel_base = 2.7;
+        if (detect_result.wheel_base() > 2.55 && detect_result.wheel_base() < 3.2)
+            init_wheel_base = detect_result.wheel_base();
+        double init_width = 1.85;
+        double init_theta_front = 0 * M_PI / 180.0;
+
+        double variable[] = {cx, cy, init_theta, init_wheel_base, init_width, init_theta_front};
+        // 第二部分：构建寻优问题
+        ceres::Problem problem;
+        auto *cost_func = new CostFunctor3d(m_left_front_cloud, m_right_front_cloud, m_left_rear_cloud,
+                                            m_right_rear_cloud,
+                                            grids[0], grids[1], model_ratio);
+        //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+        ceres::CostFunction *cost_function = new
+                ceres::AutoDiffCostFunction<CostFunctor3d, ceres::DYNAMIC, 6>(
+                cost_func,
+                m_left_front_cloud.size() + m_right_front_cloud.size() + m_left_rear_cloud.size() +
+                m_right_rear_cloud.size());
+        problem.AddResidualBlock(cost_function, nullptr, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+
+
+        //第三部分： 配置并运行求解器
+        ceres::Solver::Options options;
+        options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+        options.max_num_iterations = 500;
+        options.num_threads = 1;
+        options.minimizer_progress_to_stdout = false;//输出到cout
+        ceres::Solver::Summary summary;//优化信息
+        ceres::Solve(options, &problem, &summary);//求解!!!
+
+        double loss = summary.final_cost /
+                      (m_left_front_cloud.size() + m_right_front_cloud.size() + m_left_rear_cloud.size() +
+                       m_right_rear_cloud.size());
+
+        detect_result.set_cx(variable[0]);
+        detect_result.set_cy(variable[1]);
+        detect_result.set_theta((-variable[2]) * 180.0 / M_PI);
+        detect_result.set_wheel_base(variable[3]);
+        detect_result.set_width(variable[4]);
+        detect_result.set_front_theta(-(variable[5] * 180.0 / M_PI));
+
+        if (detect_result.theta() > 180.0)
+            detect_result.set_theta(detect_result.theta() - 180.0);
+        if (detect_result.theta() < 0)
+            detect_result.set_theta(detect_result.theta() + 180);
+
+        if (summary.iterations.size() <= 1) {
+            error_info.append(std::string("仅迭代一轮，优化异常"));
+            return false;
+        }
+        if (summary.iterations[summary.iterations.size() - 1].iteration <= 1) {
+            error_info.append(std::string("最终仅迭代一轮，优化异常"));
+            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")));
+            return false;
+        }
+
+        if (detect_result.theta() > 120 || detect_result.theta() < 60) {
+            error_info.append("车身角度错误 ").append(std::to_string(detect_result.theta())).append("\t");
+            return false;
+        }
+        if (fabs(detect_result.front_theta()) > 35) {
+            error_info.append("前轮角度错误 ").append(std::to_string(detect_result.front_theta())).append("\t");
+            return false;
+        }
+        // 将loss传出
+        double costs_lf, costs_rf, costs_lr, costs_rr;
+        get_costs(variable, costs_lf, costs_rf, costs_lr, costs_rr);
+
+        loss_result.set_lf_loss(costs_lf / float(m_left_front_cloud.size() + 1e-6f));
+        loss_result.set_rf_loss(costs_rf / float(m_right_front_cloud.size() + 1e-6f));
+        loss_result.set_lb_loss(costs_lr / float(m_left_rear_cloud.size() + 1e-6f));
+        loss_result.set_rb_loss(costs_rr / float(m_right_rear_cloud.size() + 1e-6f));
+        loss_result.set_total_avg_loss(loss);
+        detect_result.mutable_loss()->CopyFrom(loss_result);
+        if (loss_result.lf_loss() > 0.4 || loss_result.rf_loss() > 0.4
+            || loss_result.lb_loss() > 0.4 || loss_result.rb_loss() > 0.4) {
+            error_info.append("loss过大 lf").append(std::to_string(loss_result.lf_loss())).append("，rf").append(
+                    std::to_string(loss_result.rf_loss())).append(", lr").
+                    append(std::to_string(loss_result.lb_loss())).append("，rb").append(
+                    std::to_string(loss_result.rb_loss()));
+            return false;
+        }
+
+        return true;
+    }
+
+// 将点云数据根据costFunction函数的求解进行求解，获取各个轮子的losses值
+    void get_costs(const double *variable, double &lf, double &rf, double &lr, double &rr) {
+        double losses[4] = {0};
+        double cx = variable[0];
+        double cy = variable[1];
+        double theta = variable[2];
+        double length = variable[3];
+        double width = variable[4];
+        double theta_front = variable[5];
+
+        //整车旋转
+        Eigen::Rotation2D<double> rotation(theta);
+        Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //左前偏移
+        Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(length / 2.0, width / 2.0);
+        //右前偏移
+        Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(length / 2.0, -width / 2.0);
+        //左后偏移
+        Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-length / 2.0, width / 2.0);
+        //右后偏移
+        Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(-length / 2.0, -width / 2.0);
+
+        //前轮旋转
+        Eigen::Rotation2D<double> rotation_front(theta_front);
+        Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
+
+
+        InterpolatedProbabilityGrid left_interpolated_grid(grids[0]);
+        InterpolatedProbabilityGrid right_interpolated_grid(grids[1]);
+        //左前轮
+        m_left_front_transformed_cloud.clear();
+        for (auto &i: m_left_front_cloud) {
+            const Eigen::Matrix<double, 2, 1> point((double(i.x) - cx),
+                                                    (double(i.y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
+            //旋转
+            const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+            losses[0] += 1.0 - left_interpolated_grid.GetInterpolatedValue(point_rotation[0], point_rotation[1],
+                                                                           double(i.z));
+            m_left_front_transformed_cloud.emplace_back(
+                    pcl::PointXYZ(point_rotation[0], point_rotation[1], i.z));
+        }
+        //右前轮
+        m_right_front_transformed_cloud.clear();
+        for (auto &i: m_right_front_cloud) {
+            const Eigen::Matrix<double, 2, 1> point((double(i.x) - cx),
+                                                    (double(i.y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<double, 2, 1> tanslate_point =
+                    rotation_matrix * point - wheel_center_normal_right_front;
+            //旋转
+            const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
+
+            losses[1] += 1.0 - right_interpolated_grid.GetInterpolatedValue(point_rotation[0], point_rotation[1],
+                                                                            double(i.z));
+            m_right_front_transformed_cloud.emplace_back(pcl::PointXYZ(point_rotation[0], point_rotation[1],
+                                                                       double(i.z)));
+        }
+        //左后轮
+        m_left_rear_transformed_cloud.clear();
+        for (auto &i: m_left_rear_cloud) {
+            const Eigen::Matrix<double, 2, 1> point((double(i.x) - cx),
+                                                    (double(i.y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
+
+            losses[2] += 1.0 -
+                         left_interpolated_grid.GetInterpolatedValue(tanslate_point[0], tanslate_point[1],
+                                                                     double(i.z));
+            m_left_rear_transformed_cloud.emplace_back(pcl::PointXYZ(tanslate_point[0], tanslate_point[1],
+                                                                     double(i.z)));
+        }
+
+        //右后轮
+        m_right_rear_transformed_cloud.clear();
+        for (auto &i: m_right_rear_cloud) {
+            const Eigen::Matrix<double, 2, 1> point((double(i.x) - cx),
+                                                    (double(i.y) - cy));
+            //减去经过车辆旋转计算的左前中心
+            const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
+
+            losses[3] += 1.0 -
+                         right_interpolated_grid.GetInterpolatedValue(tanslate_point[0], tanslate_point[1],
+                                                                      double(i.z));
+            m_right_rear_transformed_cloud.emplace_back(pcl::PointXYZ(tanslate_point[0], tanslate_point[1],
+                                                                      double(i.z)));
+        }
+
+        lf = losses[0];
+        rf = losses[1];
+        lr = losses[2];
+        rr = losses[3];
+    }
+
+    // 定义函数对象
+    struct CostFunction {
+        explicit CostFunction(const cv::Point3f &point, const HybridGrid &iter) : x_(point.x), y_(point.y), z_(point.z),
+                                                                                  iter_grid(iter) {}
+
+        template<typename JetT>
+        bool operator()(const JetT *const input, JetT *residual) const {
+            JetT r = ((input[0] + x_) * ceres::cos(input[2]) - (input[1] + y_) * ceres::sin(input[2]));
+            JetT c = ((input[0] + x_) * ceres::sin(input[2]) + (input[1] + y_) * ceres::cos(input[2]));
+            JetT t(z_);
+
+            residual[0] = iter_grid.GetInterpolatedValue(r, c, t) + 1.0;
+
+//            LOG(INFO) << r << c << t << residual[0];
+
+            return true;
+        }
+
+    private:
+        const double x_;
+        const double y_;
+        const double z_;
+        const InterpolatedGrid<HybridGrid> iter_grid;
+    };
+
+public:
+    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;     //右后点云
+    pcl::PointCloud<pcl::PointXYZ> m_left_front_transformed_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ> m_right_front_transformed_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ> m_left_rear_transformed_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ> m_right_rear_transformed_cloud;     //右后点云
+
+protected:
+    // protected成员
+
+private:
+    // private成员
+//    HybridGrid *mp_left_grid;
+//    HybridGrid *mp_right_grid;
+    std::vector<HybridGrid> grids;
+
+    // 左右模型原始点云, 用于动态调整模型大小
+    pcl::PointCloud<pcl::PointXYZ> m_left_model_cloud;
+    pcl::PointCloud<pcl::PointXYZ> m_right_model_cloud;
+};

Modules/ClampSafety/.gitignore

@@ -0,0 +1,5 @@
+.vscode/
+.idea/
+*build*/
+record*/
+thirdpart/

Modules/ClampSafety/CMakeLists.txt

@@ -0,0 +1,45 @@
+if(TEST)
+	add_definitions(-DETC_PATH="${CMAKE_CURRENT_LIST_DIR}")
+endif ()
+
+set(Target_Name
+		clamp_safety
+		)
+
+include_directories(
+	/usr/local/include/snap7
+)
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/lidar LIDAR_SRC)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect DETECT_SRC)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc PLC_SRC)
+
+set(Target_source
+		${PLC_SRC}
+		${LIDAR_SRC}
+		${DETECT_SRC}
+		${CMAKE_CURRENT_LIST_DIR}/main.cpp
+)
+
+set(Target_libary
+		zx
+		snap7
+		-lpthread
+)
+
+message(${Target_Name} ":" ${Target_libary})
+
+add_executable(${Target_Name} ${Target_source})
+target_link_libraries(${Target_Name} ${Target_libary})
+
+# 将库文件，可执行文件，头文件安装到指定目录
+install(TARGETS ${Target_Name}
+		LIBRARY DESTINATION lib  # 动态库安装路径
+		ARCHIVE DESTINATION lib  # 静态库安装路径
+		RUNTIME DESTINATION bin  # 可执行文件安装路径
+		PUBLIC_HEADER DESTINATION include  # 头文件安装路径
+		)
+
+install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/etc
+		DESTINATION ${CMAKE_INSTALL_PREFIX}
+		)

Modules/ClampSafety/README.md

@@ -0,0 +1,33 @@
+# 一、protoc配置文件更新
+
+```bash
+protoc -I=./ clamp_parameter.proto --cpp_out=./
+```
+
+# 二、通讯点位
+
+| 名称                 | 数据类型 | 注释                                                     |
+| -------------------- | -------- | -------------------------------------------------------- |
+| 心跳位               | int      | plc逻辑：如果是1，延迟1s写2，上位机：如果是2，延迟1s写1; |
+| 夹杆前是否有轮子     | int      | 1有，0无;                                                |
+| 轮子距离夹杆中心便宜 | float    | 以雷达为视角，轮子在前，左边为负，右边为正;              |
+| 夹杆是否完全夹到位   | int      | 1到位，0不到位;                                          |
+
+# 备注：异常记录
+1、电气代码连接plc时会出现写数据错误;
+
+2、移动过程中也会出现写书据错误;
+
+
+
+尊敬的领导：
+
+本人于2023年2月2日加入公司，并顺利完成了三个月试用期。在试用期期间，我全心投入工作，不断学习和提升自己的能力，同时也得到了同事和上级的支持和帮助。
+
+首先，我想简要概括一下在试用期中我所承担的主要工作职责：熟悉公司代码架构、智能立体车库用户分类、夹持杆代码、速腾聚创16P雷达数据传输、雷达车轮点云优化。
+
+其次，在试用期中，我意识到自己还存在许多需要改进的地方，比如：数学方面理论基础薄弱，，并在这方面进行了努力。例如下班补习了一下基础的线性代数理论并通过多种方式查询学习了有关非线性优化问题。在接下来的工作中，我将继续努力，以更好地适应公司的需要，不断提升自己的能力。
+
+最后，我要感谢公司领导和同事们对我的信任和支持，让我全身心地投入到工作中。同时，我也会继续保持良好的工作态度，依据公司的要求和标准，完成好自己所承担的工作任务。
+
+在这份试用期工作总结中，我简要概述了自己在智象的工作表现和成绩，并且指出了自己需要改进和提高的方面。希望这份总结对于转正申请有所帮助。再次感谢公司领导和同事们的帮助和支持。

Modules/ClampSafety/detect/clamp_safety_detect.cpp

@@ -0,0 +1,329 @@
+#include "clamp_safety_detect.h"
+
+bool clamp_safety_detect::fit_line(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Line &line) {
+    if (cloud->size() < 20)
+        return false;
+
+    Eigen::MatrixXd A, B;
+    A.resize(cloud->size(), 2);
+    B.resize(cloud->size(), 1);
+    for (int i = 0; i < cloud->size(); ++i) {
+        A(i, 0) = cloud->points[i].x;
+        A(i, 1) = 1.0;
+        B(i, 0) = cloud->points[i].y;
+    }
+    Eigen::MatrixXd P = (A.transpose() * A).inverse() * (A.transpose()) * B;
+
+    float a = P(0, 0);
+    float b = -1.0;
+    float c = P(1, 0);
+    float sum = 0;
+    float sumx = 0, sumy = 0, w = 0, sumw = 0;
+
+    for (int i = 0; i < cloud->size(); ++i) {
+        pcl::PointXYZ pt = cloud->points[i];
+        w = float(1 / pow(cos(atan(fabs(pt.x / pt.y))), 2));
+        sumw += w;
+        sumx += w * pt.x;
+        sumy += pt.y;
+        sum += std::pow(a * pt.x + b * pt.y + c, 2) / (a * a + b * b);
+    }
+
+    line.cov = sqrt(sum) / float(cloud->size());
+    line.cx = sumx / sumw;
+    line.cy = sumy / float(cloud->size());
+
+    line.a = a;
+    line.b = b;
+    line.c = c;
+    line.theta_by_x = acos(abs(b / sqrt(a * a + b * b))) * 180.0 / M_PI;
+    line.cloud = cloud;
+
+    //printf(" abc(%f,%f,%f) line.vonv  cov:%f,%f,%f,%f,%f\n",a,b,c,line.theta_by_x,line.cov);
+    return true;
+
+}
+
+bool compare_r(const Line &line1, const Line &line2) {
+    return line1.theta_by_x < line2.theta_by_x;
+}
+
+bool pca(pcl::PointCloud<pcl::PointXY>::Ptr neighber, float &theta) {
+
+    if (neighber->size() < 10) {
+        return false;
+    }
+    float sumx = 0, sumy = 0;
+    for (int i = 0; i < neighber->size(); ++i) {
+
+        sumx += neighber->points[i].x;
+        sumy += neighber->points[i].y;
+
+    }
+    float mean_x = sumx / float(neighber->size());
+    float mean_y = sumy / float(neighber->size());
+    Eigen::MatrixXd A(2, neighber->size());
+    for (int i = 0; i < neighber->size(); ++i) {
+        pcl::PointXY pt = neighber->points[i];
+        A(0, i) = pt.x - mean_x;
+        A(1, i) = pt.y - mean_y;
+
+    }
+    Eigen::MatrixXd cov = A * A.transpose();
+    Eigen::EigenSolver<Eigen::MatrixXd> es(cov);
+
+    Eigen::VectorXd values = es.eigenvalues().real().normalized();
+    Eigen::MatrixXd vectors = es.eigenvectors().real();
+    //printf("pca values: %.5f  %.5f ",values[0],values[1]);
+    float max_value = values[0];
+    Eigen::Vector2d normal = vectors.col(0);
+    if (values[1] > values[0]) {
+        max_value = values[1];
+        normal = vectors.col(1);
+    }
+
+    if (isnanf(max_value)) {
+        return false;
+    }
+    if (max_value < 0.90) {
+        return false;
+    }
+
+
+    float r = 90 * 0.005 / (2. * M_PI);
+    theta = acos(abs(normal[0])) * r;
+
+    return true;
+}
+
+void pca_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr features, int length) {
+    if (cloud->size() < length) {
+        printf("ERROR  pca points size < %d\n", length);
+        return;
+    }
+    features->clear();
+    int fail_pca = 0;
+    for (int i = length / 2; i < cloud->size() - length / 2; ++i) {
+        pcl::PointCloud<pcl::PointXY>::Ptr neighber(new pcl::PointCloud<pcl::PointXY>);
+        for (int j = i - length / 2; j < i + length / 2; ++j) {
+            pcl::PointXYZ pt = cloud->points[j];
+            if (pt.y < 0.05 || pt.y > 0.5 || fabs(pt.x) > 0.5)
+                continue;
+            pcl::PointXY p_xy;
+            p_xy.x = cloud->points[j].x;
+            p_xy.y = cloud->points[j].y;
+            if (isnan(p_xy.x) || isnan(p_xy.y))
+                continue;
+            neighber->push_back(p_xy);
+        }
+        float theta = 0;
+        if (pca(neighber, theta)) {
+            pcl::PointXYZ pt = cloud->points[i];
+            pt.z = theta;
+            features->push_back(pt);
+
+        } else {
+            fail_pca++;
+        }
+    }
+
+}
+
+void pca_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr features, float radius) {
+    if (cloud->size() < 200)
+        return;
+
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    //pcl::KdTreeFLANN<pcl::PointXYZ> tree;
+    tree->setInputCloud(cloud);
+    features->clear();
+    for (int i = 0; i < cloud->size(); ++i) {
+        std::vector<float> distances;    //distance Vector
+        std::vector<int> neighber_ids;
+        tree->radiusSearch(cloud->points[i], radius, neighber_ids, distances); //KD tree
+
+        pcl::PointCloud<pcl::PointXY>::Ptr neighber(new pcl::PointCloud<pcl::PointXY>);
+        for (int neighber_id : neighber_ids) {
+            pcl::PointXY p_xy{};
+            p_xy.x = cloud->points[neighber_id].x;
+            p_xy.y = cloud->points[neighber_id].y;
+            neighber->push_back(p_xy);
+        }
+        float theta = 0;
+        if (pca(neighber, theta)) {
+            pcl::PointXYZ pt = cloud->points[i];
+            pt.z = theta;
+            features->push_back(pt);
+        }
+    }
+
+}
+
+
+clamp_safety_detect::clamp_safety_detect() = default;
+
+clamp_safety_detect::~clamp_safety_detect() = default;
+
+#if __VIEW__PCL
+
+void clamp_safety_detect::set_viewer(pcl::visualization::PCLVisualizer* viewer,int port)
+{
+    m_viewer=viewer;
+    m_port=port;
+}
+
+void clamp_safety_detect::view_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string name,int r,int g,int b)
+{
+    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud,r,g,b);
+    m_viewer->addPointCloud(cloud, single_color2, name,m_port);
+    m_viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, name,m_port);
+}
+
+#endif
+
+
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clamp_safety_detect::classify_cloud(
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cluster_cloud;
+    //printf("  cloud size():%d\n",cloud->size());
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_r(new pcl::PointCloud<pcl::PointXYZ>);
+
+    pca_cloud(cloud, cloud_r, 20);
+
+    if (cloud_r->size() == 0) {
+        return cluster_cloud;
+    }
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+
+    tree->setInputCloud(cloud_r);
+
+    std::vector<pcl::PointIndices> cluster_indices;
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
+    ec.setClusterTolerance(0.1);
+    ec.setMinClusterSize(100);
+    ec.setMaxClusterSize(1000);
+    ec.setSearchMethod(tree);
+    ec.setInputCloud(cloud_r);
+    ec.extract(cluster_indices);
+
+
+    cluster_cloud.resize(cluster_indices.size());
+
+    for (int i = 0; i < cluster_indices.size(); ++i) {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr clust(new pcl::PointCloud<pcl::PointXYZ>);
+        for (int j = 0; j < cluster_indices[i].indices.size(); ++j) {
+            pcl::PointXYZ pt;
+            pcl::PointXYZ ptr = cloud_r->points[cluster_indices[i].indices[j]];
+            pt.x = ptr.x;
+            pt.y = ptr.y;
+            pt.z = ptr.z; //z表示角度
+            clust->push_back(pt);
+        }
+        cluster_cloud[i] = clust;
+
+    }
+
+    return cluster_cloud;
+}
+
+bool clamp_safety_detect::check_wheel_line(const Line &line, Safety_status &safety) {
+    if (line.cloud->size() < 20) {
+        printf("line cloud size <20\n");
+        return false;
+    }
+    ///判断轮胎
+    pcl::PointXYZ minp, maxp;
+    pcl::getMinMax3D(*line.cloud, minp, maxp);
+    float length = maxp.x - minp.x;
+    if (line.cov < 0.03 && fabs(line.theta_by_x) < 10.
+        && (length > 0.1 && length < 0.5)) {
+        //前方直线是轮胎,判断中心是否居中、直线长度是否在范围内
+
+        safety.cx = line.cx;
+        // printf("safety.cx = %.3f\n", safety.cx);
+        //雷达距离轮胎的距离   间隙
+        safety.gap = fabs(line.cy);
+        //中心偏差5cm内，可夹，plc决定
+        safety.wheel_exist = true;
+    }
+#if __VIEW__PCL
+    char name[64]={0};
+    //显示直线，正确为绿，偏移为黄，错误为红，
+    char buf[64]={0};
+    sprintf(buf,"cx:%.4f,gap:%.4f,Θ:%.4f cov:%.4f",safety.cx,fabs(line.cy),line.theta_by_x,line.cov);
+    sprintf(name,"wheel_%d",m_port);
+    m_viewer->addText3D(buf,pcl::PointXYZ(line.cx-0.2,line.cy,0),0.01,0,1,0,"wheel",m_port);
+
+    float x1=line.cx-0.2;
+    float x2=line.cx+0.2;
+    float y1=-(x1*line.a/line.b+line.c/line.b);
+    float y2=-(x2*line.a/line.b+line.c/line.b);
+
+    sprintf(name,"line_%d",m_port);
+    if (safety.wheel_exist)
+    {
+        m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 0, 255, 0, name,m_port);
+    }
+    else if (fabs(safety.cx) < 0.5)
+    {
+        m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 255, 0, name,m_port);
+    }
+    else
+    {
+        m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 0, 0, name,m_port);
+    }
+#endif
+    return safety.wheel_exist;
+
+}
+
+bool clamp_safety_detect::check_clamp_lines(const Line &line1, const Line &line2, Safety_status &safety) {
+    if (fabs(line1.theta_by_x - line2.theta_by_x) > 10 || line1.cov > 0.03 || line2.cov > 0.03) {
+
+    }
+
+    if (fabs(line1.theta_by_x - 90.) < 15. && fabs(line2.theta_by_x - 90.) < 15.)
+        safety.clamp_completed = true;
+
+#if __VIEW__PCL
+    char name[64]={0};
+    float x1=line1.cx-0.2;
+    float x2=line1.cx+0.2;
+    float y1=-(x1*line1.a/line1.b+line1.c/line1.b);
+    float y2=-(x2*line1.a/line1.b+line1.c/line1.b);
+    sprintf(name,"line1_%d",m_port);
+    m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 0, 0, name,m_port);
+    char buf[32]={0};
+    sprintf(buf,"cx:%.3f,Θ:%.1f cov:%.3f",line1.cx,line1.theta_by_x,line1.cov);
+    sprintf(name,"line1_txt_%d",m_port);
+    m_viewer->addText3D(buf,pcl::PointXYZ(line1.cx-0.15,line1.cy,0),0.01,0,1,0,name,m_port);
+
+    x1=line2.cx-0.2;
+    x2=line2.cx+0.2;
+    y1=-(x1*line2.a/line2.b+line2.c/line2.b);
+    y2=-(x2*line2.a/line2.b+line2.c/line2.b);
+    sprintf(name,"line2_%d",m_port);
+    m_viewer->addLine(pcl::PointXYZ(x1,y1,0), pcl::PointXYZ(x2,y2,0), 255, 0, 0, name,m_port);
+    memset(buf,0,32);
+    sprintf(buf,"cx:%.3f,Θ:%.3f cov:%.3f",line2.cx,line2.theta_by_x,line2.cov);
+    sprintf(name,"line2_txt%d",m_port);
+    m_viewer->addText3D(buf,pcl::PointXYZ(line2.cx-0.15,line2.cy,0),0.01,0,1,0,name,m_port);
+#endif
+    return true;
+}
+
+bool clamp_safety_detect::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Safety_status &safety) {
+#if __VIEW__PCL
+    m_viewer->removeAllPointClouds(m_port);
+    m_viewer->removeAllShapes(m_port);
+    char buf[32]={0};
+    sprintf(buf,"cloud_%d",m_port);
+    view_cloud(cloud,buf,255,255,255);
+#endif
+    //找直线，记录内点率，直线与x轴夹角，斜率b/a
+    Line line;
+    if (fit_line(cloud, line) && check_wheel_line(line, safety)) {
+        return true;
+    }
+    return false;
+}

Modules/ClampSafety/detect/clamp_safety_detect.h

@@ -0,0 +1,120 @@
+//
+// Created by zx on 22-8-30.
+//
+
+#pragma once
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/point_representation.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/common/common_headers.h>
+#include <glog/logging.h>
+#include <iostream>
+#include <cmath>
+
+#if __VIEW__PCL
+#include <pcl/visualization/pcl_visualizer.h>
+#endif
+
+//ax+by+c=0
+typedef struct {
+    float a;
+    float b;
+    float c;
+    float cx;
+    float cy;
+    float theta_by_x;
+    float cov;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+} Line;
+
+class Safety_status {
+public:
+    Safety_status() {
+        clamp_completed = false;
+        wheel_exist = false;
+        cx = 0;
+        gap = 0;
+        time_point = std::chrono::steady_clock::now();
+        timeout_s = 0.5;
+    };
+
+    ~Safety_status() = default;
+
+    Safety_status(const Safety_status &other) {
+        clamp_completed = other.clamp_completed;
+        wheel_exist = other.wheel_exist;
+        cx = other.cx;
+        gap = other.gap;
+        time_point = other.time_point;
+        timeout_s = other.timeout_s;
+    }
+
+    Safety_status &operator=(const Safety_status &other) {
+        clamp_completed = other.clamp_completed;
+        wheel_exist = other.wheel_exist;
+        cx = other.cx;
+        gap = other.gap;
+        time_point = other.time_point;
+        timeout_s = other.timeout_s;
+        return *this;
+    }
+
+    void set_timeout(float s) {
+        timeout_s = s;
+    }
+
+    bool is_timeout() const {
+        auto t2 = std::chrono::steady_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - time_point);
+
+        double time = double(duration.count()) * std::chrono::microseconds::period::num /
+                      std::chrono::microseconds::period::den;
+        DLOG(INFO) << "time " << time;
+        return time > timeout_s;
+    }
+
+    bool clamp_completed;
+    bool wheel_exist;           //点云直线是否是轮子
+    float cx;
+    float gap;                  //轮子距离雷达
+    std::chrono::steady_clock::time_point time_point;
+    float timeout_s;
+    std::mutex mutex_;
+};
+
+class clamp_safety_detect {
+public:
+    clamp_safety_detect();
+
+    ~clamp_safety_detect();
+
+#if __VIEW__PCL
+    void set_viewer(pcl::visualization::PCLVisualizer* m_viewer,int port);
+#endif
+
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Safety_status &safety);
+
+    bool fit_line(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Line &line);
+
+    bool check_wheel_line(const Line &line, Safety_status &safety);
+
+protected:
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> classify_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
+
+    bool check_clamp_lines(const Line &line1, const Line &line2, Safety_status &safety);
+
+#if __VIEW__PCL
+    void view_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string name,int r,int g,int b);
+     pcl::visualization::PCLVisualizer* m_viewer;
+     int m_port;
+#endif
+};
+

Modules/ClampSafety/etc/clamp_safety_lidars.json

@@ -0,0 +1,82 @@
+[
+    {
+        "enable": true,
+        "angle_min": -2.35619449,
+        "angle_max": 2.35619449,
+        "angle_increment": 0.00582,
+        "time_increment": 0.000062,
+        "range_min": 0,
+        "range_max": 30,
+        "net_config": {
+            "ip_address": "10.211.31.150",
+            "port": 2110
+        },
+        "scan_box_limit": {
+            "dist_limit": 8,
+            "minx": -0.35,
+            "maxx": 0.35,
+            "miny": 0.05,
+            "maxy": 0.5
+        }
+    },
+    {
+        "enable": true,
+        "angle_min": -2.35619449,
+        "angle_max": 2.35619449,
+        "angle_increment": 0.00582,
+        "time_increment": 0.000062,
+        "range_min": 0,
+        "range_max": 30,
+        "net_config": {
+            "ip_address": "10.211.31.151",
+            "port": 2110
+        },
+        "scan_box_limit": {
+            "dist_limit": 8,
+            "minx": -0.35,
+            "maxx": 0.35,
+            "miny": 0.05,
+            "maxy": 0.5
+        }
+    },
+    {
+        "enable": true,
+        "angle_min": -2.35619449,
+        "angle_max": 2.35619449,
+        "angle_increment": 0.00582,
+        "time_increment": 0.000062,
+        "range_min": 0,
+        "range_max": 30,
+        "net_config": {
+            "ip_address": "10.211.31.152",
+            "port": 2110
+        },
+        "scan_box_limit": {
+            "dist_limit": 8,
+            "minx": -0.35,
+            "maxx": 0.35,
+            "miny": 0.05,
+            "maxy": 0.5
+        }
+    },
+    {
+        "enable": true,
+        "angle_min": -2.35619449,
+        "angle_max": 2.35619449,
+        "angle_increment": 0.00582,
+        "time_increment": 0.000062,
+        "range_min": 0,
+        "range_max": 30,
+        "net_config": {
+            "ip_address": "10.211.31.153",
+            "port": 2110
+        },
+        "scan_box_limit": {
+            "dist_limit": 8,
+            "minx": -0.35,
+            "maxx": 0.35,
+            "miny": 0.05,
+            "maxy": 0.5
+        }
+    }
+]

Modules/ClampSafety/etc/parameter.prototxt

@@ -0,0 +1,103 @@
+# 配置本设备ip
+local_parameter
+{
+    eth_name:"enx000ec6fe0a74"
+    local_ip:"10.211.31.140"
+}
+#左前夹 Y3
+lidars
+{
+	angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"10.211.31.150"
+        port:2110
+    }
+    scan_box_limit
+    {
+        dist_limit:8
+        minx:-0.35
+        maxx:0.35
+        miny:0.05
+        maxy:0.5
+    }
+}
+#右前夹 Y4
+lidars
+{
+	angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"10.211.31.151"
+        port:2110
+    }
+    scan_box_limit
+    {
+        dist_limit:8
+        minx:-0.35
+        maxx:0.35
+        miny:0.05
+        maxy:0.5
+    }
+}
+#左后夹 Y5
+lidars
+{
+	angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"10.211.31.152"
+        port:2110
+    }
+    scan_box_limit
+    {
+        dist_limit:8
+        minx:-0.35
+        maxx:0.35
+        miny:0.05
+        maxy:0.5
+    }
+}
+#右后夹 Y6
+lidars
+{
+	angle_min:-2.35619449
+    angle_max:2.35619449
+    angle_increment:0.00582
+    time_increment:0.000062
+    range_min:0
+    range_max:30
+    net_config
+    {
+        ip_address:"10.211.31.153"
+        port:2110
+    }
+    scan_box_limit
+    {
+        dist_limit:8
+        minx:-0.35
+        maxx:0.35
+        miny:0.05
+        maxy:0.5
+    }
+}
+
+plc_parameter
+{
+	ip_address:"10.211.31.1"
+}

Modules/ClampSafety/etc/plc.json

@@ -0,0 +1,3 @@
+{
+    "ip": "10.211.31.1"
+}

Modules/ClampSafety/lidar/async_client.cpp

@@ -0,0 +1,314 @@
+#include "async_client.h"
+
+// 构造
+Async_Client::Async_Client() : m_socket(m_io_service) {
+    m_communication_status = E_UNKNOWN;    //通信状态
+    m_is_reconnection_flag = false;        // 断线是否重连的标志位, true:断线自动重连, false:断线不重连, 保持断线状态.
+
+    mp_thread_receive = NULL;        // 接受线程, 内存由本模块管理
+    mp_thread_check_connect = NULL;        // 检查线程, 内存由本模块管理
+    m_check_connect_wait_time_ms = 0;    // 检查连接线程 的等待时间
+
+    m_data_updata_time = std::chrono::system_clock::now() - std::chrono::hours(1);
+
+    memset(m_data_buf, 0, DATA_LENGTH_MAX);
+    mp_communication_data_queue = NULL;
+}
+
+/**
+ * 析构函数
+ * */
+Async_Client::~Async_Client() {
+    uninit();
+}
+
+//初始化, 默认重连
+Error_manager Async_Client::init(std::string ip, int port, Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
+                                 bool is_reconnection_flag) {
+    if (p_communication_data_queue == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL ");
+    }
+
+    m_is_reconnection_flag = is_reconnection_flag;
+
+    m_check_connect_wait_time_ms = 0;    // 检查连接线程 的等待时间, 第一次直接通过
+
+    boost::asio::ip::tcp::endpoint t_ep(boost::asio::ip::address_v4::from_string(ip), port);
+    m_ep = t_ep;
+    mp_communication_data_queue = p_communication_data_queue;
+
+    //接受线程, 默认等待
+    m_condition_receive.reset(false, false, false);
+    mp_thread_receive = new std::thread(&Async_Client::thread_receive, this);
+    //检查连接线程, 默认等待, 内部的wait_for会让其通过
+    m_condition_check_connect.reset(false, false, false);
+    mp_thread_check_connect = new std::thread(&Async_Client::thread_check_connect, this);
+
+    return Error_code::SUCCESS;
+}
+
+//反初始化
+Error_manager Async_Client::uninit() {
+    //注注注注注意了, 这里一定要在关闭通信线程之前, 关闭socket,
+    //因为异步操作自带一个线程, 这个线程可能会卡住我们的线程...
+    //关闭通信
+    m_socket.close();
+
+    //杀死2个线程，强制退出
+    if (mp_thread_receive) {
+        m_condition_receive.kill_all();
+    }
+    if (mp_thread_check_connect) {
+        m_condition_check_connect.kill_all();
+    }
+
+    //回收2个线程的资源
+    if (mp_thread_receive) {
+        mp_thread_receive->join();
+        delete mp_thread_receive;
+        mp_thread_receive = NULL;
+    }
+    if (mp_thread_check_connect) {
+        mp_thread_check_connect->join();
+        delete mp_thread_check_connect;
+        mp_thread_check_connect = NULL;
+    }
+
+
+    m_io_service.stop();
+
+
+    mp_communication_data_queue = NULL;
+
+    if (m_communication_status != E_FAULT) {
+        m_communication_status = E_UNKNOWN;
+    }
+    return Error_code::SUCCESS;
+}
+
+//检查状态
+Error_manager Async_Client::check_status() {
+    if (m_communication_status == E_READY) {
+        return Error_code::SUCCESS;
+    } else if (m_communication_status == E_UNKNOWN) {
+        return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_UNINITIALIZED, Error_level::MINOR_ERROR,
+                             " Async_Client::check_status() error ");
+    } else if (m_communication_status == E_DISCONNECT) {
+        return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_DISCONNECT, Error_level::MINOR_ERROR,
+                             " Async_Client::check_status() error ");
+    } else {
+        return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_FAULT, Error_level::MINOR_ERROR,
+                             " Async_Client::check_status() error ");
+    }
+}
+
+//判断是否正常
+bool Async_Client::is_ready() {
+    return (m_communication_status == E_READY);
+}
+
+//获取状态
+Async_Client::Communication_status Async_Client::get_status() {
+    return m_communication_status;
+}
+
+
+//线程接受函数，包括连接,重连和接受数据
+void Async_Client::thread_receive() {
+    LOG(INFO) << " ---Async_Client::thread_receive start ---" << this;
+
+    //接受雷达消息，包括连接,重连和接受数据
+    while (m_condition_receive.is_alive()) {
+        m_condition_receive.wait();
+        if (m_condition_receive.is_alive()) {
+            std::this_thread::yield();
+
+            // 连接成功后开启异步读
+            client_async_read();
+
+        }
+    }
+    LOG(INFO) << " Async_Client::thread_receive end :" << this;
+    return;
+}
+
+//线程检查连接函数，进行连接, 并检查消息是否按时更新, 如果超时, 那么触发重连
+void Async_Client::thread_check_connect() {
+    LOG(INFO) << " ---Async_Client::thread_check_connect start ---" << this;
+
+    //检查连接, 连接, 重连, 判断时间
+    while (m_condition_check_connect.is_alive()) {
+        m_condition_check_connect.wait_for_millisecond(m_check_connect_wait_time_ms);
+        if (m_condition_check_connect.is_alive()) {
+            std::this_thread::yield();
+
+            switch ((Communication_status) m_communication_status) {
+                case E_UNKNOWN: {
+                    //连接
+                    socket_connect();
+                    break;
+                }
+                case E_DISCONNECT: {
+                    //重连
+                    socket_reconnect();
+                    break;
+                }
+                case E_READY: {
+                    //检查
+                    socket_check();
+                    break;
+                }
+                default: {
+
+                    break;
+                }
+            }
+
+        }
+    }
+    LOG(INFO) << " Async_Client::thread_check_connect end :" << this;
+}
+
+
+// 开启连接
+void Async_Client::socket_connect() {
+    m_io_service.reset();
+    //开启异步连接, 使用m_ep里面的ip和port进行tcp异步连接
+    // 只有在run()之后, 才会真正的执行连接函数
+    m_socket.async_connect(m_ep, boost::bind(&Async_Client::handle_connect, this, boost::asio::placeholders::error));
+    //注注注注注注意了:async_connect是阻塞函数, 如果连接不上就会一直卡在这, 而不是通过之后, 回调返回错误码.
+    //这个函数大约1分钟之后就会通过, 并调用里面的回调函数,返回我们错误码.
+    //如果硬件恢复连接, 那么这个函数会立即连上,
+    //如果我们中途退出, 一定要 m_socket.close()退出异步通信他自己后台的线程, 防止这里卡死, 影响我们的线程关闭........
+    m_io_service.run();
+}
+
+//关闭连接
+void Async_Client::socket_close() {
+    m_socket.close();
+}
+
+// 重新连接
+void Async_Client::socket_reconnect() {
+    if (m_is_reconnection_flag) {
+        //关闭原有的连接
+        socket_close();
+        // 开启连接
+        socket_connect();
+    }
+}
+
+// 检查连接
+void Async_Client::socket_check() {
+    // 判断距上次读取到数据超时时间，过长则主动断开并重连
+    int duration = std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::system_clock::now() - m_data_updata_time).count();
+    if (duration > READ_TIMEOUT_MILLISECONDS) {
+        LOG(WARNING) << "connection check thread found read timeout, close socket and try to reinitialize/reconnect.";
+
+        // 通信超时后, 修改状态为断连,
+        m_communication_status = E_DISCONNECT;
+        //检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+        m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+        //关闭接受线程
+        m_condition_receive.notify_all(false);
+    }
+}
+
+// 异步写入
+void Async_Client::client_async_write(char *buf, int len) {
+    m_io_service.run();
+    boost::asio::async_write(m_socket,
+                             boost::asio::buffer(buf, len),
+                             boost::bind(&Async_Client::handle_write, this,
+                                         boost::asio::placeholders::error));
+    m_io_service.run();
+}
+
+// 异步读取
+void Async_Client::client_async_read() {
+    //异步读取, 在run()之后, 会自动接受数据, 并存入mp_data_buf里面,
+    // 然后调用回调函数handle_read(), bytes_transferred是返回的数据有效长度
+
+    m_io_service.reset();
+    m_socket.async_read_some(boost::asio::buffer(m_data_buf, DATA_LENGTH_MAX),
+                             boost::bind(&Async_Client::handle_read, this,
+                                         boost::asio::placeholders::error,
+                                         boost::asio::placeholders::bytes_transferred));
+    //注:async_read_some, 如果断连, 那么就会回调handle_read, 并填充错误码 boost::asio::placeholders::error
+    m_io_service.run();
+}
+
+
+//异步连接回调, 失败后重连
+void Async_Client::handle_connect(const boost::system::error_code &error) {
+    if (!error) {
+        LOG(INFO) << "Async_Client::handle_connect Connection Successful! " << m_ep.address().to_string() << ":"
+                  << m_ep.port();
+        // 连接成功后修改状态为正常待机,
+        m_communication_status = E_READY;
+        //检查连接等待时间变为 CHECK_WAIT_TIME_MS,
+        m_check_connect_wait_time_ms = CHECK_WAIT_TIME_MS;
+        //唤醒接受线程
+        m_condition_receive.notify_all(true);
+
+
+    } else {
+        LOG(WARNING) << "connect failed, " << boost::system::system_error(error).what() << ", waiting for reconnection "
+                     << m_ep.address().to_string() << ":" << m_ep.port();
+        // 连接失败后修改状态为断连,
+        m_communication_status = E_DISCONNECT;
+        //检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+        m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+        //关闭接受线程
+        m_condition_receive.notify_all(false);
+    }
+}
+
+
+//异步读取回调, 失败后重连. bytes_transferred是返回的数据有效长度
+void Async_Client::handle_read(const boost::system::error_code &error,
+                               size_t bytes_transferred) {
+    if (!error) {
+        Binary_buf *tp_binary_buf = new Binary_buf(m_data_buf, bytes_transferred);
+        if (mp_communication_data_queue != NULL) {
+            //将数据添加到队列中, 然后内存权限转交给队列.
+            mp_communication_data_queue->push(tp_binary_buf);
+        }
+
+        //更新时间
+        m_data_updata_time = std::chrono::system_clock::now();
+
+    } else {
+        LOG(WARNING) << "handle_read, " << boost::system::system_error(error).what() << ", waiting for reconnection "
+                     << m_ep.address().to_string() << ":" << m_ep.port();
+
+        // 读取失败后修改状态为断连,
+        m_communication_status = E_DISCONNECT;
+        //检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+        m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+        //关闭接受线程
+        m_condition_receive.notify_all(false);
+    }
+}
+
+
+//异步写入回调, 失败后重连
+void Async_Client::handle_write(const boost::system::error_code &error) {
+    if (!error) {
+        //发送成功, 暂时不做处理
+//		LOG(INFO) << "handle write no error";
+    } else {
+        LOG(WARNING) << "handle_write, " << boost::system::system_error(error).what() << ", waiting for reconnection "
+                     << m_ep.address().to_string() << ":" << m_ep.port();
+        // 读取失败后修改状态为断连,
+        m_communication_status = E_DISCONNECT;
+        //检查连接等待时间变为 RECONNECTION_WAIT_TIME_MS,
+        m_check_connect_wait_time_ms = RECONNECTION_WAIT_TIME_MS;
+        //关闭接受线程
+        m_condition_receive.notify_all(false);
+    }
+}
+
+

Modules/ClampSafety/lidar/async_client.h

@@ -0,0 +1,189 @@
+/*
+ * 	//启动异步服务
+ *
+ * 必须使用如下三步
+	m_io_service.reset();
+ 	//异步事件函数
+ 	m_io_service.run();
+
+
+	//run运行之后, 代码会卡在run()这里
+	//socket.async_connect() socket.async_read_some() boost::asio::async_write()等函数内部才会真正的执行,
+	// 执行完成后就会自动调用里面的回调函数,
+ //里面的事件函数执行完之后, 才会结束run(), 此时才会执行run()后面的代码
+
+	//异步通信的机制就是, 如上3个函数,在被调用的时候会先跳过,
+	//并执行后面的其他代码, 只有在run之后, 才会启动如上3个函数,
+	//由m_io_service.run() m_io_service.stop() m_io_service.reset() m_socket.close() 来控制
+	//功能有点像线程的条件变量.
+
+ //注, run()函数只能启动前面的异步事件,
+ 在run之后, 如果还有异步事件, 那么需要reset和run再次使用.
+
+ //注注注注注意了
+	//m_io_service.run(), m_io_service.stop() , m_io_service.reset()只是控制异步服务的事件函数
+	//m_socket.close() 控制通信
+
+ * */
+
+
+
+#ifndef ASYNC_CLIENT_H
+#define ASYNC_CLIENT_H
+
+#include <iostream>
+#include <stdio.h>
+#include <time.h>
+#include <boost/asio.hpp>
+#include <boost/bind.hpp>
+#include <boost/thread.hpp>
+#include <chrono>
+#include <mutex>
+#include <thread>
+#include <atomic>
+#include <string>
+#include <unistd.h>
+#include "glog/logging.h"
+
+#include "error_code/error_code.hpp"
+#include "tool/thread_condition.h"
+#include "tool/binary_buf.h"
+#include "tool/thread_safe_queue.hpp"
+
+
+/**
+ * 异步tcp通信客户端
+ * */
+class Async_Client {
+
+public:
+#define DATA_LENGTH_MAX 50000            //接受消息的缓存长度50000byte, 实际通信数据不能超过它, 如果超过了, 则需要增大
+#define RECONNECTION_WAIT_TIME_MS 1000    //每隔1000ms重连一次
+#define CHECK_WAIT_TIME_MS 75            //每隔75ms检查一次通信, 万集雷达15HZ, 必须要大于一个通信周期, 防止重连之后没有及时刷新消息就又超时断连了
+#define READ_TIMEOUT_MILLISECONDS 5000    //通信数据更新 的超时时间5000ms, 如果数据一直不更新那么就断开然后重连
+    //注:隐藏bug:如果通信正常,但是服务端
+
+    typedef void (*fundata_t)(const char *data, const int len, const void *p);
+
+    typedef void (*Callback_function)(const char *data, const int len, const void *p);
+
+    //通信状态
+    enum Communication_status {//default  = 0
+        E_UNKNOWN = 0,    //未知
+        E_READY = 1,    //正常待机
+        E_DISCONNECT = 2,    //断连
+
+        E_FAULT = 10,    //故障
+    };
+public:
+    // 构造
+    Async_Client();
+
+    // 析构
+    ~Async_Client();
+
+    //初始化, 默认重连
+    Error_manager init(std::string ip, int port, Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
+                       bool is_reconnection_flag = true);
+
+    //反初始化
+    Error_manager uninit();
+
+    //检查状态
+    Error_manager check_status();
+
+    //判断是否正常
+    bool is_ready();
+
+    //获取状态
+    Communication_status get_status();
+
+protected:
+    //线程接受函数，自动接受数据到 mp_data_buf, 然后触发回调函数,
+    void thread_receive();
+
+    //线程检查连接函数，进行连接, 并检查消息是否按时更新, 如果超时, 那么触发重连
+    void thread_check_connect();
+
+    // 开启连接
+    void socket_connect();
+
+    // 关闭连接
+    void socket_close();
+
+    // 重新连接
+    void socket_reconnect();
+
+    // 检查连接
+    void socket_check();
+
+    // 异步写入
+    void client_async_write(char *buf, int len);
+
+    // 异步读取
+    void client_async_read();
+
+    // 异步连接回调
+    void handle_connect(const boost::system::error_code &error);
+
+    //异步读取回调, 失败后重连. bytes_transferred是返回的数据有效长度
+    void handle_read(const boost::system::error_code &error, size_t bytes_transferred);
+
+    // 异步写回调
+    void handle_write(const boost::system::error_code &error);
+
+protected:
+    //状态
+    std::atomic<Communication_status> m_communication_status;    //通信状态
+    std::atomic<bool> m_is_reconnection_flag;        // 断线是否重连的标志位, true:断线自动重连, false:断线不重连, 保持断线状态.
+
+    //通信
+    //把m_io_service作为参数, 构造m_socket, 那么m_io_service.run()可以异步启动m_socket的通信
+    boost::asio::io_service m_io_service;        // 异步控制服务, 负责异步操作, run启动异步服务, stop停止异步服务
+    boost::asio::ip::tcp::socket m_socket;            // socket句柄, 负责tcp通信, 主要是connect,read,write,close
+    //m_socket.async_connect() 传入m_ep, 可以tcp连接 指定的ip和port
+    boost::asio::ip::tcp::endpoint m_ep;                // 连接参数, 主要是ip和port
+
+    //数据
+    char m_data_buf[DATA_LENGTH_MAX];            // 通信消息接受缓存, 默认50000byte
+    std::chrono::system_clock::time_point m_data_updata_time;            // 数据更新时间
+    Thread_safe_queue<Binary_buf *> *mp_communication_data_queue;    //通信数据队列, 内存由上级管理
+
+
+    //接受数据的线程, 自动接受数据到 mp_data_buf, 然后存入队列
+    std::thread *mp_thread_receive;        // 接受线程, 内存由本模块管理
+    Thread_condition m_condition_receive;    //接受线程的条件变量
+    // 检查连接线程, 进行连接, 并检查消息是否按时更新, 如果超时, 那么触发重连
+    std::thread *mp_thread_check_connect;        // 检查连接线程, 内存由本模块管理
+    Thread_condition m_condition_check_connect;        //检查连接线程的条件变量
+    int m_check_connect_wait_time_ms;    // 检查连接线程 的等待时间
+
+};
+
+#endif // ASYNC_CLIENT_H
+
+
+/*
+ * boost::asio::io_service使用时的注意事项：
+
+①请让boost::asio::io_service和boost::asio::io_service::work搭配使用。
+
+②想让event按照进入(strand)时的顺序被执行，需要boost::asio::io_service要和boost::asio::io_service::strand搭配使用。
+
+③一般情况下，io_service的成员函数的使用顺序：
+
+boost::asio::io_service构造，
+boost::asio::io_service::run()，
+boost::asio::io_service::stop()，
+boost::asio::io_service::reset()，
+boost::asio::io_service::run()，
+......
+boost::asio::io_service析构，
+④不论有没有使用io_service::work，run()都会执行完io_service里面的event，(若没有用work，run就会退出)。
+⑤一个新创建的io_service不需要执行reset()函数。
+⑥在调用stop()后，在调用run()之前，请先调用reset()函数。
+⑦函数stop()和reset()并不能清除掉io_service里面尚未执行的event。
+我个人认为，也只有析构掉io_service，才能清空它里面的那些尚未执行的event了。(可以用智能指针)。
+
+⑧函数stop(),stopped(),reset(),很简单，请单步调试，以明白它在函数里做了什么。
+ */

Modules/ClampSafety/lidar/lidarsJsonConfig.cpp

@@ -0,0 +1,238 @@
+/**
+  * @project shutter_verify
+  * @brief   $BRIEF$
+  * @author  lz
+  * @data    2023/4/13
+ **/
+#include "lidarsJsonConfig.h"
+
+#include <utility>
+
+lidarsJsonConfig::lidarsJsonConfig(std::string path) {
+    m_path = path;
+}
+
+bool lidarsJsonConfig::getLidarConfig(int ordinal, lidarsJsonConfig::LidarConfig &lidarconfig) {
+    if (m_config.empty() && !update()) {
+        return false;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return false;
+    }
+    JV_DOUBLE(m_config[ordinal], "angle_min", lidarconfig.angle_min, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal], "angle_max", lidarconfig.angle_max, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal], "angle_increment", lidarconfig.angle_increment, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal], "time_increment", lidarconfig.time_increment, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "minx", lidarconfig.scan_box_limit.minx, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "maxx", lidarconfig.scan_box_limit.maxx, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "miny", lidarconfig.scan_box_limit.miny, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "maxy", lidarconfig.scan_box_limit.maxy, DEFAULT_DOUBLE)
+    JV_INT(m_config[ordinal], "range_min", lidarconfig.range_min, DEFAULT_INT)
+    JV_INT(m_config[ordinal], "range_max", lidarconfig.range_max, DEFAULT_INT)
+    JV_INT(m_config[ordinal]["net_config"], "port", lidarconfig.net_config.port, DEFAULT_INT)
+    JV_INT(m_config[ordinal]["scan_box_limit"], "dist_limit", lidarconfig.scan_box_limit.dist_limit, DEFAULT_INT)
+    JV_STRING(m_config[ordinal]["net_config"], "ip_address", lidarconfig.net_config.ip_address, DEFAULT_STRING)
+    return true;
+}
+
+bool lidarsJsonConfig::getNetConfig(int ordinal, lidarsJsonConfig::NetConfig &net_config) {
+    if (m_config.empty() && !update()) {
+        return false;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return false;
+    }
+
+    JV_INT(m_config[ordinal]["net_config"], "port", net_config.port, DEFAULT_INT)
+    JV_STRING(m_config[ordinal]["net_config"], "ip_address", net_config.ip_address, DEFAULT_STRING)
+
+    return true;
+}
+
+bool lidarsJsonConfig::getScanBoxLimit(int ordinal, lidarsJsonConfig::ScanBoxLimit &scan_box_limit) {
+    if (m_config.empty() && !update()) {
+        return false;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return false;
+    }
+
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "minx", scan_box_limit.minx, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "maxx", scan_box_limit.maxx, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "miny", scan_box_limit.miny, DEFAULT_DOUBLE)
+    JV_DOUBLE(m_config[ordinal]["scan_box_limit"], "maxy", scan_box_limit.maxy, DEFAULT_DOUBLE)
+    JV_INT(m_config[ordinal]["scan_box_limit"], "dist_limit", scan_box_limit.dist_limit, DEFAULT_INT)
+
+    return true;
+}
+
+bool lidarsJsonConfig::update() {
+    Json::Value config;
+    if (!ReadJsonFile(m_path, config)) {
+        return false;
+    }
+
+    for (int i = 0; i < config.size(); i++) {
+        JV_DOUBLE(config[i], "angle_min", m_config[i]["angle_min"], DEFAULT_DOUBLE)
+        JV_DOUBLE(config[i], "angle_max", m_config[i]["angle_max"], DEFAULT_DOUBLE)
+        JV_DOUBLE(config[i], "angle_increment", m_config[i]["angle_increment"], DEFAULT_DOUBLE)
+        JV_DOUBLE(config[i], "time_increment", m_config[i]["time_increment"], DEFAULT_DOUBLE)
+        JV_DOUBLE(config[i]["scan_box_limit"], "minx", m_config[i]["scan_box_limit"]["minx"], DEFAULT_DOUBLE)
+        JV_DOUBLE(config[i]["scan_box_limit"], "maxx", m_config[i]["scan_box_limit"]["maxx"], DEFAULT_DOUBLE)
+        JV_DOUBLE(config[i]["scan_box_limit"], "miny", m_config[i]["scan_box_limit"]["miny"], DEFAULT_DOUBLE)
+        JV_DOUBLE(config[i]["scan_box_limit"], "maxy", m_config[i]["scan_box_limit"]["maxy"], DEFAULT_DOUBLE)
+        JV_INT(config[i], "range_min", m_config[i]["range_min"], DEFAULT_INT)
+        JV_INT(config[i], "range_max", m_config[i]["range_max"], DEFAULT_INT)
+        JV_INT(config[i]["net_config"], "port", m_config[i]["net_config"]["port"], DEFAULT_INT)
+        JV_INT(config[i]["scan_box_limit"], "dist_limit", m_config[i]["scan_box_limit"]["dist_limit"], DEFAULT_INT)
+        JV_STRING(config[i]["net_config"], "ip_address", m_config[i]["net_config"]["ip_address"], DEFAULT_STRING)
+    }
+
+    return true;
+}
+
+double lidarsJsonConfig::angle_min(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["angle_min"].asDouble();
+}
+
+double lidarsJsonConfig::angle_max(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["angle_max"].asDouble();
+}
+
+double lidarsJsonConfig::angle_increment(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["angle_increment"].asDouble();
+}
+
+double lidarsJsonConfig::time_increment(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["time_increment"].asDouble();
+}
+
+int lidarsJsonConfig::range_min(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_INT;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_INT;
+    }
+    return m_config[ordinal]["range_min"].asInt();
+}
+
+int lidarsJsonConfig::range_max(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_INT;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_INT;
+    }
+    return m_config[ordinal]["range_max"].asInt();
+}
+
+std::string lidarsJsonConfig::ip_address(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_STRING;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_STRING;
+    }
+    return m_config[ordinal]["net_config"]["ip_address"].asString();
+}
+
+int lidarsJsonConfig::port(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_INT;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_INT;
+    }
+    return m_config[ordinal]["net_config"]["port"].asInt();
+}
+
+int lidarsJsonConfig::dist_limit(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_INT;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_INT;
+    }
+    return m_config[ordinal]["scan_box_limit"]["dist_limit"].asInt();
+}
+
+double lidarsJsonConfig::minx(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["scan_box_limit"]["minx"].asDouble();
+}
+
+double lidarsJsonConfig::maxx(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["scan_box_limit"]["maxx"].asDouble();
+}
+
+double lidarsJsonConfig::miny(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["scan_box_limit"]["miny"].asDouble();
+}
+
+double lidarsJsonConfig::maxy(int ordinal) {
+    if (m_config.empty() && !update()) {
+        return DEFAULT_DOUBLE;
+    }
+
+    if (m_config.size() <= ordinal) {
+        return DEFAULT_DOUBLE;
+    }
+    return m_config[ordinal]["scan_box_limit"]["maxy"].asDouble();
+}

Modules/ClampSafety/lidar/lidarsJsonConfig.h

@@ -0,0 +1,99 @@
+/**
+  * @project shutter_verify
+  * @brief   $BRIEF$
+  * @author  lz
+  * @data    2023/4/13
+ **/
+#pragma once
+
+#include "json/json.h"
+
+class lidarsJsonConfig {
+public:
+    struct NetConfig {
+        std::string ip_address;
+        int port;
+
+        void info() {
+            printf("---Debug %s %d : ip_address %s .\n", __func__, __LINE__, ip_address.c_str());
+            printf("---Debug %s %d : port %d .\n", __func__, __LINE__, port);
+        }
+    };
+    struct ScanBoxLimit {
+        int dist_limit;
+        double minx;
+        double maxx;
+        double miny;
+        double maxy;
+
+        void info() {
+            printf("---Debug %s %d : minx %f .\n", __func__, __LINE__, minx);
+            printf("---Debug %s %d : maxx %f .\n", __func__, __LINE__, maxx);
+            printf("---Debug %s %d : miny %f .\n", __func__, __LINE__, miny);
+            printf("---Debug %s %d : maxy %f .\n", __func__, __LINE__, maxy);
+        }
+    };
+    struct LidarConfig {
+        double angle_min;
+        double angle_max;
+        double angle_increment;
+        double time_increment;
+        int range_min;
+        int range_max;
+        NetConfig net_config;
+        ScanBoxLimit scan_box_limit;
+
+        void info() {
+            printf("---Debug %s %d : angle_min %f .\n", __func__, __LINE__, angle_min);
+            printf("---Debug %s %d : angle_max %f .\n", __func__, __LINE__, angle_max);
+            printf("---Debug %s %d : angle_increment %f .\n", __func__, __LINE__, angle_increment);
+            printf("---Debug %s %d : time_increment %f .\n", __func__, __LINE__, time_increment);
+            printf("---Debug %s %d : range_min %d .\n", __func__, __LINE__, range_min);
+            printf("---Debug %s %d : range_max %d .\n", __func__, __LINE__, range_max);
+            net_config.info();
+            scan_box_limit.info();
+        }
+    };
+public:
+    explicit lidarsJsonConfig(std::string path);
+
+    ~lidarsJsonConfig() = default;
+
+    bool getLidarConfig(int ordinal, LidarConfig &lidarconfig);
+
+    bool getNetConfig(int ordinal, NetConfig &net_config);
+
+    bool getScanBoxLimit(int ordinal, ScanBoxLimit &scan_box_limit);
+
+    double angle_min(int ordinal);
+
+    double angle_max(int ordinal);
+
+    double angle_increment(int ordinal);
+
+    double time_increment(int ordinal);
+
+    int range_min(int ordinal);
+
+    int range_max(int ordinal);
+
+    std::string ip_address(int ordinal);
+
+    int port(int ordinal);
+
+    int dist_limit(int ordinal);
+
+    double minx(int ordinal);
+
+    double maxx(int ordinal);
+
+    double miny(int ordinal);
+
+    double maxy(int ordinal);
+
+    bool update();
+
+private:
+    std::string m_path;
+    Json::Value m_config;
+};

Modules/ClampSafety/lidar/wanji_716N_device.cpp

@@ -0,0 +1,220 @@
+#include "wanji_716N_device.h"
+
+
+// 万集雷达封装类构造函数
+Wanji_716N_lidar_device::Wanji_716N_lidar_device() {
+    m_wanji_lidar_device_status = E_UNKNOWN;
+}
+
+// 万集雷达封装类析构函数
+Wanji_716N_lidar_device::~Wanji_716N_lidar_device() {
+    uninit();
+}
+
+// 初始化
+Error_manager Wanji_716N_lidar_device::init(std::string t_ip, int t_port,
+                                            Point2D_tool::Polar_coordinates_box t_polar_coordinates_box,
+                                            Point2D_tool::Point2D_box t_point2D_box,
+                                            Point2D_tool::Point2D_transform t_point2D_transform) {
+    Error_manager t_error;
+    LOG(INFO) << " Wanji_716N_lidar_device::init " << t_ip << this;
+
+    //唤醒队列
+    m_communication_data_queue.wake_queue();
+
+
+    t_point2D_transform.m00 = 1;
+    t_point2D_transform.m01 = 0;
+    t_point2D_transform.m02 = 0;
+    t_point2D_transform.m10 = 0;
+    t_point2D_transform.m11 = 1;
+    t_point2D_transform.m12 = 0;
+
+    //初始化通信协议
+    t_error = m_protocol.init(&m_communication_data_queue, t_polar_coordinates_box, t_point2D_box, t_point2D_transform);
+    if (t_error != SUCCESS) {
+        return t_error;
+    }
+
+    //初始化雷达通信模块
+
+    t_error = m_async_client.init(t_ip, t_port, &m_communication_data_queue);
+    if (t_error != SUCCESS) {
+        return t_error;
+    }
+
+
+    //启动 内部线程。默认wait。
+    m_execute_condition.reset(false, false, false);
+    // mp_execute_thread = new std::thread(&Wanji_716N_lidar_device::execute_thread_fun, this);
+
+    m_wanji_lidar_device_status = E_READY;
+
+    return t_error;
+}
+
+//反初始化
+Error_manager Wanji_716N_lidar_device::uninit() {
+//终止队列，防止 wait_and_pop 阻塞线程。
+    m_communication_data_queue.termination_queue();
+
+    //关闭线程
+    if (mp_execute_thread) {
+        m_execute_condition.kill_all();
+    }
+    //回收线程的资源
+    if (mp_execute_thread) {
+        mp_execute_thread->join();
+        delete mp_execute_thread;
+        mp_execute_thread = NULL;
+    }
+
+    m_async_client.uninit();
+    m_protocol.uninit();
+
+//清空队列
+    m_communication_data_queue.clear_and_delete();
+
+    if (m_wanji_lidar_device_status != E_FAULT) {
+        m_wanji_lidar_device_status = E_UNKNOWN;
+    }
+    return Error_code::SUCCESS;
+}
+
+//检查雷达状态,是否正常运行
+Error_manager Wanji_716N_lidar_device::check_status() {
+    updata_status();
+    if (m_wanji_lidar_device_status == E_READY) {
+        return Error_code::SUCCESS;
+    } else if (m_wanji_lidar_device_status == E_BUSY) {
+        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
+                             "  Wanji_716N_lidar_device::check_status() error ");
+    } else {
+        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::check_status() error ");
+    }
+    return Error_code::SUCCESS;
+}
+
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Wanji_716N_lidar_device::is_ready() {
+    updata_status();
+    return m_wanji_lidar_device_status == E_READY;
+}
+
+//外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
+Error_manager
+Wanji_716N_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                                std::chrono::system_clock::time_point command_time, int timeout_ms) {
+    if (p_mutex == NULL || p_cloud.get() == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL ");
+    }
+    //判断是否超时
+    while (std::chrono::system_clock::now() - command_time < std::chrono::milliseconds(timeout_ms)) {
+        //获取指令时间之后的点云, 如果没有就会循环
+        if (m_protocol.get_scan_time() > command_time) {
+            std::unique_lock<std::mutex> lck(*p_mutex);
+            Error_manager code = m_protocol.get_scan_points(p_cloud);
+            return code;
+        }
+        //else 等待下一次数据更新
+
+        //等1ms
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    //超时退出就报错
+    return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
+                         " Wanji_716N_lidar_device::get_new_cloud_and_wait error ");
+}
+
+//外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
+Error_manager
+Wanji_716N_lidar_device::get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                           std::chrono::system_clock::time_point command_time) {
+    if (p_mutex == NULL || p_cloud.get() == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL ");
+    }
+    //获取指令时间之后的点云, 如果没有就会报错, 不会等待
+    if (m_protocol.get_scan_time() > command_time) {
+        std::unique_lock<std::mutex> lck(*p_mutex);
+        Error_manager code = m_protocol.get_scan_points(p_cloud);
+        return code;
+    } else {
+        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::get_current_cloud error ");
+    }
+}
+
+//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+Error_manager Wanji_716N_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                                      std::chrono::system_clock::time_point command_time) {
+    if (p_mutex == NULL || p_cloud.get() == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL ");
+    }
+    int t_scan_cycle_ms = m_protocol.get_scan_cycle();
+    //将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
+    //就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
+    if (m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms)) {
+        std::unique_lock<std::mutex> lck(*p_mutex);
+        Error_manager code = m_protocol.get_scan_points(p_cloud);
+        return code;
+    } else {
+        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::get_last_cloud error ");
+    }
+}
+
+//外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+//注:函数内部不加锁, 由外部统一加锁.
+Error_manager Wanji_716N_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                                      std::chrono::system_clock::time_point command_time) {
+    if (p_cloud.get() == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL ");
+    }
+    int t_scan_cycle_ms = m_protocol.get_scan_cycle();
+    //将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
+    //就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
+    if (m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms * 2)) {
+        Error_manager code = m_protocol.get_scan_points(p_cloud);
+        // LOG(WARNING) << " !!!!!!!!!!!!! wj scan cloud: "<<p_cloud->size();
+        return code;
+    } else {
+        //    LOG(WARNING) << "get cloud failed..... "<<m_protocol.get_scan_time().time_since_epoch().count()<<", "<<command_time.time_since_epoch().count();
+        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::get_last_cloud error ");
+    }
+}
+
+Wanji_716N_lidar_device::Wanji_716N_device_status Wanji_716N_lidar_device::get_status() {
+    updata_status();
+    return m_wanji_lidar_device_status;
+}
+
+void Wanji_716N_lidar_device::updata_status() {
+    Async_Client::Communication_status communication_status = m_async_client.get_status();
+    // Wj_716N_lidar_protocol::Wanji_716N_device_status wanji_lidar_protocol_status = m_protocol.get_status();
+
+    if (communication_status == Async_Client::Communication_status::E_FAULT)// ||
+        //  wanji_lidar_protocol_status == Wj_716N_lidar_protocol::Wanji_716N_device_status::E_FAULT)
+    {
+        m_wanji_lidar_device_status = E_FAULT;
+    }
+    if (communication_status == Async_Client::Communication_status::E_DISCONNECT) {
+        m_wanji_lidar_device_status = E_DISCONNECT;
+    } else if (communication_status == Async_Client::Communication_status::E_READY)// &&
+        //   wanji_lidar_protocol_status == Wj_716N_lidar_protocol::Wanji_716N_device_status::E_READY )
+    {
+        if (m_execute_condition.get_pass_ever()) {
+            m_wanji_lidar_device_status = E_BUSY;
+        } else {
+            m_wanji_lidar_device_status = E_READY;
+        }
+    } else {
+        m_wanji_lidar_device_status = E_UNKNOWN;
+    }
+}
+

Modules/ClampSafety/lidar/wanji_716N_device.h

@@ -0,0 +1,90 @@
+/*
+ * @Author: yct 18202736439@163.com
+ * @Date: 2022-09-22 18:00:51
+ * @LastEditors: yct 18202736439@163.com
+ * @LastEditTime: 2022-09-25 22:10:59
+ * @FilePath: /puai_wj_2021/wanji_lidar/wanji_716N_device.h
+ * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
+ */
+#pragma once
+#include <chrono>
+#include <mutex>
+#include <thread>
+#include <atomic>
+#include <string>
+#include <unistd.h>
+#include <glog/logging.h>
+
+#include "async_client.h"
+#include "wj_716N_lidar_protocol.h"
+
+class Wanji_716N_lidar_device {
+public:
+    //万集设备身状态
+    enum Wanji_716N_device_status {
+        E_UNKNOWN = 0,      //未知
+        E_READY = 1,        //正常待机
+        E_DISCONNECT = 2,   //断连
+        E_BUSY = 3,         //工作正忙
+        E_FAULT = 10,       //故障
+    };
+
+public:
+    Wanji_716N_lidar_device();
+    ~Wanji_716N_lidar_device();
+
+    // 初始化
+    Error_manager init(std::string ip, int port,
+                       Point2D_tool::Polar_coordinates_box t_polar_coordinates_box,
+                       Point2D_tool::Point2D_box t_point2D_box,
+                       Point2D_tool::Point2D_transform t_point2D_transform);
+
+    //反初始化
+    Error_manager uninit();
+
+    //检查雷达状态,是否正常运行
+    Error_manager check_status();
+
+    //判断是否为待机，如果已经准备好，则可以执行任务。
+    bool is_ready();
+
+    //外部调用获取新的点云, 获取指令时间之后的点云, 如果没有就会等待点云刷新, 直到超时, (内置while)
+    Error_manager get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                         std::chrono::system_clock::time_point command_time, int timeout_ms = 1500);
+
+    //外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
+    Error_manager get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                    std::chrono::system_clock::time_point command_time);
+
+    //外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+    Error_manager get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                 std::chrono::system_clock::time_point command_time);
+
+    //外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
+    //注:函数内部不加锁, 由外部统一加锁.
+    Error_manager get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
+                                 std::chrono::system_clock::time_point command_time);
+
+public:
+    Wanji_716N_device_status get_status();
+
+protected:
+    void updata_status();
+
+protected:
+    //状态
+    Wanji_716N_device_status    m_wanji_lidar_device_status;    //万集设备身状态
+
+    //子模块
+    Async_Client            m_async_client;        //异步客户端, 负责雷达通信, 接受雷达数据
+    wj_716N_lidar_protocol  m_protocol;            // 万集雷达协议
+
+    //中间缓存
+    Thread_safe_queue<Binary_buf *> m_communication_data_queue;    //通信数据队列
+
+    //任务执行线程
+    std::thread         *mp_execute_thread;             //执行的线程指针，内存由本类管理
+    Thread_condition    m_execute_condition;            //执行的条件变量
+
+};
+

Modules/ClampSafety/lidar/wj_716N_lidar_protocol.cpp

@@ -0,0 +1,392 @@
+#include "wj_716N_lidar_protocol.h"
+#include <iostream>
+
+
+wj_716N_lidar_protocol::wj_716N_lidar_protocol() {
+    memset(&m_sdata, 0, sizeof(m_sdata));
+    long scan_time = std::chrono::duration_cast<std::chrono::milliseconds>(
+            std::chrono::system_clock::now().time_since_epoch()).count();
+    scan.header.stamp.msecs = scan_time;
+
+    mp_communication_data_queue = nullptr;
+
+    freq_scan = 1;
+    m_u32PreFrameNo = 0;
+    m_u32ExpectedPackageNo = 0;
+    m_n32currentDataNo = 0;
+    total_point = 1081;
+
+    scan.header.frame_id = "wj_716N_lidar_frame";
+    scan.angle_min = -2.35619449;
+    scan.angle_max = 2.35619449;
+    scan.angle_increment = 0.017453 / 4;
+    scan.time_increment = 1 / 15.00000000 / 1440;
+    scan.range_min = 0;
+    scan.range_max = 30;
+    scan.ranges.resize(1081);
+    scan.intensities.resize(1081);
+    scan.x.resize(1081);
+    scan.y.resize(1081);
+    scan.angle_to_point.resize(1081);
+
+    index_start = (config_.min_ang + 2.35619449) / scan.angle_increment;
+    // adjust angle_max to max_ang config param
+    index_end = 1081 - ((2.35619449 - config_.max_ang) / scan.angle_increment);
+
+    mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    mp_thread_analysis = nullptr;
+
+    std::cout << "wj_716N_lidar_protocl start success" << std::endl;
+}
+
+bool wj_716N_lidar_protocol::setConfig(wj_716_lidarConfig &new_config, uint32_t level) {
+
+    config_ = new_config;
+    scan.header.frame_id = config_.frame_id;
+    scan.angle_min = config_.min_ang;
+    scan.angle_max = config_.max_ang;
+    scan.range_min = config_.range_min;
+    scan.range_max = config_.range_max;
+    freq_scan = config_.frequency_scan;
+
+    scan.angle_increment = 0.017453 / 4;
+    if (freq_scan == 1) //0.25°_15hz
+    {
+        scan.time_increment = 1 / 15.00000000 / 1440;
+        total_point = 1081;
+    } else if (freq_scan == 2) //0.25°_25hz
+    {
+        scan.time_increment = 1 / 25.00000000 / 1440;
+        total_point = 1081;
+    }
+
+    // adjust angle_min to min_ang config param
+    index_start = (config_.min_ang + 2.35619449) / scan.angle_increment;
+
+    // adjust angle_max to max_ang config param
+    index_end = 1081 - ((2.35619449 - config_.max_ang) / scan.angle_increment);
+    int samples = index_end - index_start;
+
+    std::cout << "frame_id:" << scan.header.frame_id << std::endl;
+    std::cout << "min_ang:" << scan.angle_min << std::endl;
+    std::cout << "max_ang:" << scan.angle_max << std::endl;
+    std::cout << "angle_increment:" << scan.angle_increment << std::endl;
+    std::cout << "time_increment:" << scan.time_increment << std::endl;
+    std::cout << "range_min:" << scan.range_min << std::endl;
+    std::cout << "range_max:" << scan.range_max << std::endl;
+    std::cout << "samples_per_scan:" << samples << std::endl;
+    return true;
+}
+
+// 初始化
+Error_manager wj_716N_lidar_protocol::init(Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
+                                           Point2D_tool::Polar_coordinates_box polar_coordinates_box,
+                                           Point2D_tool::Point2D_box point2D_box,
+                                           Point2D_tool::Point2D_transform point2D_transform) {
+    if (p_communication_data_queue == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL ");
+    }
+    wj_716_lidarConfig t_config;
+    t_config.min_ang = polar_coordinates_box.angle_min;
+    t_config.max_ang = polar_coordinates_box.angle_max;
+    t_config.range_min = polar_coordinates_box.distance_min;
+    t_config.range_max = polar_coordinates_box.distance_max;
+    t_config.frequency_scan = 1;
+    setConfig(t_config, 0);
+
+    mp_communication_data_queue = p_communication_data_queue;
+    m_point2D_box = point2D_box;
+    std::cout << "box xmin:" << m_point2D_box.x_min << std::endl;
+    std::cout << "box xmax:" << m_point2D_box.x_max << std::endl;
+    std::cout << "box ymin:" << m_point2D_box.y_min << std::endl;
+    std::cout << "box ymax:" << m_point2D_box.y_max << std::endl;
+    m_point2D_transform = point2D_transform;
+    //接受线程, 默认开启循环, 在内部的wait_and_pop进行等待
+    m_condition_analysis.reset(false, true, false);
+    mp_thread_analysis = new std::thread(&wj_716N_lidar_protocol::thread_analysis, this);
+
+    return Error_code::SUCCESS;
+}
+
+//反初始化
+Error_manager wj_716N_lidar_protocol::uninit() {
+    LOG(INFO) << " ---wj_716N_lidar_protocol uninit --- " << this;
+
+    if (mp_communication_data_queue) {
+        //终止队列，防止 wait_and_pop 阻塞线程。
+        mp_communication_data_queue->termination_queue();
+    }
+
+    //关闭线程
+    if (mp_thread_analysis) {
+        m_condition_analysis.kill_all();
+    }
+    //回收线程的资源
+    if (mp_thread_analysis) {
+        mp_thread_analysis->join();
+        delete mp_thread_analysis;
+        mp_thread_analysis = nullptr;
+    }
+
+    if (mp_communication_data_queue) {
+        //清空队列
+        mp_communication_data_queue->clear_and_delete();
+    }
+    //队列的内存由上级管理, 这里写空就好了.
+    mp_communication_data_queue = nullptr;
+
+    return Error_code::SUCCESS;
+}
+
+//获取扫描点云
+Error_manager wj_716N_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out) {
+    if (p_cloud_out.get() == nullptr) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             " Wj_716_lidar_protocol::get_scan_points POINTER IS NULL ");
+    }
+    std::unique_lock<std::mutex> lck(m_scan_mutex);
+    //将扫描点云追加到p_cloud_out后面, 不要清除p_cloud_out原有的数据
+    (*p_cloud_out) += (*mp_scan_points);
+
+    return Error_code::SUCCESS;
+}
+
+//解析线程函数
+void wj_716N_lidar_protocol::thread_analysis() {
+    LOG(INFO) << " ---Wj_716_lidar_protocol::thread_analysis start ---" << this;
+
+    Error_manager t_error;
+    //接受雷达消息，包括连接,重连和接受数据
+    while (m_condition_analysis.is_alive()) {
+        m_condition_analysis.wait();
+        if (m_condition_analysis.is_alive()) {
+            std::this_thread::yield();
+
+            if (mp_communication_data_queue != nullptr) {
+                Binary_buf *tp_binary_buf = nullptr;
+                bool is_pop = mp_communication_data_queue->wait_and_pop(tp_binary_buf);
+                if (is_pop && tp_binary_buf != nullptr) {
+                    if (dataProcess((unsigned char *) tp_binary_buf->get_buf(), tp_binary_buf->get_length())) {
+                        t_error = SUCCESS;
+                    } else {
+                        t_error = ERROR;
+                    }
+                    //else 错误不管, 当做无效消息处理
+                    delete (tp_binary_buf);
+                }
+            }
+        }
+    }
+    LOG(INFO) << " -Wj_716_lidar_protocol::thread_analysis end :" << this;
+}
+
+
+bool wj_716N_lidar_protocol::dataProcess(unsigned char *data, const int reclen) {
+
+    if (reclen > MAX_LENGTH_DATA_PROCESS) {
+        m_sdata.m_u32out = 0;
+        m_sdata.m_u32in = 0;
+        return false;
+    }
+
+    if (m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS) {
+        m_sdata.m_u32out = 0;
+        m_sdata.m_u32in = 0;
+        return false;
+    }
+    memcpy(&m_sdata.m_acdata[m_sdata.m_u32in], data, reclen * sizeof(char));
+    m_sdata.m_u32in += reclen;
+    while (m_sdata.m_u32out < m_sdata.m_u32in) {
+        if (m_sdata.m_acdata[m_sdata.m_u32out] == 0xFF && m_sdata.m_acdata[m_sdata.m_u32out + 1] == 0xAA) {
+            unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 2] << 8) |
+                                    (m_sdata.m_acdata[m_sdata.m_u32out + 3] << 0);
+            l_u32reallen = l_u32reallen + 4;
+
+            if (l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1)) {
+                if (OnRecvProcess(&m_sdata.m_acdata[m_sdata.m_u32out], l_u32reallen)) {
+                    m_sdata.m_u32out += l_u32reallen;
+                } else {
+                    LOG_EVERY_N(WARNING, 10) << "continuous search frame header";
+                    m_sdata.m_u32out++;
+                }
+            } else if (l_u32reallen >= MAX_LENGTH_DATA_PROCESS) {
+                m_sdata.m_u32out++;
+            } else {
+                break;
+            }
+        } else {
+            m_sdata.m_u32out++;
+        }
+    } //end while(m_sdata.m_u32out < m_sdata.m_u32in)
+
+    if (m_sdata.m_u32out >= m_sdata.m_u32in) {
+        m_sdata.m_u32out = 0;
+        m_sdata.m_u32in = 0;
+    } else if (m_sdata.m_u32out < m_sdata.m_u32in && m_sdata.m_u32out != 0) {
+        movedata(m_sdata);
+    }
+    return true;
+}
+
+void wj_716N_lidar_protocol::movedata(DataCache &sdata) {
+    for (int i = sdata.m_u32out; i < sdata.m_u32in; i++) {
+        sdata.m_acdata[i - sdata.m_u32out] = sdata.m_acdata[i];
+    }
+    sdata.m_u32in = sdata.m_u32in - sdata.m_u32out;
+    sdata.m_u32out = 0;
+}
+
+bool wj_716N_lidar_protocol::OnRecvProcess(unsigned char *data, int len) {
+    if (len > 0) {
+        if (checkXor(data, len)) {
+            protocl(data, len);
+        } else {
+            return false;
+        }
+    } else {
+        return false;
+    }
+    return true;
+}
+
+bool wj_716N_lidar_protocol::protocl(unsigned char *data, const int len) {
+
+    if ((data[22] == 0x02 && data[23] == 0x02) || (data[22] == 0x02 && data[23] == 0x01)) //command type:0x02 0x01/0X02
+    {
+
+        heartstate = true;
+        int l_n32TotalPackage = data[80];
+        int l_n32PackageNo = data[81];
+        unsigned int l_u32FrameNo = (data[75] << 24) + (data[76] << 16) + (data[77] << 8) + data[78];
+        int l_n32PointNum = (data[83] << 8) + data[84];
+        int l_n32Frequency = data[79];
+
+        if (l_n32Frequency != freq_scan) {
+            std::cout << "The scan frequency " << l_n32Frequency << " does not match the one you setted " << freq_scan
+                      << " !" << std::endl;
+            return false;
+        }
+
+
+        if (m_u32PreFrameNo != l_u32FrameNo) {
+            m_u32PreFrameNo = l_u32FrameNo;
+            m_u32ExpectedPackageNo = 1;
+            m_n32currentDataNo = 0;
+        }
+
+        if (l_n32PackageNo == m_u32ExpectedPackageNo && m_u32PreFrameNo == l_u32FrameNo) {
+            if (data[82] == 0x00) //Dist
+            {
+                for (int j = 0; j < l_n32PointNum; j++) {
+                    scandata[m_n32currentDataNo] = (((unsigned char) data[85 + j * 2]) << 8) +
+                                                   ((unsigned char) data[86 + j * 2]);
+                    scandata[m_n32currentDataNo] /= 1000.0;
+                    scanintensity[m_n32currentDataNo] = 0;
+                    if (scandata[m_n32currentDataNo] > scan.range_max ||
+                        scandata[m_n32currentDataNo] < scan.range_min || scandata[m_n32currentDataNo] == 0) {
+                        scandata[m_n32currentDataNo] = NAN;
+                    }
+                    m_n32currentDataNo++;
+                }
+                m_u32ExpectedPackageNo++;
+            } else if (data[82] == 0x01 && m_n32currentDataNo >= total_point) //intensities
+            {
+                for (int j = 0; j < l_n32PointNum; j++) {
+                    scanintensity[m_n32currentDataNo - total_point] = (((unsigned char) data[85 + j * 2]) << 8) +
+                                                                      ((unsigned char) data[86 + j * 2]);
+                    m_n32currentDataNo++;
+                }
+                m_u32ExpectedPackageNo++;
+            }
+
+            if (m_u32ExpectedPackageNo - 1 == l_n32TotalPackage) {
+
+                m_read_write_mtx.lock();
+                mp_scan_points->clear();
+                for (int i = index_start; i < index_end; i++) {
+                    scan.ranges[i - index_start] = scandata[i];
+                    //std::cout<<scan.ranges[i - index_start]<<std::endl;
+                    update_data(i - index_start);
+
+                    if (scandata[i - index_start] == NAN) {
+                        scan.intensities[i - index_start] = 0;
+                        continue;
+                    } else {
+                        scan.intensities[i - index_start] = scanintensity[i];
+                    }
+
+                    pcl::PointXYZ point;
+                    //判断平面坐标点是否在限制范围
+                    double x = scan.x[i - index_start];
+                    double y = scan.y[i - index_start];
+                    if (Point2D_tool::limit_with_point2D_box(x, y, &m_point2D_box)) {
+                        //平面坐标的转换, 可以进行旋转和平移, 不能缩放
+                        point.x = x * m_point2D_transform.m00 + y * m_point2D_transform.m01 + m_point2D_transform.m02;
+                        point.y = x * m_point2D_transform.m10 + y * m_point2D_transform.m11 + m_point2D_transform.m12;
+                        //添加到最终点云
+                        mp_scan_points->push_back(point);
+                    }
+                }
+                scan.header.stamp.msecs = std::chrono::duration_cast<std::chrono::milliseconds>(
+                        std::chrono::system_clock::now().time_since_epoch()).count();
+                m_read_write_mtx.unlock();
+
+//                    ros::Time scan_time = ros::Time::now();
+//                    scan.header.stamp = scan_time;
+//                    marker_pub.publish(scan);
+            }
+        }
+        return true;
+    } else {
+        return false;
+    }
+}
+
+//index点数组下标【0～l_n32PointCount-1】 l_n32PointCount总点数
+void wj_716N_lidar_protocol::update_data(int index) {
+    if (index >= total_point) {
+        return;
+    }
+
+//    每个下标对应的弧度
+    scan.angle_to_point[index] =
+            scan.angle_min + (total_point - 1 - index) * (scan.angle_max - scan.angle_min) / (total_point - 1);
+    scan.y[index] = scan.ranges[index] * cos(scan.angle_min +
+                                             (total_point - 1 - index) * (scan.angle_max - scan.angle_min) /
+                                             (total_point - 1));
+    scan.x[index] = scan.ranges[index] * sin(scan.angle_min +
+                                             (total_point - 1 - index) * (scan.angle_max - scan.angle_min) /
+                                             (total_point - 1));
+}
+
+bool wj_716N_lidar_protocol::checkXor(unsigned char *recvbuf, int recvlen) {
+    int i = 0;
+    unsigned char check = 0;
+    unsigned char *p = recvbuf;
+    int len;
+    if (*p == 0xFF) {
+        p = p + 2;
+        len = recvlen - 6;
+        for (i = 0; i < len; i++) {
+            check ^= *p++;
+        }
+        p++;
+        if (check == *p) {
+            return true;
+        } else
+            return false;
+    } else {
+        return false;
+    }
+}
+
+std::chrono::system_clock::time_point wj_716N_lidar_protocol::get_scan_time() const {
+    return std::chrono::time_point<std::chrono::system_clock, std::chrono::milliseconds>(
+            std::chrono::milliseconds(scan.header.stamp.msecs));
+}
+
+int wj_716N_lidar_protocol::get_scan_cycle() const {
+    return (config_.frequency_scan == 1 ? 67 : 40);
+}
+

Modules/ClampSafety/lidar/wj_716N_lidar_protocol.h

@@ -0,0 +1,154 @@
+#pragma once
+
+#include <iostream>
+#include <mutex>
+#include <chrono>
+#include <math.h>
+
+#include "glog/logging.h"
+#include "string.h"
+
+#include "error_code/error_code.hpp"
+#include "tool/binary_buf.h"
+#include "tool/thread_safe_queue.hpp"
+#include "tool/thread_condition.h"
+#include "tool/point2D_tool.h"
+
+#include <boost/shared_ptr.hpp>
+#include <boost/asio.hpp>
+#include <boost/asio/placeholders.hpp>
+#include <boost/system/error_code.hpp>
+#include <boost/bind/bind.hpp>
+
+#include "pcl/point_types.h"
+#include "pcl/point_cloud.h"
+#include "pcl/conversions.h"
+#include "pcl/common/common.h"
+
+typedef struct {
+    std::string frame_id = "laser";
+    double min_ang = -2.35619449;
+    double max_ang = 2.35619449;
+    double range_min = 0.0;
+    double range_max = 30.0;
+    int frequency_scan = 1;
+} wj_716_lidarConfig;
+
+#define MAX_LENGTH_DATA_PROCESS 200000
+typedef struct TagDataCache {
+    unsigned char m_acdata[MAX_LENGTH_DATA_PROCESS];
+    unsigned int m_u32in;
+    unsigned int m_u32out;
+} DataCache;
+
+struct Stamp            //    Laser内参
+{
+    long msecs;
+};
+
+struct Header           //    Laser内参
+{
+    int seq;
+    //替换成只用ms表示
+    Stamp stamp;
+    std::string frame_id;
+};
+
+struct Laser            //    雷达数据及雷达参数
+{
+    Header header;
+    double angle_min;
+    double angle_max;
+    double angle_increment;
+
+    double range_min;
+    double range_max;
+
+    double time_increment;
+    long long scan_time;
+
+    std::vector<double> ranges;
+    std::vector<double> x;
+    std::vector<double> y;
+    std::vector<double> angle_to_point;
+    std::vector<double> intensities;
+};
+
+
+class wj_716N_lidar_protocol {
+public:
+    wj_716N_lidar_protocol();
+
+    // 初始化
+    Error_manager init(Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
+                       Point2D_tool::Polar_coordinates_box polar_coordinates_box,
+                       Point2D_tool::Point2D_box point2D_box,
+                       Point2D_tool::Point2D_transform point2D_transform);
+
+    //反初始化
+    Error_manager uninit();
+
+    //获取扫描周期
+    int get_scan_cycle() const;
+
+    // 获取扫描点云
+    Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
+
+    // 获取扫描点云更新时间点
+    std::chrono::system_clock::time_point get_scan_time() const;
+
+    bool setConfig(wj_716_lidarConfig &new_config, uint32_t level);
+
+    void update_data(int index);
+
+private:
+    //解析线程函数
+    void thread_analysis();
+
+    void movedata(DataCache &sdata);
+
+    bool dataProcess(unsigned char *data, int reclen);
+
+    bool OnRecvProcess(unsigned char *data, int len);
+
+    bool checkXor(unsigned char *recvbuf, int recvlen);
+
+    bool protocl(unsigned char *data, int len);
+
+
+public:
+    Laser scan;
+    int freq_scan;
+    int resizeNum;
+    int index_start;
+    int index_end;
+    bool heartstate;
+    int total_point;
+    std::mutex m_read_write_mtx;            // 点云获取互斥锁
+    std::mutex m_scan_mutex;                // 扫描点的锁
+
+    Thread_safe_queue<Binary_buf *> *mp_communication_data_queue;    //通信数据队列, 内存由上级管理
+
+    // 雷达扫描点平面坐标
+    pcl::PointCloud<pcl::PointXYZ>::Ptr mp_scan_points;                //扫描点的点云, 内存由本类管理
+
+    // 控制参数
+    Point2D_tool::Polar_coordinates_box m_polar_coordinates_box;    //极坐标的限定范围
+    Point2D_tool::Point2D_box m_point2D_box;                        //平面坐标的限定范围
+    Point2D_tool::Point2D_transform m_point2D_transform;            //平面坐标的转换矩阵
+
+    // 解析数据的线程, 自动从队列取出原始数据, 然后解析为点云
+    std::thread *mp_thread_analysis;                // 解析线程, 内存由本模块管理
+    Thread_condition m_condition_analysis;          //解析线程的条件变量
+
+private:
+    DataCache m_sdata;
+    wj_716_lidarConfig config_;
+    unsigned int m_u32PreFrameNo;
+    unsigned int m_u32ExpectedPackageNo;
+    int   m_n32currentDataNo;
+    float scandata[1081];
+    float scanintensity[1081];
+
+};
+

Modules/ClampSafety/main.cpp

@@ -0,0 +1,245 @@
+//
+// Created by zx on 22-8-30.
+//
+#include <cstdio>
+#include <unistd.h>
+#include <cstring>
+#include <fstream>
+#include <byteswap.h>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <google/protobuf/text_format.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+
+#include "detect/clamp_safety_detect.h"
+#include "plc/snap7_clamp.h"
+#include "lidar/wanji_716N_device.h"
+#include "tool/pathcreator.h"
+#include "lidar/lidarsJsonConfig.h"
+
+#if __VIEW__PCL
+pcl::visualization::PCLVisualizer g_viewer;
+#endif
+
+void shut_down_logging(const char *data, size_t size) {
+    time_t tt;
+    time(&tt);
+    tt = tt + 8 * 3600;  // transform the time zone
+    tm *t = gmtime(&tt);
+    char buf[255] = {0};
+    sprintf(buf, "./%d%02d%02d-%02d%02d%02d-dump.txt",
+            t->tm_year + 1900,
+            t->tm_mon + 1,
+            t->tm_mday,
+            t->tm_hour,
+            t->tm_min,
+            t->tm_sec);
+
+    FILE *tp_file = fopen(buf, "w");
+    fprintf(tp_file, data, strlen(data));
+    fclose(tp_file);
+}
+
+void init_glog() {
+    time_t tt = time(nullptr);//时间cuo
+    struct tm *t = localtime(&tt);
+
+    char strYear[255] = {0};
+    char strMonth[255] = {0};
+    char strDay[255] = {0};
+
+    sprintf(strYear, "%04d", t->tm_year + 1900);
+    sprintf(strMonth, "%02d", t->tm_mon + 1);
+    sprintf(strDay, "%02d", t->tm_mday);
+
+    char buf[255] = {0};
+    getcwd(buf, 255);
+    char strdir[255] = {0};
+    sprintf(strdir, "%s/log/%s/%s/%s", buf, strYear, strMonth, strDay);
+    PathCreator creator;
+    creator.Mkdir(strdir);
+
+    char logPath[255] = {0};
+    sprintf(logPath, "%s/", strdir);
+    FLAGS_max_log_size = 100;
+    FLAGS_logbufsecs = 0;
+    google::InitGoogleLogging("clamp_safety");
+    google::SetStderrLogging(google::INFO);
+    google::SetLogDestination(google::INFO, logPath);
+    google::SetLogFilenameExtension("log");
+    google::InstallFailureSignalHandler();
+    google::InstallFailureWriter(&shut_down_logging);
+    FLAGS_colorlogtostderr = true;                  // Set log color
+    FLAGS_logbufsecs = 0;                           // Set log output speed(s)
+    FLAGS_max_log_size = 1024;                      // Set max log file size(GB)
+    FLAGS_stop_logging_if_full_disk = true;
+}
+
+typedef struct {
+    Wanji_716N_lidar_device lidar;
+    clamp_safety_detect detector;
+    Safety_status safety_statu;
+    std::mutex mutex;
+} Clamp;
+
+bool run(Wanji_716N_lidar_device *device, clamp_safety_detect *pdetector, Safety_status *safety) {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    device->get_last_cloud(cloud, std::chrono::system_clock::now());
+
+    if (cloud->size() < 150) {
+        return false;
+    }
+    pdetector->detect(cloud, *safety);
+    return true;
+}
+
+void thread_func(Clamp *clamp) {
+    while (true) {
+        clamp->mutex.lock();
+        Safety_status safety_t;
+        if (run(&(clamp->lidar), &(clamp->detector), &safety_t)) {
+            clamp->safety_statu = safety_t;
+        }
+        clamp->mutex.unlock();
+
+        usleep(100 * 1000);
+    }
+}
+
+int main() {
+    //初始化日志
+    init_glog();
+
+    // 雷达数量
+    const int CLAMP_SIZE = 4;
+    //调试模式下，先打开调试窗口
+#if __VIEW__PCL
+    int v1,v2,v3,v4;
+    //窗口参数分别对应 x_min, y_min, x_max, y_max, viewport
+    g_viewer.createViewPort(0.0, 0.5, 0.5, 1, v1);
+    g_viewer.createViewPort(0.5, 0.5, 1.0, 1, v2);
+    g_viewer.createViewPort(0.0, 0, 0.5, 0.5, v3);
+    g_viewer.createViewPort(0.5, 0, 1.0, 0.5, v4);
+    g_viewer.addText("192.168.0.2", 10,10, 20, 0,1,0, "ip v1", v1);
+    g_viewer.addText("192.168.0.x2", 10,10, 20, 0,1,0, "ip v2", v2);
+    g_viewer.addText("192.168.0.x3", 10,10, 20, 0,1,0, "ip v3", v3);
+    g_viewer.addText("192.168.0.x4", 10,10, 20, 0,1,0, "ip v4", v4);
+
+    g_viewer.addCoordinateSystem(0.2,"v1_axis",v1);
+    g_viewer.addCoordinateSystem(0.2,"v2_axis",v2);
+    g_viewer.addCoordinateSystem(0.2,"v3_axis",v3);
+    g_viewer.addCoordinateSystem(0.2,"v4_axis",v4);
+#endif
+    //雷达
+    Clamp clamps[CLAMP_SIZE];
+
+    lidarsJsonConfig config(ETC_PATH"/etc/clamp_safety_lidars.json");
+    for (int i = 0; i < CLAMP_SIZE; ++i) {
+        lidarsJsonConfig::LidarConfig lidar;
+        config.getLidarConfig(i, lidar);
+        lidar.info();
+        Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;
+        Point2D_tool::Point2D_transform t_point2D_transform;
+
+        t_polar_coordinates_box.angle_min = lidar.angle_min;
+        t_polar_coordinates_box.angle_max = lidar.angle_max;
+        t_polar_coordinates_box.distance_min = lidar.range_min;
+        t_polar_coordinates_box.distance_max = lidar.range_max;
+
+        Point2D_tool::Point2D_box t_point2D_box;
+        t_point2D_box.x_min = lidar.scan_box_limit.minx;
+        t_point2D_box.x_max = lidar.scan_box_limit.maxx;
+        t_point2D_box.y_min = lidar.scan_box_limit.miny;
+        t_point2D_box.y_max = lidar.scan_box_limit.maxy;
+
+        Error_manager code = clamps[i].lidar.init(lidar.net_config.ip_address, lidar.net_config.port,
+                                                  t_polar_coordinates_box, t_point2D_box, t_point2D_transform);
+        if (code != SUCCESS) {
+            LOG(ERROR) << code.get_error_description();
+            return EXIT_FAILURE;
+        }
+        clamps[i].safety_statu.set_timeout(0.3);
+    }
+
+    //调试状态下，绑定识别器输出窗口
+#if __VIEW__PCL
+    if (parameter.lidars_size()>0)
+        clamps[0].detector.set_viewer(&g_viewer,v1);
+    if (parameter.lidars_size()>1)
+        clamps[1].detector.set_viewer(&g_viewer,v2);
+    if (parameter.lidars_size()>2)
+        clamps[2].detector.set_viewer(&g_viewer,v3);
+    if (parameter.lidars_size()>3)
+        clamps[3].detector.set_viewer(&g_viewer,v4);
+
+    while(1)
+    {
+        usleep(100*1000);
+        auto t1=std::chrono::steady_clock::now();
+        //调试状态下，顺序执行，因为viewer不支持多线程
+        if (parameter.lidars_size()>0)
+            run(&(clamps[0].lidar),&(clamps[0].detector),&(clamps[0].safety_statu));
+        if (parameter.lidars_size()>1)
+            run(&(clamps[1].lidar),&(clamps[1].detector),&(clamps[1].safety_statu));
+        if (parameter.lidars_size()>2)
+            run(&(clamps[2].lidar),&(clamps[2].detector),&(clamps[2].safety_statu));
+        if (parameter.lidars_size()>3)
+            run(&(clamps[3].lidar),&(clamps[3].detector),&(clamps[3].safety_statu));
+            auto t2=std::chrono::steady_clock::now();
+            auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+
+            double time= double(duration.count()) * std::chrono::microseconds::period::num /
+                    std::chrono::microseconds::period::den;
+
+        //printf("total time :%.3f s\n",time);
+        g_viewer.spinOnce();
+    }
+#else
+    // 和plc建立连接，一直等待连接成功
+//    auto snap7_client = &Snap7Clamp::get_instance_references();
+//    snap7_client->communication_init();
+//    Snap7Clamp::Snap7_communication_statu status = snap7_client->get_status();
+//    while (status == Snap7Clamp::SNAP7_COMMUNICATION_READY || status == Snap7Clamp::SNAP7_COMMUNICATION_RECEIVE) {
+//        sleep(1);
+//        status = snap7_client->get_status();
+//    }
+
+    // 启动四个线程，分别采集四个雷达的数据。
+    std::thread threads[CLAMP_SIZE];
+    for (int i = 0; i < CLAMP_SIZE; ++i) {
+        threads[i] = std::thread(thread_func, &clamps[i]);
+    }
+
+    int heart = 0;
+    while (true) {
+        usleep(100 * 1000);
+        heart = (heart + 1) % 255;
+//        snap7_client->plcData.pingpong = heart;
+
+        for (int i = 0; i < 4; i++) {
+            std::unique_lock<std::mutex> t_lock(clamps[i].mutex);
+//            std::unique_lock<std::mutex> t_lock1(snap7_client->m_data_lock);
+            if (!clamps[i].safety_statu.is_timeout()) {
+                LOG(WARNING) << "id: " << i << ", we: " << clamps[i].safety_statu.wheel_exist << ", cx: " << clamps[i].safety_statu.cx * 1000;
+//                LOG(INFO) << "orid " << i;
+//                LOG(INFO) << "wheel_exist " << clamps[i].safety_statu.wheel_exist;
+//                LOG(INFO) << "cx " << clamps[i].safety_statu.cx * 1000;
+//                LOG(INFO) << "gap " << clamps[i].safety_statu.gap * 1000;
+//                LOG(INFO) << "clamp_completed " << clamps[i].safety_statu.clamp_completed;
+//                snap7_client->plcData.wheels[i].wheel_exist = clamps[i].safety_statu.wheel_exist;
+////                snap7_client->plcData.wheels[i].offset = heart;
+////                snap7_client->plcData.wheels[i].gap = 255 - heart;
+//                snap7_client->plcData.wheels[i].offset = clamps[i].safety_statu.cx * 1000;
+//                snap7_client->plcData.wheels[i].gap = clamps[i].safety_statu.gap * 1000;
+//                snap7_client->plcData.wheels[i].clamp_completed = clamps[i].safety_statu.clamp_completed;
+            } else {
+//                snap7_client->plcData.clear();
+            }
+        }
+    }
+
+#endif
+
+    return EXIT_SUCCESS;
+}

Modules/ClampSafety/plc/plcJsonConfig.cpp

@@ -0,0 +1,19 @@
+/**
+  * @project shutter_verify
+  * @brief   $BRIEF$
+  * @author  lz
+  * @data    2023/4/13
+ **/
+#include "plcJsonConfig.h"
+#include <utility>
+
+plcJsonConfig::plcJsonConfig(std::string path) {
+    m_path = path;
+}
+
+std::string plcJsonConfig::ip() {
+    std::string ip;
+    ReadJsonFile(m_path, m_config);
+    JV_STRING(m_config, "ip", ip, DEFAULT_STRING);
+    return ip;
+}

Modules/ClampSafety/plc/plcJsonConfig.h

@@ -0,0 +1,21 @@
+/**
+  * @project shutter_verify
+  * @brief   $BRIEF$
+  * @author  lz
+  * @data    2023/4/13
+ **/
+#pragma once
+
+#include "json/json.h"
+
+class plcJsonConfig {
+public:
+    explicit plcJsonConfig(std::string path);
+    ~plcJsonConfig() = default;
+
+    std::string ip();
+
+private:
+    std::string m_path;
+    Json::Value m_config;
+};

Modules/ClampSafety/plc/s7_plc.cpp

@@ -0,0 +1,78 @@
+#include "s7_plc.h"
+
+S7PLC::S7PLC() : bConnected_(false) {
+}
+
+S7PLC::~S7PLC() {
+    disconnect();
+}
+
+bool S7PLC::getConnection() {
+    return bConnected_;
+}
+
+bool S7PLC::connect(std::string ip) {
+    std::lock_guard<std::mutex> lck(mutex_);
+    int ret = client_.ConnectTo(ip.c_str(), 0, 1);
+    bConnected_ = (ret == 0);
+    return bConnected_;
+}
+
+bool S7PLC::ReadShorts(int DBNumber, int start, int size, short *pdata) {
+    short *plc_data = (short *) malloc(size * sizeof(short));
+    bool ret = read_dbs(DBNumber, start * sizeof(short), size * sizeof(short), pdata);
+    if (ret) {
+        reverse_byte(pdata, size * sizeof(short), plc_data);
+        for (int i = 0; i < size; ++i)
+            pdata[i] = plc_data[size - i - 1];
+    }
+    free(plc_data);
+    return ret;
+
+}
+
+bool S7PLC::WriteShorts(int DBNumber, int start, int size, short *pdata) {
+    short *plc_data = (short *) malloc(size * sizeof(short));
+    memcpy(plc_data, pdata, size * sizeof(short));
+    for (int i = 0; i < size; ++i)
+        plc_data[i] = HTON(plc_data[i]);
+
+    bool ret = write_dbs(DBNumber, start * sizeof(short), size * sizeof(short), plc_data);
+    free(plc_data);
+    return ret;
+}
+
+bool S7PLC::read_dbs(int DBNumber, int start, int size, void *pdata) {
+    std::lock_guard<std::mutex> lck(mutex_);
+    usleep(1000 * 50);
+    if (bConnected_ == false)
+        return false;
+
+    int ret = client_.AsDBRead(DBNumber, start, size, pdata);
+
+    return ret == 0;
+}
+
+bool S7PLC::write_dbs(int DBNumber, int start, int size, void *pdata) {
+    std::lock_guard<std::mutex> lck(mutex_);
+    usleep(1000 * 50);
+    if (bConnected_ == false)
+        return false;
+
+    int ret = client_.AsDBWrite(DBNumber, start, size, pdata);
+
+    return ret == 0;
+}
+
+void S7PLC::disconnect() {
+    std::lock_guard<std::mutex> lck(mutex_);
+    client_.Disconnect();
+}
+
+void S7PLC::reverse_byte(void *pdata, int num_byte, void *out) {
+    char *pin = (char *) pdata;
+    char *pout = (char *) out;
+    for (int i = 0; i < num_byte; ++i) {
+        pout[i] = pin[num_byte - i - 1];
+    }
+}

Modules/ClampSafety/plc/s7_plc.h

@@ -0,0 +1,39 @@
+#ifndef S7__PLC__H
+#define S7__PLC__H
+
+#include <s7_client.h>
+#include <mutex>
+#include <iostream>
+
+class S7PLC {
+public:
+#define HTON(T) ((T) << 8) | ((T) >> 8)
+protected:
+    bool bConnected_;
+    std::mutex mutex_;
+    TSnap7Client client_;
+public:
+    S7PLC();
+
+    ~S7PLC();
+
+    bool connect(std::string ip);
+
+    bool getConnection();
+
+    bool ReadShorts(int DBNumber, int start, int size, short *pdata);
+
+    bool WriteShorts(int DBNumber, int start, int size, short *pdata);
+
+    void disconnect();
+
+private:
+    bool read_dbs(int DBNumber, int start, int size, void *pdata);
+
+    bool write_dbs(int DBNumber, int start, int size, void *pdata);
+
+    void reverse_byte(void *pdata, int num_byte, void *out);
+
+};
+
+#endif // !S7__PLC__H

Modules/ClampSafety/plc/snap7_buf.cpp

@@ -0,0 +1,217 @@
+#include "snap7_buf.h"
+#include <string>
+#include <string.h>
+
+Snap7_buf::Snap7_buf() {
+    m_id = 0;
+    m_start_index = 0;
+    m_size = 0;
+    mp_buf_obverse = nullptr;
+    mp_buf_reverse = nullptr;
+    m_communication_mode = NO_COMMUNICATION;
+
+}
+
+Snap7_buf::Snap7_buf(const Snap7_buf &other) {
+    m_id = other.m_id;
+    m_start_index = other.m_start_index;
+    m_communication_mode = other.m_communication_mode;
+
+    if (other.m_size > 0 && other.mp_buf_obverse != nullptr && other.mp_buf_reverse != nullptr) {
+        mp_buf_obverse = (void *) malloc(other.m_size);
+        memcpy(mp_buf_obverse, other.mp_buf_obverse, other.m_size);
+        mp_buf_reverse = (void *) malloc(other.m_size);
+        memcpy(mp_buf_reverse, other.mp_buf_reverse, other.m_size);
+        m_size = other.m_size;
+        m_variable_information_vector = other.m_variable_information_vector;
+    }
+}
+
+Snap7_buf &Snap7_buf::operator=(const Snap7_buf &other) {
+    m_id = other.m_id;
+    m_start_index = other.m_start_index;
+    m_communication_mode = other.m_communication_mode;
+
+    if (other.m_size > 0 && other.mp_buf_obverse != nullptr && other.mp_buf_reverse != nullptr) {
+        mp_buf_obverse = (void *) malloc(other.m_size);
+        memcpy(mp_buf_obverse, other.mp_buf_obverse, other.m_size);
+        mp_buf_reverse = (void *) malloc(other.m_size);
+        memcpy(mp_buf_reverse, other.mp_buf_reverse, other.m_size);
+        m_size = other.m_size;
+        m_variable_information_vector = other.m_variable_information_vector;
+    }
+    return *this;
+}
+
+Snap7_buf::~Snap7_buf() {
+    if (mp_buf_obverse) {
+        free(mp_buf_obverse);
+        mp_buf_obverse = NULL;
+    }
+    if (mp_buf_reverse) {
+        free(mp_buf_reverse);
+        mp_buf_reverse = NULL;
+    }
+}
+
+Snap7_buf::Snap7_buf(int id, int start_index, int size,
+                     std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+                     Communication_mode communication_mode) {
+    m_id = id;
+    m_start_index = start_index;
+    m_communication_mode = communication_mode;
+
+    if (size > 0) {
+        mp_buf_obverse = (void *) malloc(size);
+        memset(mp_buf_obverse, 0, size);
+        mp_buf_reverse = (void *) malloc(size);
+        memset(mp_buf_reverse, 0, size);
+        m_size = size;
+        m_variable_information_vector = variable_information_vector;
+    }
+}
+
+Snap7_buf::Snap7_buf(int id, int start_index, int size, void *p_buf_obverse, void *p_buf_reverse,
+                     std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+                     Communication_mode communication_mode) {
+    m_id = id;
+    m_start_index = start_index;
+    m_communication_mode = communication_mode;
+
+    if (size > 0 && p_buf_obverse != nullptr && p_buf_reverse != nullptr) {
+        mp_buf_obverse = (void *) malloc(size);
+        memcpy(mp_buf_obverse, p_buf_obverse, size);
+        mp_buf_reverse = (void *) malloc(size);
+        memcpy(mp_buf_reverse, p_buf_reverse, size);
+        m_size = size;
+        m_variable_information_vector = variable_information_vector;
+    }
+}
+
+void Snap7_buf::init(int id, int start_index, int size,
+                     std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+                     Communication_mode communication_mode) {
+    m_id = id;
+    m_start_index = start_index;
+    m_communication_mode = communication_mode;
+
+    if (mp_buf_obverse) {
+        free(mp_buf_obverse);
+        mp_buf_obverse = NULL;
+    }
+    if (mp_buf_reverse) {
+        free(mp_buf_reverse);
+        mp_buf_reverse = NULL;
+    }
+
+    if (size > 0) {
+        mp_buf_obverse = (void *) malloc(size);
+        memset(mp_buf_obverse, 0, size);
+        mp_buf_reverse = (void *) malloc(size);
+        memset(mp_buf_reverse, 0, size);
+        m_size = size;
+        m_variable_information_vector = variable_information_vector;
+    }
+}
+
+void Snap7_buf::init(int id, int start_index, int size, void *p_buf_obverse, void *p_buf_reverse,
+                     std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+                     Communication_mode communication_mode) {
+    m_id = id;
+    m_start_index = start_index;
+    m_communication_mode = communication_mode;
+
+    if (mp_buf_obverse) {
+        free(mp_buf_obverse);
+        mp_buf_obverse = NULL;
+    }
+    if (mp_buf_reverse) {
+        free(mp_buf_reverse);
+        mp_buf_reverse = NULL;
+    }
+
+    if (size > 0 && p_buf_obverse != nullptr && p_buf_reverse != nullptr) {
+        mp_buf_obverse = (void *) malloc(size);
+        memcpy(mp_buf_obverse, p_buf_obverse, size);
+        mp_buf_reverse = (void *) malloc(size);
+        memcpy(mp_buf_reverse, p_buf_reverse, size);
+        m_size = size;
+        m_variable_information_vector = variable_information_vector;
+    }
+}
+
+//正序数据 转为 倒序数据
+void Snap7_buf::obverse_to_reverse() {
+    char *p_in = (char *) mp_buf_obverse;
+    char *p_out = (char *) mp_buf_reverse;
+
+    for (auto &iter: m_variable_information_vector) {
+        for (int i = 0; i < iter.m_variable_count; ++i) {
+            for (int j = 0; j < iter.m_variable_size; ++j) {
+                p_out[iter.m_variable_index + iter.m_variable_size * i + j] = p_in[iter.m_variable_index +
+                                                                                   iter.m_variable_size * (i + 1) - j -
+                                                                                   1];
+            }
+        }
+//		for (int i = 0; i < iter.m_variable_count; ++i)
+//		{
+//			for (int j = iter.m_variable_index*i; j < iter.m_variable_index*i+iter.m_variable_size ; ++j)
+//			{
+//				p_out[j] = p_in[iter.m_variable_index*i+iter.m_variable_size - j -1];
+//			}
+//		}
+    }
+}
+
+//倒序数据 转为 正序数据
+void Snap7_buf::reverse_to_obverse() {
+    char *p_in = (char *) mp_buf_reverse;
+    char *p_out = (char *) mp_buf_obverse;
+
+
+    for (auto &iter: m_variable_information_vector) {
+        for (int i = 0; i < iter.m_variable_count; ++i) {
+            for (int j = 0; j < iter.m_variable_size; ++j) {
+                p_out[iter.m_variable_index + iter.m_variable_size * i + j] = p_in[iter.m_variable_index +
+                                                                                   iter.m_variable_size * (i + 1) - j -
+                                                                                   1];
+            }
+        }
+
+//		for (int i = 0; i < iter.m_variable_count; ++i)
+//		{
+//			for (int j = iter.m_variable_index*i; j < iter.m_variable_index*i+iter.m_variable_size ; ++j)
+//			{
+//				p_out[j] = p_in[iter.m_variable_index*i+iter.m_variable_size - j -1];
+//			}
+//		}
+    }
+}
+
+int Snap7_buf::get_id() {
+    return m_id;
+}
+
+int Snap7_buf::get_start_index() {
+    return m_start_index;
+}
+
+int Snap7_buf::get_size() {
+    return m_size;
+}
+
+void *Snap7_buf::get_buf_obverse() {
+    return mp_buf_obverse;
+}
+
+void *Snap7_buf::get_buf_reverse() {
+    return mp_buf_reverse;
+}
+
+Snap7_buf::Communication_mode Snap7_buf::get_communication_mode() {
+    return m_communication_mode;
+}
+
+void Snap7_buf::set_communication_mode(Communication_mode communication_mode) {
+    m_communication_mode = communication_mode;
+}

Modules/ClampSafety/plc/snap7_buf.h

@@ -0,0 +1,98 @@
+#ifndef NNXX_TESTS_SNAP7_BUF_H
+#define NNXX_TESTS_SNAP7_BUF_H
+
+#include <string>
+#include <map>
+#include <vector>
+#include <iostream>
+
+//Snap7协议的数据结构
+class Snap7_buf {
+public:
+    //通信模式
+    enum Communication_mode {
+        NO_COMMUNICATION = 0,    //不通信
+        ONCE_COMMUNICATION = 1,    //一次通信
+        LOOP_COMMUNICATION = 2,    //循环通信
+    };
+
+    //变量信息
+    struct Variable_information {
+        std::string m_variable_name;        //变量名称
+        std::string m_variable_type;        //变量类型, 使用 typeid(a).name() 获取
+        int m_variable_index;        //变量下标, 偏移量
+        int m_variable_size;        //变量类型大小
+        int m_variable_count;        //变量个数
+    };
+public:
+    Snap7_buf();
+
+    Snap7_buf(const Snap7_buf &other);
+
+    Snap7_buf &operator=(const Snap7_buf &other);
+
+    ~Snap7_buf();
+
+public://API functions
+    Snap7_buf(int id, int start_index, int size,
+              std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+              Communication_mode communication_mode = NO_COMMUNICATION);
+
+    Snap7_buf(int id, int start_index, int size, void *p_buf_obverse, void *p_buf_reverse,
+              std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+              Communication_mode communication_mode = NO_COMMUNICATION);
+
+    void init(int id, int start_index, int size,
+              std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+              Communication_mode communication_mode = NO_COMMUNICATION);
+
+    void init(int id, int start_index, int size, void *p_buf_obverse, void *p_buf_reverse,
+              std::vector<Snap7_buf::Variable_information> &variable_information_vector,
+              Communication_mode communication_mode = NO_COMMUNICATION);
+
+
+    //正序数据 转为 倒序数据
+    void obverse_to_reverse();
+
+    //倒序数据 转为 正序数据
+    void reverse_to_obverse();
+
+public://get or set member variable
+    int get_id();
+
+    int get_start_index();
+
+    int get_size();
+
+    void *get_buf_obverse();
+
+    void *get_buf_reverse();
+
+    Communication_mode get_communication_mode();
+
+    void set_communication_mode(Communication_mode communication_mode);
+
+protected://member functions
+
+public://member variable
+
+    int m_id;                    //Snap7协议的数据块的编号
+    int m_start_index;            //Snap7协议的数据起始位下标
+    int m_size;                    //Snap7协议的数据字节大小
+    void *mp_buf_obverse;            //Snap7协议的正序数据指针, 和数据结构体进行强转, 内存由本类管理
+    void *mp_buf_reverse;            //Snap7协议的倒序数据指针, 用作s7通信, 内存由本类管理
+    //注:s7的通信的数据必须要倒序之后才能进行通信,
+
+//	std::map<std::string, Variable_information>			m_variable_information_map;
+    std::vector<Variable_information> m_variable_information_vector;
+
+    Communication_mode m_communication_mode;    //Snap7协议的通信模式
+    //注:s7协议传输很慢, 防止相同的数据重复发送...
+
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_SNAP7_BUF_H

Modules/ClampSafety/plc/snap7_clamp.cpp

@@ -0,0 +1,116 @@
+//
+// Created by huli on 2020/9/25.
+//
+
+#include "snap7_clamp.h"
+
+Snap7Clamp::Snap7Clamp() = default;
+Snap7Clamp::~Snap7Clamp() = default;
+
+//初始化 通信 模块。如下三选一
+Error_manager Snap7Clamp::communication_init()
+{
+	int t_index = 0;
+	std::vector<Snap7_buf::Variable_information>		t_variable_information_vector;
+
+	//往map通信缓存里面添加所需要的buf
+	std::unique_lock<std::mutex> t_lock1(m_receive_buf_lock);
+	std::unique_lock<std::mutex> t_lock2(m_send_buf_lock);
+	Snap7_buf t_snap7_buf;
+
+    // plcData
+    t_index = 0;
+    t_variable_information_vector.clear();
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"pingpong", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"wheel_exist", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"offset", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"gap", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"clamp_completed", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"wheel_exist", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"offset", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"gap", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"clamp_completed", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"wheel_exist", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"offset", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"gap", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"clamp_completed", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"wheel_exist", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"offset", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"gap", typeid(float ).name(), t_index,sizeof(float), 1 });
+    t_index += sizeof(float)*1;
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"clamp_completed", typeid(unsigned short ).name(), t_index,sizeof(unsigned short), 1 });
+    t_index += sizeof(unsigned short)*1;
+
+    LOG(INFO) << t_index;
+
+    t_snap7_buf.init(CLAMP_SAFETY_PLC_DBNUMBER , 0, sizeof(PLCData), t_variable_information_vector, Snap7_buf::LOOP_COMMUNICATION);
+	m_send_buf_map[0] = t_snap7_buf;
+
+//    t_index = 0;
+//    t_variable_information_vector.clear();
+//    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"heart", typeid(unsigned short).name(), t_index,sizeof(unsigned short), 1 });
+//
+//    t_snap7_buf.init(CLAMP_SAFETY_PLC_DBNUMBER , 50, sizeof(unsigned short), t_variable_information_vector, Snap7_buf::LOOP_COMMUNICATION);
+//    m_receive_buf_map[0] = t_snap7_buf;
+
+    plcJsonConfig config(ETC_PATH"/etc/plc.json");
+    return Snap7_communication_base::communication_init(config.ip());
+    return SUCCESS;
+}
+
+//反初始化 通信 模块。
+Error_manager Snap7Clamp::communication_uninit()
+{
+	return Snap7_communication_base::communication_uninit();
+}
+
+//更新数据
+Error_manager Snap7Clamp::updata_receive_buf()
+{
+//	std::unique_lock<std::mutex> t_lock1(m_receive_buf_lock);
+//	std::unique_lock<std::mutex> t_lock2(m_data_lock);
+//    static unsigned short heart;
+//    memcpy(&heart, m_receive_buf_map[0].mp_buf_obverse, m_receive_buf_map[0].m_size);
+
+    Error_manager ret = Error_code::SUCCESS;
+
+//    if (heart == m_heart) {
+//        printf("---Debug %s %d : heart not change %d ---> %d\n", __func__, __LINE__, m_heart, heart);
+//        ret = Error_code::FAILED;
+//    }
+//    m_heart = heart;
+	return ret;
+}
+
+Error_manager Snap7Clamp::updata_send_buf()
+{
+	std::unique_lock<std::mutex> t_lock1(m_send_buf_lock);
+	std::unique_lock<std::mutex> t_lock2(m_data_lock);
+
+    if (plcData.pingpong != 0) {
+        plcData.info();
+    }
+
+	memcpy(m_send_buf_map[0].mp_buf_obverse, &plcData, m_send_buf_map[0].m_size);
+
+    return Error_code::SUCCESS;
+}
+
+
+
+

Modules/ClampSafety/plc/snap7_clamp.h

@@ -0,0 +1,105 @@
+//
+// Created by huli on 2020/9/25.
+//
+#pragma once
+
+#include "tool/singleton.h"
+#include "snap7_communication_base.h"
+#include <glog/logging.h>
+
+class Snap7Clamp:public Singleton<Snap7Clamp>, public Snap7_communication_base
+{
+public:
+	//发送db快的标记位, 保证先发数据再发唯一码
+	enum Send_database_flag
+	{
+		E_SEND_UNKNOW					= 0,
+		E_SEND_DATA_START              	= 1,    //开始发送数据
+		E_SEND_DATA_END              	= 2,    //结束发送数据
+		E_SEND_KEY_START				= 3,	//开始发送唯一码
+		E_SEND_KEY_END					= 4,	//结束发送唯一码
+	};
+
+
+#pragma pack(push, 1)	//struct按照1个byte对齐
+#define CLAMP_SAFETY_HEART_DBNUMBER		9070
+#define CLAMP_SAFETY_PLC_DBNUMBER		9070
+    struct WheeLData {
+        unsigned short wheel_exist;
+        float offset;
+        float gap;
+        unsigned short clamp_completed;
+
+        void info() const {
+            if (wheel_exist == 0) {
+                return;
+            }
+            LOG(INFO) << "wheel_exist = " << wheel_exist
+                       << ", offset = " << offset
+                       << ", gap = " << gap
+                       << ", clamp_completed = " << clamp_completed;
+        }
+
+        void clear() {
+            wheel_exist = false;
+            offset = 0;
+            gap = 0;
+            clamp_completed = 0;
+        }
+    };
+
+    struct PLCData {
+        unsigned short pingpong;
+
+        struct WheeLData wheels[4];
+
+        void info() {
+            LOG(INFO) << "pingdong = " << pingpong;
+            wheels[0].info();
+            wheels[1].info();
+            wheels[2].info();
+            wheels[3].info();
+        }
+
+        void clear() {
+            wheels[0].clear();
+            wheels[1].clear();
+            wheels[2].clear();
+            wheels[3].clear();
+        }
+    };
+#pragma pack(pop)		//取消对齐
+
+    // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<Snap7Clamp>;
+
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+    Snap7Clamp();
+
+public:
+	//必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Snap7Clamp(const Snap7Clamp& other) = delete;
+    Snap7Clamp& operator =(const Snap7Clamp& other) = delete;
+	~Snap7Clamp() override;
+
+public://API functions
+	//初始化 通信 模块。如下三选一
+	Error_manager communication_init();
+	//反初始化 通信 模块。
+	Error_manager communication_uninit() override;
+
+protected://member functions
+	//更新数据
+    Error_manager updata_receive_buf() override;
+    Error_manager updata_send_buf() override;
+
+protected://member variable
+public:
+	std::mutex      m_data_lock;						//数据锁
+
+	PLCData         plcData{};
+    unsigned short            m_heart;
+private:
+
+};

Modules/ClampSafety/plc/snap7_communication_base.cpp

@@ -0,0 +1,308 @@
+//
+// Created by huli on 2020/9/25.
+//
+
+#include "snap7_communication_base.h"
+
+Snap7_communication_base::Snap7_communication_base() {
+    m_communication_status = SNAP7_COMMUNICATION_UNKNOWN;
+    m_communication_delay_time_ms = SNAP7_COMMUNICATION_DELAY_TIME_MS;
+    mp_communication_thread = NULL;
+}
+
+Snap7_communication_base::~Snap7_communication_base() {
+    communication_uninit();
+}
+
+//初始化 通信 模块。如下三选一
+Error_manager Snap7_communication_base::communication_init() {
+    plcJsonConfig config(ETC_PATH"/etc/plc.json");
+    return communication_init(config.ip());
+}
+
+Error_manager Snap7_communication_base::communication_init(std::string ip) {
+    m_ip_string = ip;
+    Error_manager t_error = communication_connect(m_ip_string);
+    if (t_error != Error_code::SUCCESS) {
+        //连接失败, 不要直接返回, 而是改为断连, 后面继续启动线程, (线程内部有重连功能)
+        printf("---Debug %s %d : CONNECT ERROR !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", __func__, __LINE__);
+        m_communication_status = SNAP7_COMMUNICATION_DISCONNECT;
+    } else {
+
+        printf("---Debug %s %d : CONNECT SUCCESS !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n", __func__, __LINE__);
+        m_communication_status = SNAP7_COMMUNICATION_READY;
+    }
+    return communication_run();
+}
+
+//反初始化 通信 模块。
+Error_manager Snap7_communication_base::communication_uninit() {
+    //关闭线程并回收资源
+    if (mp_communication_thread) {
+        m_communication_condition.kill_all();
+    }
+    if (mp_communication_thread) {
+        mp_communication_thread->join();
+        delete mp_communication_thread;
+        mp_communication_thread = NULL;
+    }
+
+    //清空map
+    {
+        std::unique_lock<std::mutex> t_lock(m_receive_buf_lock);
+        m_receive_buf_map.clear();
+    }
+    {
+        std::unique_lock<std::mutex> t_lock(m_send_buf_lock);
+        m_send_buf_map.clear();
+    }
+
+    communication_disconnect();
+    m_communication_status = SNAP7_COMMUNICATION_UNKNOWN;
+
+    return Error_code::SUCCESS;
+}
+
+
+//唤醒s7通信线程
+Error_manager Snap7_communication_base::communication_start() {
+    m_communication_condition.notify_all(true);
+    return Error_code::SUCCESS;
+}
+
+//停止s7通信线程
+Error_manager Snap7_communication_base::communication_stop() {
+    m_communication_condition.notify_all(false);
+    return Error_code::SUCCESS;
+}
+
+
+Snap7_communication_base::Snap7_communication_statu Snap7_communication_base::get_status() {
+    return m_communication_status;
+}
+
+//通信连接
+Error_manager Snap7_communication_base::communication_connect(std::string ip_string) {
+    std::unique_lock<std::mutex> t_lock(m_communication_lock);
+    int result = m_snap7_client.ConnectTo(ip_string.c_str(), 0, 1);
+    std::this_thread::sleep_for(std::chrono::milliseconds(m_communication_delay_time_ms));
+    printf("---Debug %s %d : connect %s result is %d \n", __func__, __LINE__, ip_string.c_str(), result);
+    if (result == 0) {
+        return Error_code::SUCCESS;
+    } else {
+        return Error_manager(Error_code::SNAP7_CONNECT_ERROR, Error_level::MINOR_ERROR,
+                             " Snap7_communication_base::communication_connect error ");
+    }
+}
+
+//启动通信, run thread
+Error_manager Snap7_communication_base::communication_run() {
+    //启动4个线程。
+    //接受线程默认循环, 内部的nn_recv进行等待, 超时1ms
+    m_communication_condition.reset(false, false, false);
+    mp_communication_thread = new std::thread(&Snap7_communication_base::communication_thread, this);
+
+    return Error_code::SUCCESS;
+}
+
+//通信断连
+Error_manager Snap7_communication_base::communication_disconnect() {
+    std::unique_lock<std::mutex> t_lock(m_communication_lock);
+    int result = m_snap7_client.Disconnect();
+    std::this_thread::sleep_for(std::chrono::milliseconds(m_communication_delay_time_ms));
+    if (result == 0) {
+        return Error_code::SUCCESS;
+    } else {
+        return Error_manager(Error_code::SNAP7_DISCONNECT_ERROR, Error_level::MINOR_ERROR,
+                             " Snap7_communication_base::communication_disconnect error ");
+    }
+    return Error_code::SUCCESS;
+}
+
+//mp_communication_thread线程的执行函数, 负责进行s7的通信
+void Snap7_communication_base::communication_thread() {
+    //LOG(INFO) << " ---Snap7_communication_base::communication_thread()--- "<< this;
+    Error_manager t_error;
+    while (m_communication_condition.is_alive()) {
+        m_communication_condition.wait_for_millisecond(1);
+
+        //s7的通信时间较长, 所以将发送和接受分开
+        //发送多个时, 必须加锁后一起发送, 不允许分段写入, 防止数据错误
+        if (m_communication_condition.is_alive()) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(m_communication_delay_time_ms));
+            std::this_thread::yield();
+            switch (m_communication_status) {
+                case SNAP7_COMMUNICATION_READY:
+                case SNAP7_COMMUNICATION_RECEIVE: {
+                    {
+                        std::unique_lock<std::mutex> t_lock(m_receive_buf_lock);
+                        auto iter = m_receive_buf_map.begin();
+                        for (; iter != m_receive_buf_map.end(); ++iter) {
+                            //接受数据, 读取DB块,
+                            t_error = read_data_buf(iter->second);
+                            if (t_error == Error_code::SNAP7_READ_ERROR) {
+                                m_communication_status = SNAP7_COMMUNICATION_DISCONNECT;
+                                printf("---Debug %s %d : t_error = %s \n", __func__, __LINE__, t_error.to_string().c_str());
+                                break;
+                            }
+                        }
+                        if (iter != m_receive_buf_map.end()) {
+                            break;
+                        }
+                    }
+                    //注:数据更新放在锁的外面, 防止重复加锁....
+                    if(updata_receive_buf() != Error_code::SUCCESS) {
+                        m_communication_status = SNAP7_COMMUNICATION_DISCONNECT;
+                    }
+                    else {
+                        m_communication_status = SNAP7_COMMUNICATION_SEND;
+                    }
+                    break;
+                }
+                case SNAP7_COMMUNICATION_SEND: {
+                    //注:数据更新放在锁的外面, 防止重复加锁....
+                    updata_send_buf();
+                    {
+                        std::unique_lock<std::mutex> t_lock(m_send_buf_lock);
+                        auto iter = m_send_buf_map.begin();
+                        for (; iter != m_send_buf_map.end(); ++iter) {
+                            //发送数据, 写入DB块,
+                            t_error = write_data_buf(iter->second);
+                            if (t_error == Error_code::SNAP7_WRITE_ERROR) {
+                                m_communication_status = SNAP7_COMMUNICATION_DISCONNECT;
+                                printf("---Debug %s %d : t_error = %s \n", __func__, __LINE__, t_error.to_string().c_str());
+                                break;
+                            }
+                        }
+                        if (iter != m_send_buf_map.end()) {
+                            break;
+                        }
+                    }
+                    m_communication_status = SNAP7_COMMUNICATION_RECEIVE;
+                    break;
+                }
+                case SNAP7_COMMUNICATION_DISCONNECT: {
+                    //重连
+                    printf("---Debug %s %d : find plc connection error, trying to reconnect.\n", __func__, __LINE__);
+                    communication_disconnect();
+                    std::this_thread::sleep_for(std::chrono::milliseconds(m_communication_delay_time_ms));
+                    t_error = communication_connect(m_ip_string);
+                    if (t_error != Error_code::SUCCESS) {
+                        //连接失败, 不要直接返回, 而是改为断连, 后面继续启动线程, (线程内部有重连功能)
+                        m_communication_status = SNAP7_COMMUNICATION_DISCONNECT;
+                    } else {
+                        m_communication_status = SNAP7_COMMUNICATION_READY;
+                    }
+                    std::this_thread::sleep_for(std::chrono::milliseconds(m_communication_delay_time_ms));
+
+                    break;
+                }
+                default: {
+
+                    break;
+                }
+            }
+        }
+    }
+
+    //LOG(INFO) << " Communication_socket_base::send_data_thread end "<< this;
+    return;
+}
+
+//接受数据, 读取DB块,
+Error_manager Snap7_communication_base::read_data_buf(Snap7_buf &snap7_buf) {
+    Error_manager t_error;
+    if (snap7_buf.m_communication_mode != Snap7_buf::NO_COMMUNICATION) {
+        if (snap7_buf.m_communication_mode == Snap7_buf::ONCE_COMMUNICATION) {
+            snap7_buf.m_communication_mode = Snap7_buf::NO_COMMUNICATION;
+        }
+
+        std::unique_lock<std::mutex> lck(m_communication_lock);
+//        printf("---Debug %s %d : snap7_buf,m_id %d \n", __func__, __LINE__, snap7_buf.m_id);
+//        printf("---Debug %s %d : snap7_buf,m_start_index %d\n", __func__, __LINE__, snap7_buf.m_start_index);
+//        printf("---Debug %s %d : snap7_buf,m_size %d\n", __func__, __LINE__, snap7_buf.m_size);
+        int result = m_snap7_client.AsDBRead(snap7_buf.m_id, snap7_buf.m_start_index, snap7_buf.m_size,
+                                             snap7_buf.mp_buf_reverse);
+
+//		std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        if (result == 0) {
+            m_snap7_client.WaitAsCompletion(100);
+            //倒序数据 转为 正序数据
+            snap7_buf.reverse_to_obverse();
+        } else {
+            std::cout << " huli test :::: " << " result = " << result << std::endl;
+            return Error_manager(Error_code::SNAP7_READ_ERROR, Error_level::MINOR_ERROR,
+                                 " Snap7_communication_base::read_data_buf error ");
+        }
+    }
+
+    return Error_code::SUCCESS;
+}
+
+//发送数据, 写入DB块,
+Error_manager Snap7_communication_base::write_data_buf(Snap7_buf &snap7_buf) {
+    Error_manager t_error;
+    if (snap7_buf.m_communication_mode != Snap7_buf::NO_COMMUNICATION) {
+        if (snap7_buf.m_communication_mode == Snap7_buf::ONCE_COMMUNICATION) {
+            snap7_buf.m_communication_mode = Snap7_buf::NO_COMMUNICATION;
+        }
+
+        //正序数据 转为 倒序数据
+        snap7_buf.obverse_to_reverse();
+        std::unique_lock<std::mutex> lck(m_communication_lock);
+        unsigned short a = 0;
+        memcpy(&a, snap7_buf.mp_buf_reverse, 2);
+        //printf("id %d  start %d  size :%d   value :%d\n",snap7_buf.m_id, snap7_buf.m_start_index, snap7_buf.m_size,a);
+        int result = m_snap7_client.AsDBWrite(snap7_buf.m_id, snap7_buf.m_start_index, snap7_buf.m_size,
+                                              snap7_buf.mp_buf_reverse);
+
+//		std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        if (result == 0) {
+            if (0 != m_snap7_client.WaitAsCompletion(1000)) {
+//                printf("---Debug %s %d : snap7_buf,m_id %d \n", __func__, __LINE__, snap7_buf.m_id);
+//                printf("---Debug %s %d : snap7_buf,m_start_index %d\n", __func__, __LINE__, snap7_buf.m_start_index);
+//                printf("---Debug %s %d : snap7_buf,m_size %d\n", __func__, __LINE__, snap7_buf.m_size);
+//                return Error_manager(Error_code::SNAP7_WRITE_ERROR, Error_level::MINOR_ERROR,
+//                                     " Snap7_communication_base::write_data_buf error ");
+            }
+        } else {
+            printf("---Debug %s %d : snap7_buf,m_id %d \n", __func__, __LINE__, snap7_buf.m_id);
+            printf("---Debug %s %d : snap7_buf,m_start_index %d\n", __func__, __LINE__, snap7_buf.m_start_index);
+            printf("---Debug %s %d : snap7_buf,m_size %d\n", __func__, __LINE__, snap7_buf.m_size);
+            return Error_manager(Error_code::SNAP7_WRITE_ERROR, Error_level::MINOR_ERROR,
+                                 " Snap7_communication_base::write_data_buf error ");
+        }
+    }
+    return Error_code::SUCCESS;
+}
+
+//数据颠倒
+Error_manager Snap7_communication_base::reverse_byte(void *p_buf_in, void *p_buf_out, int size) {
+    if (p_buf_in == NULL || p_buf_out == NULL) {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL ");
+    }
+    char *tp_in = (char *) p_buf_in;
+    char *tp_out = (char *) p_buf_out;
+//	for(int i=0;i<size;++i)
+//	{
+//		tp_out[i]=tp_in[size-i-1];
+//	}
+    for (int i = 0; i < size; ++i) {
+        tp_out[i] = tp_in[i];
+    }
+    return Error_code::SUCCESS;
+}
+
+//更新数据
+Error_manager Snap7_communication_base::updata_receive_buf() {
+    return Error_code::SUCCESS;
+}
+
+Error_manager Snap7_communication_base::updata_send_buf() {
+
+    return Error_code::SUCCESS;
+}
+
+
+

Modules/ClampSafety/plc/snap7_communication_base.h

@@ -0,0 +1,117 @@
+//
+// Created by huli on 2020/9/25.
+//
+
+#ifndef NNXX_TESTS_SNAP7_E_BASE_H
+#define NNXX_TESTS_SNAP7_E_BASE_H
+
+#include <s7_client.h>
+#include <mutex>
+#include <map>
+
+#include "error_code/error_code.hpp"
+#include "tool/thread_condition.h"
+#include "snap7_buf.h"
+#include "s7_plc.h"
+#include "plcJsonConfig.h"
+
+class Snap7_communication_base {
+public:
+    //snap7的通信延时, 默认50ms
+#define SNAP7_COMMUNICATION_DELAY_TIME_MS    10
+//snap7的通信参数路径
+#define SNAP7_COMMUNICATION_PARAMETER_PATH    "../etc/snap7_communication.prototxt"
+    //通信状态
+    enum Snap7_communication_statu {
+        SNAP7_COMMUNICATION_UNKNOWN = 0,            //通信状态 未知
+        SNAP7_COMMUNICATION_READY = 1,            //通信状态 正常
+        SNAP7_COMMUNICATION_RECEIVE = 2,        //接受
+        SNAP7_COMMUNICATION_SEND = 3,        //发送
+        SNAP7_COMMUNICATION_DISCONNECT = 4,        //断连
+        SNAP7_COMMUNICATION_FAULT = 10,         //通信状态 错误
+    };
+protected:
+    Snap7_communication_base();
+
+    Snap7_communication_base(const Snap7_communication_base &other) = default;
+
+    Snap7_communication_base &operator=(const Snap7_communication_base &other) = default;
+
+    ~Snap7_communication_base();
+
+public://API functions
+    //初始化 通信 模块。如下三选一
+    virtual Error_manager communication_init();
+
+    virtual Error_manager communication_init(std::string ip);
+
+    //反初始化 通信 模块。
+    virtual Error_manager communication_uninit();
+
+    //唤醒s7通信线程
+    virtual Error_manager communication_start();
+
+    //停止s7通信线程
+    virtual Error_manager communication_stop();
+
+    //更新数据
+    virtual Error_manager updata_receive_buf();
+
+    virtual Error_manager updata_send_buf();
+
+public://get or set member variable
+    Snap7_communication_statu get_status();
+
+protected://member functions
+    //接受数据, 读取DB块,
+    Error_manager read_data_buf(Snap7_buf &snap7_buf);
+
+    //发送数据, 写入DB块,
+    Error_manager write_data_buf(Snap7_buf &snap7_buf);
+
+    //通信连接
+    Error_manager communication_connect(std::string ip_string);
+
+    //启动通信, run thread
+    Error_manager communication_run();
+
+    //通信断连
+    Error_manager communication_disconnect();
+
+    //mp_communication_thread线程的执行函数, 负责进行s7的通信
+    void communication_thread();
+
+
+    //数据颠倒
+    Error_manager reverse_byte(void *p_buf_in, void *p_buf_out, int size);
+
+
+protected://member variable
+public:
+    //状态
+    Snap7_communication_statu m_communication_status;    //通信状态
+    std::string m_ip_string;            //通信ip
+
+    //通信模块
+    std::mutex m_communication_lock;    //通信锁
+    TSnap7Client m_snap7_client;            //通信的客户端
+    int m_communication_delay_time_ms;//通信延时, 单位ms
+    //注:s7协议通信很不稳定, 在每次使用 TSnap7Client 之后, 都需要加延时
+
+    //数据
+    std::mutex m_receive_buf_lock;        //接受的锁
+    std::map<int, Snap7_buf> m_receive_buf_map;        //接受的map容器
+    std::mutex m_send_buf_lock;        //发送的锁
+    std::map<int, Snap7_buf> m_send_buf_map;        //发送的map容器
+
+    //线程, snap7的通信核心就是对 发送和接受内存的 周期性读写, 所以使用一个线程即可.
+    std::thread *mp_communication_thread;            //通信的线程指针
+    Thread_condition m_communication_condition;            //通信的条件变量
+
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_SNAP7_E_BASE_H

Modules/DataToCloud/CMakeLists.txt

@@ -0,0 +1,44 @@
+if(TEST)
+    add_definitions(-DETC_PATH="${CMAKE_CURRENT_LIST_DIR}")
+endif ()
+# 设置工程名
+set(target_name receive_data)
+
+# 添加文件
+set(RslidarFile
+        rslidar/rslidar_config.hpp
+        rslidar/rslidar_driver.cpp
+        rslidar/rslidar_driver.h
+        rslidar/rslidar_manager.cpp
+        rslidar/rslidar_manager.h
+        rslidar/rslidar_mqtt_async.cpp
+        rslidar/rslidar_mqtt_async.h
+)
+
+# 生成可执行程序
+add_executable(${target_name}
+        ${RslidarFile}
+        lidar_manager.h
+        lidar_manager.cpp
+        main.cpp
+        )
+
+# 添加链接静态库
+target_link_libraries(${target_name}
+        zx
+        ${rs_driver_LIBRARIES}
+        -lpthread
+        )
+
+# 安装
+install(TARGETS ${target_name}
+        LIBRARY DESTINATION lib  # 动态库安装路径
+        ARCHIVE DESTINATION lib  # 静态库安装路径
+        RUNTIME DESTINATION bin  # 可执行文件安装路径
+        PUBLIC_HEADER DESTINATION include  # 头文件安装路径
+        )
+
+install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/etc
+        DESTINATION ${CMAKE_INSTALL_PREFIX}
+        )
+

Modules/DataToCloud/lidar_manager.cpp

@@ -0,0 +1,292 @@
+#include "lidar_manager.h"
+
+#define VIEWER 1
+
+CloudDataManager::CloudDataManager() = default;
+
+CloudDataManager::~CloudDataManager() = default;
+
+void CloudDataManager::receiveClient() {
+    RslidarMqttAsyncClient client;
+    client.init();
+    google::protobuf::RepeatedPtrField<SubscribeEtc> subs = client.getSubscribeEtc();
+    int view_port[4];
+    std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds_xyz;
+    std::map<std::string, pcl::PointCloud<pcl::PointXYZI>::Ptr> clouds_xyzi;
+    for (const auto &sub: subs) {
+        clouds_xyz.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>(sub.topic(),
+                                                                                      new pcl::PointCloud<pcl::PointXYZ>));
+        clouds_xyzi.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZI>::Ptr>(sub.topic(),
+                                                                                        new pcl::PointCloud<pcl::PointXYZI>));
+    }
+    pcl::visualization::PCLVisualizer viewer("viewer_0");
+    viewer.createViewPort(0.0, 0.5, 0.5, 1, view_port[0]);
+    viewer.createViewPort(0.5, 0.5, 1.0, 1, view_port[1]);
+    viewer.createViewPort(0.0, 0, 0.5, 0.5, view_port[2]);
+    viewer.createViewPort(0.5, 0, 1.0, 0.5, view_port[3]);
+    viewer.addText("view_port_1", 10, 10, 20, 0, 1, 0, "view_port_1", view_port[0]);
+    viewer.addText("view_port_2", 10, 10, 20, 0, 1, 0, "view_port_2", view_port[1]);
+    viewer.addText("view_port_3", 10, 10, 20, 0, 1, 0, "view_port_3", view_port[2]);
+    viewer.addText("view_port_4", 10, 10, 20, 0, 1, 0, "view_port_4", view_port[3]);
+    viewer.addCoordinateSystem(1, "v1_axis", view_port[0]);
+    viewer.addCoordinateSystem(1, "v2_axis", view_port[1]);
+    viewer.addCoordinateSystem(1, "v3_axis", view_port[2]);
+    viewer.addCoordinateSystem(1, "v4_axis", view_port[3]);
+    sleep(3);
+
+    while (true) {
+        for (auto iter = clouds_xyz.begin(); iter != clouds_xyz.end(); iter++) {
+            client.getCloudData(iter->first, iter->second);
+        }
+
+        int i = 0;
+        for (const auto &iter: clouds_xyz) {
+            viewer.removeAllPointClouds(view_port[i]);
+            viewer.addPointCloud(iter.second, iter.first, view_port[i++]);
+        }
+        viewer.spinOnce();
+    }
+}
+
+int CloudDataManager::sendClient() {
+    DLOG(INFO) << "\n---Debug testRslidar begain\n";
+//    XYZ2RYRBin("../etc/data/car.txt", "../etc/data/car.bin");
+    RslidarManager lidar_manager;
+    lidar_manager.init();
+    // get lidar
+    if (lidar_manager.getLidarNumber() == 0) {
+        return false;
+    }
+
+    std::vector<int> id_list;
+    lidar_manager.getIdList(id_list);
+    RslidarDevice *lidar[id_list.size()];
+    for (int i = 0; i < id_list.size(); i++) {
+        lidar[i] = lidar_manager.getRslidar(id_list[i]);
+    }
+
+    RslidarMqttAsyncClient mqtt_client;
+    mqtt_client.init();
+    CloudPointPacket *packet = new LidarMessageRsHelios();
+    CloudPointUnpacket unpacket;
+
+    std::vector<unsigned char> data;
+
+    std::map<std::string, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds_xyz;
+    for(auto &l:lidar) {
+        clouds_xyz.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>(LIDAR_SEND_TOPIC + std::to_string(l->id()), new pcl::PointCloud<pcl::PointXYZRGB>));
+    }
+#if VIEWER
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgb_pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+    // 添加显示窗口
+    pcl::PointCloud<pcl::PointALL>::Ptr zx_points(new pcl::PointCloud<pcl::PointALL>);
+    pcl::visualization::PCLVisualizer viewer("viewer_0");
+    viewer.setBackgroundColor(0, 0, 0);
+    viewer.addCoordinateSystem(0.1);
+    double color_list[6][3] = {{255, 0, 0},
+                               {0, 255, 0},
+                               {0, 0, 255},
+                               {255, 255, 0},
+                               {0, 255, 255},
+                               {255, 0, 255}
+    };
+    sleep(1);
+#endif
+
+    auto t = std::chrono::steady_clock::now();
+    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t;
+    while (true) {
+        for (int i = 0; i < lidar_manager.getLidarNumber(); i++) {
+            t = std::chrono::steady_clock::now();
+            lidar_manager.updateRSTransformParam(lidar[i]->id());
+            data.clear();
+//            cost = std::chrono::steady_clock::now() - t;
+//            LOG(INFO) << "before get_last_cloud " << cost.count() * 1000 << " ms.";
+            lidar[i]->get_last_cloud(data);
+//            cost = std::chrono::steady_clock::now() - t;
+//            LOG(INFO) << "after get_last_cloud " << cost.count() * 1000 << " ms.";
+            // 测试代码，直接发的本地数据
+//            GetRYRBin(ETC_PATH"/etc/data/car.bin", data);
+//            usleep(1000 * 25);
+            if (!data.empty()) {
+//                LOG(INFO) << "Get cloud data, data size: " << data.size();
+//                cost = std::chrono::steady_clock::now() - t;
+//                LOG(INFO) << "before packet " << cost.count() * 1000 << " ms.";
+                if (packet->packet(data.data(), data.size())) {
+//                    cost = std::chrono::steady_clock::now() - t;
+//                    LOG(INFO) << "after packet " << cost.count() * 1000 << " ms.";
+//                    DLOG(INFO) << "sned topic: " << LIDAR_SEND_TOPIC + std::to_string(lidar[i]->id());
+                    mqtt_client.SendMessage(LIDAR_SEND_TOPIC + std::to_string(lidar[i]->id()), packet->data(),
+                                            packet->size());
+//                    cost = std::chrono::steady_clock::now() - t;
+//                    LOG(INFO) << "SendMessage " << cost.count() * 1000 << " ms.";
+#if VIEWER
+                    zx_points->clear();
+                    auto iter = clouds_xyz.find(LIDAR_SEND_TOPIC + std::to_string(lidar[i]->id()));
+                    unpacket.unpacket(packet->data(), packet->size(), zx_points);
+//                    if(zx_points->size() > 0) {
+//                        LOG(INFO) << zx_points->begin()->x << " " << zx_points->begin()->y;
+//                    }
+                    mqtt_client.setCloudData(iter->first, zx_points);
+#endif
+                }
+                DLOG(INFO) << "Send cloud data over";
+            }
+        }
+#if VIEWER
+        viewer.removeAllPointClouds();
+        int i = 0;
+        for(auto &iter:clouds_xyz) {
+//            LOG(INFO) << i;
+//            if (iter.second->empty()) {
+//                continue;
+//            }
+            mqtt_client.getCloudData(iter.first, iter.second, color_list[i++]);
+            pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>rgb_color_handler(iter.second);
+            viewer.addPointCloud<pcl::PointXYZRGB>(iter.second, rgb_color_handler, LIDAR_SEND_TOPIC + iter.first);
+//            viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, LIDAR_SEND_TOPIC + iter.first);
+//            viewer.addPointCloud(iter.second, LIDAR_SEND_TOPIC + iter.first);
+        }
+        viewer.spinOnce();
+#endif
+    }
+    return 0;
+}
+
+int CloudDataManager::testRslidar() {
+    RslidarManager lidar_manager;
+    lidar_manager.init();
+    // get lidar
+    if (lidar_manager.getLidarNumber() == 0) {
+        return false;
+    }
+    RslidarDevice *lidar[lidar_manager.getLidarNumber()];
+    for (int i = 0; i < lidar_manager.getLidarNumber(); i++) {
+        lidar[i] = lidar_manager.getRslidar(i);
+    }
+
+    std::vector<unsigned char> data;
+    auto now = std::chrono::steady_clock::now();
+    while (true) {
+        for (int i = 0; i < lidar_manager.getLidarNumber(); i++) {
+            std::chrono::duration<double> diff = std::chrono::steady_clock::now() - now;
+            printf("---Debug time: %fms\n", diff.count() * 1000);
+            lidar[i]->get_last_cloud(data);
+            if (!data.empty()) {
+                LOG(INFO) << "lidar data size" << data.size() << std::endl;
+            }
+        }
+    }
+}
+
+int
+CloudDataManager::test_call_back(MqttAsyncClient *client, char *topicName, int topicLen, MQTTAsync_message *message) {
+    // 将unsigned char数组转换为时间戳（单位：毫秒）
+    uint64_t timestamp = 0;
+    std::memcpy(&timestamp, message->payload, message->payloadlen);
+
+    // 还原为时间点
+    auto time_point = std::chrono::time_point<std::chrono::steady_clock>(std::chrono::milliseconds(timestamp));
+
+    std::chrono::duration<double> diffa = std::chrono::steady_clock::now() - time_point;
+    printf("---Debug time: %fms\n", diffa.count() * 1000);
+
+    return 1;
+}
+
+void CloudDataManager::testClient() {
+    RslidarMqttAsyncClient mqtt_client;
+    mqtt_client.init();
+    auto subs = mqtt_client.getSubscribeEtc();
+
+    // 循环等待消息
+    while (true) {
+        // 获取当前时间点
+        auto now = std::chrono::steady_clock::now();
+
+        // 将时间点转换为时间戳（单位：毫秒）
+        auto timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();
+
+        // 将时间戳转换为unsigned char数组
+        unsigned char buffer[sizeof(timestamp)];
+        for (size_t i = 0; i < sizeof(timestamp); i++) {
+            buffer[i] = static_cast<unsigned char>((timestamp >> (8 * i)) & 0xFF);
+        }
+
+        mqtt_client.SendMessage(subs[0].topic() + "test-b", buffer, sizeof(buffer));
+        usleep(1000 * 50);
+    }
+}
+
+bool CloudDataManager::XYZ2RYRBin(const std::string &file, const std::string &file_bin) {
+    std::vector<unsigned char> save_data;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    ReadTxtCloud(file, cloud);
+    double x, y, z, x_y, r, roll, yaw;
+    int i = 0;
+    DLOG(INFO) << "get cloud number:" << cloud->size();
+    for (auto &point: *cloud) {
+        x = point.x * 100;
+        y = point.y * 100;
+        z = point.z * 100;
+        r = sqrt(x * x + y * y + z * z);
+//        DLOG(INFO) << point.x << " " << point.y << " " << point.z << " " << r;
+        x_y = sqrt(x * x + y * y);
+        roll = atan2(z, x_y);
+        yaw = atan2(x, y);
+        roll = (roll * 18000) / M_PI;
+        yaw = (yaw * 18000) / M_PI;
+        if (roll < 0) {
+            roll += 36000;
+        }
+        if (yaw < 0) {
+            yaw += 36000;
+        }
+        r = r / 0.25;
+//        DLOG(INFO) << roll << " " << yaw << " " << r;
+        save_data.emplace_back((unsigned short) r >> 8);
+        save_data.emplace_back((unsigned short) r);
+        save_data.emplace_back((unsigned short) roll >> 8);
+        save_data.emplace_back((unsigned short) roll);
+        save_data.emplace_back((unsigned short) yaw >> 8);
+        save_data.emplace_back((unsigned short) yaw);
+        save_data.emplace_back((unsigned short) 0);
+
+//        ZX::PointALL pointa{};
+//        double distance, roll_radian, yaw_radian;
+//        distance = (double) ((save_data[i++] << 8 | save_data[i++]) * 0.25) * 0.01;
+//        pointa.roll = (double) ((save_data[i++] << 8 | save_data[i++]) * 0.01);
+//        pointa.yaw = (double) ((save_data[i++] << 8 | save_data[i++]) * 0.01);
+//        pointa.pitch = 0;
+//        roll_radian = pointa.roll * ZX::PI / 180.0;
+//        yaw_radian = pointa.yaw * ZX::PI / 180.0;
+//        pointa.x = distance * cos(roll_radian) * sin(yaw_radian);
+//        pointa.y = distance * cos(roll_radian) * cos(yaw_radian);
+//        pointa.z = distance * sin(roll_radian);
+//        pointa.r3 = distance;
+//        pointa.i = save_data[i++];
+//        DLOG(INFO) << pointa.x << " " << pointa.y << " " << pointa.z << " " << distance;
+//        DLOG(INFO) << pointa.roll << " " << pointa.yaw;
+//        DLOG(INFO) << "===========================";
+    }
+    DLOG(INFO) << "save_data :" << save_data.size();
+    std::fstream bin_file(file_bin, ofstream::trunc | ios::binary | ofstream::out);
+    bin_file.write((const char *) save_data.data(), save_data.size());
+    bin_file.close();
+//    exit(EXIT_SUCCESS);
+    return true;
+}
+
+bool CloudDataManager::GetRYRBin(const std::string &file_bin, std::vector<unsigned char> &data) {
+    std::fstream bin_file(file_bin, ios::binary | ofstream::in);
+    unsigned char uc;
+    while (bin_file.read((char *) &uc, 1)) {
+        data.emplace_back(uc);
+    }
+    bin_file.close();
+    DLOG(INFO) << "data :" << data.size();
+    usleep(1000 * 10);
+    return true;
+}
+
+

Modules/DataToCloud/lidar_manager.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <iostream>
+#include <thread>
+#include <ctime>
+#include "message/lidar_rs_helios.h"
+#include "message/unpacket.hpp"
+#include "tool/static_tool.hpp"
+#include "rslidar/rslidar_mqtt_async.h"
+#include "rslidar/rslidar_manager.h"
+#include <pcl/visualization/pcl_visualizer.h>
+
+// printf("---Debug %s %d\n", __func__, __LINE__);
+
+const std::string LIDAR_SEND_TOPIC = "rslidar/";
+
+class CloudDataManager {
+public:
+    CloudDataManager();
+
+    ~CloudDataManager();
+
+    static void receiveClient();
+
+    static int testRslidar();
+
+    static int sendClient();
+
+    static int test_call_back(MqttAsyncClient *client, char *topicName, int topicLen, MQTTAsync_message *message);
+
+    static void testClient();
+
+private:
+    static bool XYZ2RYRBin(const std::string &file, const std::string &file_bin);
+
+    static bool GetRYRBin(const std::string &file, std::vector<unsigned char> &data);
+
+private:
+};

Modules/DataToCloud/main.cpp

@@ -0,0 +1,33 @@
+#include "lidar_manager.h"
+
+int main(int argc, char **argv) {
+    std::string command;
+    if (argc < 2) {
+        command = "-help";
+    } else {
+        printf("%d %s\n", argc, argv[1]);
+        command = argv[1];
+    }
+
+    if (command == "-s") {
+        // 执行程序1
+        CloudDataManager::sendClient();
+    } else if (command == "-r") {
+        // 执行程序2
+        CloudDataManager::receiveClient();
+    } else if (command == "-t") {
+        // 执行程序3
+        CloudDataManager::sendClient();
+    } else if (command == "-lidar") {
+        // 执行程序3
+        CloudDataManager::testRslidar();
+    } else {
+        std::cerr << "-s 发送雷达数据到客户端" << std::endl;
+        std::cerr << "-r 接收雷达数据并展示" << std::endl;
+        std::cerr << "-t 测试mqtt网络延时" << std::endl;
+        std::cerr << "-lidar 测试雷达数据接收能力" << std::endl;
+        return 1;
+    }
+
+    return 0;
+}

Modules/DataToCloud/rslidar/rslidar_config.hpp

@@ -0,0 +1,159 @@
+#ifndef RSLIDAR_JSON_CONFIG_H_
+#define RSLIDAR_JSON_CONFIG_H_
+
+#include <vector>
+#include "json/json.h"
+#include "rs_driver/driver/driver_param.hpp"
+
+class RsliadrJsonConfig {
+public:
+    RsliadrJsonConfig(std::string &config_path) {
+        m_config_path_ = config_path;
+        m_config_.resize(0);
+    }
+
+    ~RsliadrJsonConfig() {};
+
+    bool getRSDriverParam(robosense::lidar::RSDriverParam &param, int id) {
+        int sn = getSN(id);
+        if (sn == -1) {
+            LOG(INFO) << id << " false";
+            return false;
+        }
+        // TODO json校验
+        param.lidar_type = strToLidarType(m_config_["etc"][sn]["lidar_type"].asString());
+        param.input_type = strToInputType(m_config_["etc"][sn]["input_type"].asString());
+
+        param.input_param.msop_port = id;
+        param.input_param.difop_port = m_config_["etc"][sn]["input_param"]["difop_port"].asUInt();
+        param.input_param.host_address = m_config_["etc"][sn]["input_param"]["host_address"].asString();
+        param.input_param.group_address = m_config_["etc"][sn]["input_param"]["group_address"].asString();
+        param.input_param.pcap_path = m_config_["etc"][sn]["input_param"]["pcap_path"].asString();
+        param.input_param.pcap_rate = m_config_["etc"][sn]["input_param"]["pcap_rate"].asFloat();
+        param.input_param.pcap_repeat = m_config_["etc"][sn]["input_param"]["pcap_repeat"].asBool();
+        param.input_param.use_vlan = m_config_["etc"][sn]["input_param"]["use_vlan"].asBool();
+        param.input_param.user_layer_bytes = m_config_["etc"][sn]["input_param"]["user_layer_bytes"].asUInt();
+        param.input_param.tail_layer_bytes = m_config_["etc"][sn]["input_param"]["tail_layer_bytes"].asUInt();
+
+        param.decoder_param.wait_for_difop = m_config_["etc"][sn]["decoder_param"]["wait_for_difop"].asBool();
+        param.decoder_param.min_distance = m_config_["etc"][sn]["decoder_param"]["min_distance"].asFloat();
+        param.decoder_param.max_distance = m_config_["etc"][sn]["decoder_param"]["max_distance"].asFloat();
+
+        float value = m_config_["etc"][sn]["decoder_param"]["start_angle"].asFloat();
+        if (value < 0 ) {
+            value += 360;
+        } else if (value > 360) {
+            value -= 360;
+        }
+        param.decoder_param.start_angle = value;
+        value = m_config_["etc"][sn]["decoder_param"]["end_angle"].asFloat();
+        if (value < 0 ) {
+            value += 360;
+        } else if (value > 360) {
+            value -= 360;
+        }
+        param.decoder_param.end_angle = value;
+
+        param.decoder_param.use_lidar_clock = m_config_["etc"][sn]["decoder_param"]["use_lidar_clock"].asBool();
+        param.decoder_param.dense_points = m_config_["etc"][sn]["decoder_param"]["dense_points"].asBool();
+        param.decoder_param.config_from_file = m_config_["etc"][sn]["decoder_param"]["config_from_file"].asBool();
+        param.decoder_param.angle_path = m_config_["etc"][sn]["decoder_param"]["angle_path"].asString();
+        param.decoder_param.split_frame_mode = (SplitFrameMode) m_config_["etc"][sn]["decoder_param"]["split_frame_mode"].asInt();
+        param.decoder_param.split_angle = m_config_["etc"][sn]["decoder_param"]["split_angle"].asFloat();
+        param.decoder_param.num_blks_split = m_config_["etc"][sn]["decoder_param"]["num_blks_split"].asUInt();
+
+        getRSTransformParam(param.decoder_param.transform_param, sn);
+
+        return true;
+    }
+
+    bool getRSTransformParam(robosense::lidar::RSTransformParam &param, int id) {
+        int sn = getSN(id);
+        if (sn == -1) {
+            return false;
+        }
+
+        param.x = m_config_["etc"][sn]["transform"]["x"].asFloat();
+        param.y = m_config_["etc"][sn]["transform"]["y"].asFloat();
+        param.z = m_config_["etc"][sn]["transform"]["z"].asFloat();
+        param.roll = m_config_["etc"][sn]["transform"]["roll"].asFloat();
+        param.pitch = m_config_["etc"][sn]["transform"]["pitch"].asFloat();
+        param.yaw = m_config_["etc"][sn]["transform"]["yaw"].asFloat();
+
+        return true;
+    }
+
+    bool getMatrix4dParam(Eigen::Matrix4d &param, int id) {
+        int sn = getSN(id);
+        if (sn == -1) {
+            return false;
+        }
+
+        float r, p, y, cx, cy, cz;
+        r = m_config_["etc"][sn]["calib"]["r"].asFloat();
+        p = m_config_["etc"][sn]["calib"]["p"].asFloat();
+        y = m_config_["etc"][sn]["calib"]["y"].asFloat();
+        cx = m_config_["etc"][sn]["calib"]["cx"].asFloat();
+        cy = m_config_["etc"][sn]["calib"]["cy"].asFloat();
+        cz = m_config_["etc"][sn]["calib"]["cz"].asFloat();
+        Eigen::AngleAxisd rot_x = Eigen::AngleAxisd(r * M_PI / 180.0, Eigen::Vector3d::UnitX());
+        Eigen::AngleAxisd rot_y = Eigen::AngleAxisd(p * M_PI / 180.0, Eigen::Vector3d::UnitY());
+        Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(y * M_PI / 180.0, Eigen::Vector3d::UnitZ());
+        Eigen::Vector3d trans(cx, cy, cz);
+        param << rot_x.toRotationMatrix() * rot_y.toRotationMatrix() *
+                 rot_z.toRotationMatrix(), trans, 0.0, 0.0, 0.0, 1.0;
+
+        return true;
+    }
+
+    bool getLidarIdList(std::vector<int> &list) {
+        if (!ReadJsonFile(m_config_path_, m_config_)) {
+            return false;
+        }
+
+        if (!m_config_.isMember("etc") || !m_config_["etc"].isArray()) {
+            return false;
+        }
+
+        for (int i = 0; i < m_config_["etc"].size(); i++) {
+                if (!m_config_["etc"][i]["enable"].asBool()) {
+                    continue;
+                }
+                list.emplace_back(m_config_["etc"][i]["input_param"]["msop_port"].asUInt());
+        }
+        return true;
+    }
+
+    bool setConfigPath(std::string path) {
+        m_config_path_ = path;
+        m_config_.resize(0);
+        return ReadJsonFile(m_config_path_, m_config_);
+    }
+
+private:
+    int getSN(const int &id) {
+        if (!ReadJsonFile(m_config_path_, m_config_)) {
+            return -1;
+        }
+
+        if (!m_config_.isMember("etc") || !m_config_["etc"].isArray()) {
+            return -1;
+        }
+
+        for (int i = 0; i < m_config_["etc"].size(); i++) {
+            if(m_config_["etc"][i]["input_param"]["msop_port"].asUInt() == id) {
+                if (!m_config_["etc"][i]["enable"].asBool()) {
+                    return -1;
+                }
+                return i;
+            }
+        }
+
+        return -1;
+    }
+private:
+    Json::Value m_config_;
+    std::string m_config_path_;
+};
+
+#endif

Modules/DataToCloud/rslidar/rslidar_driver.cpp

@@ -0,0 +1,338 @@
+#include "rslidar_driver.h"
+
+// 万集雷达封装类构造函数
+RslidarDevice::RslidarDevice() {
+    m_rs_lidar_device_status = E_UNKNOWN;
+    m_decoder_ptr_ = nullptr;
+}
+
+// 万集雷达封装类析构函数
+RslidarDevice::~RslidarDevice() {
+    uninit();
+}
+
+Error_manager RslidarDevice::init() {
+    return SUCCESS;
+}
+
+Error_manager RslidarDevice::init(RSDriverParam &param) {
+    m_lidar_params = param;
+    m_lidar_params.print();
+    m_lidar_driver.regPointCloudCallback(std::bind(&RslidarDevice::driverGetPointCloudFromCallerCallback, this),
+                                         std::bind(&RslidarDevice::driverReturnPointCloudToCallerCallback, this,
+                                                   std::placeholders::_1));
+    m_lidar_driver.regExceptionCallback(std::bind(&RslidarDevice::exceptionCallback, this, std::placeholders::_1));
+    m_lidar_driver.regPacketCallback(std::bind(&RslidarDevice::packetCallback, this, std::placeholders::_1));
+    if (m_lidar_driver.init(param)) {
+        m_decoder_ptr_ = DecoderFactory<PointCloudMsg>::createDecoder(param.lidar_type, param.decoder_param);
+        m_decoder_ptr_->regCallback(&decoderexceptionCallback, &decoderCallback);
+        m_decoder_ptr_->point_cloud_ = m_point_cloud_;
+        m_lidar_driver.start();
+    } else {
+        LOG(INFO) << param.input_param.msop_port << " init falture";
+        return FAILED;
+    }
+//    mp_cloud_handle_thread = new std::thread(std::bind(&RslidarDevice::processCloud, this));
+
+    return SUCCESS;
+}
+
+Error_manager RslidarDevice::uninit() {
+    m_lidar_driver.stop();
+    return Error_code::SUCCESS;
+}
+
+Error_manager RslidarDevice::get_last_cloud(std::vector<unsigned char> &data) {
+    if (m_lidar_data.empty()) {
+        return FAILED;
+    }
+
+    {
+        std::lock_guard<std::mutex> lck(m_cloud_mutex);
+        data.swap(m_lidar_data.front());
+        m_lidar_data.pop_front();
+    }
+    DLOG(INFO) << "M.size " << data.size() / 7 << "  " << m_lidar_data.size();
+
+    return SUCCESS;
+}
+
+bool RslidarDevice::updateRSTransformParam(RSTransformParam &transform_param) {
+    m_lidar_params.decoder_param.transform_param = transform_param;
+    return true;
+}
+
+bool RslidarDevice::is_ready() {
+    update_status();
+    return m_rs_lidar_device_status == E_READY;
+}
+
+RslidarDevice::RSLIDAR_STATUS RslidarDevice::status() {
+    update_status();
+    return m_rs_lidar_device_status;
+}
+
+void RslidarDevice::update_status() {
+    if (m_rs_protocol_status == E_FAULT) {
+        m_rs_lidar_device_status = E_FAULT;
+    } else if (m_rs_protocol_status == E_BUSY) {
+        m_rs_lidar_device_status = E_BUSY;
+    } else if (m_rs_protocol_status == E_READY) {
+        m_rs_lidar_device_status = E_READY;
+    } else {
+        m_rs_lidar_device_status = E_UNKNOWN;
+    }
+}
+
+std::shared_ptr<PointCloudMsg> RslidarDevice::driverGetPointCloudFromCallerCallback(void) {
+    std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
+    if (msg.get() != nullptr) {
+        return msg;
+    }
+    return std::make_shared<PointCloudMsg>();
+}
+
+int RslidarDevice::id() {
+    return m_lidar_params.input_param.msop_port;
+}
+
+void RslidarDevice::driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg) {
+    // 先将之前队列的首个给释放回调函数
+    free_cloud_queue.push(stuffed_cloud_queue.pop());
+
+    // 再存入一个
+    stuffed_cloud_queue.push(msg);
+    DLOG(INFO) << "Get points form driver, point size: " << msg->points.size();
+
+    // 转换
+    if (!msg->points.empty()) {
+        DLOG(INFO) << m_decoder_ptr_->point_cloud_->points.begin()->x << " " << m_decoder_ptr_->point_cloud_->points.begin()->y;
+        Packet packet;
+        int buf_size = msg->points.size();
+        float x, y, z, x_y, r, roll, yaw;
+
+//        Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+//        // 1，定义平移
+//        transform_2.translation() << m_lidar_params.decoder_param.transform_param.x, m_lidar_params.decoder_param.transform_param.y, m_lidar_params.decoder_param.transform_param.z;
+//
+//        // The same rotation matrix as before; theta radians around Z axis
+//        // 2，定义然后Z旋转
+//        transform_2.rotate(Eigen::AngleAxisf(m_lidar_params.decoder_param.transform_param.roll, Eigen::Vector3f::UnitX()));  // UnitZ 即z轴
+//        transform_2.rotate(Eigen::AngleAxisf(m_lidar_params.decoder_param.transform_param.pitch, Eigen::Vector3f::UnitY()));  // UnitZ 即z轴
+//        transform_2.rotate(Eigen::AngleAxisf(m_lidar_params.decoder_param.transform_param.yaw, Eigen::Vector3f::UnitZ()));  // UnitZ 即z轴
+//        // Executing the transformation
+//        // 定义存放变换后的点云对象。
+//        pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZI>());
+//        for (auto &point: msg->points) {
+//            pcl::PointXYZI p;
+//            p.x = point.x;
+//            p.x = point.y;
+//            p.x = point.z;
+//            p.intensity = point.intensity;
+//            transformed_cloud->points.emplace_back(p);
+//        }
+//
+//        // You can either apply transform_1 or transform_2; they are the same
+//        pcl::transformPointCloud(*transformed_cloud, *transformed_cloud, transform_2);
+//
+//        WriteTxtCloud(ETC_PATH"/etc/transformed_cloud.txt", transformed_cloud);
+//        for (auto &point: transformed_cloud->points) {
+//            x = point.x * 100;
+//            y = point.y * 100;
+//            z = point.z * 100;
+//            r = sqrt(x * x + y * y + z * z);
+//            x_y = sqrt(x * x + y * y);
+//            roll = atan2(z, x_y);
+//            yaw = atan2(x, y);
+//            roll = (roll * 18000) / 3.1415926535;
+//            yaw = (yaw * 18000) / 3.1415926535;
+//
+//            if (roll < 0) {
+//                roll += 36000;
+//            }
+//            if (roll > 36000) {
+//                roll -= 36000;
+//            }
+//            if (yaw < 0) {
+//                yaw += 36000;
+//            }
+//            if (yaw > 36000) {
+//                yaw -= 36000;
+//            }
+//            r = r * 4;
+//
+//            m_tmp_data.emplace_back((unsigned short) r >> 8);
+//            m_tmp_data.emplace_back((unsigned short) r);
+//            m_tmp_data.emplace_back((unsigned short) roll >> 8);
+//            m_tmp_data.emplace_back((unsigned short) roll);
+//            m_tmp_data.emplace_back((unsigned short) yaw >> 8);
+//            m_tmp_data.emplace_back((unsigned short) yaw);
+//            m_tmp_data.emplace_back(point.intensity);
+//        }
+        for (int i = 0; i < buf_size; i++) {
+            // roll旋转
+            double angle = (m_lidar_params.decoder_param.transform_param.roll * M_PI) / 180.0f;
+            y = msg->points[i].y * std::cos(angle) - msg->points[i].z * std::sin(angle);
+            z = msg->points[i].y * std::sin(angle) + msg->points[i].z * std::cos(angle);
+            msg->points[i].y = y;
+            msg->points[i].z = z;
+            // pitch旋转
+            angle = (m_lidar_params.decoder_param.transform_param.pitch * M_PI) / 180.0f;
+            x = msg->points[i].x * std::cos(angle) + msg->points[i].z * std::sin(angle);
+            z = msg->points[i].z * std::cos(angle) - msg->points[i].x * std::sin(angle);
+            msg->points[i].x = x;
+            msg->points[i].z = z;
+            // yaw旋转
+            angle = (m_lidar_params.decoder_param.transform_param.yaw * M_PI) / 180.0f;
+            x = msg->points[i].x * std::cos(angle) - msg->points[i].y * std::sin(angle);
+            y = msg->points[i].x * std::sin(angle) + msg->points[i].y * std::cos(angle);
+            msg->points[i].x = x;
+            msg->points[i].y = y;
+            x = x * 100.0f + m_lidar_params.decoder_param.transform_param.x ;
+            y = y * 100.0f + m_lidar_params.decoder_param.transform_param.y;
+            z = z * 100.0f + m_lidar_params.decoder_param.transform_param.z;
+            r = sqrt(x * x + y * y + z * z);
+            x_y = sqrt(x * x + y * y);
+            roll = atan2(z, x_y);
+            yaw = atan2(x, y);
+            roll = (roll * 18000) / 3.1415926535;
+            yaw = (yaw * 18000) / 3.1415926535;
+
+            if (roll < 0) {
+                roll += 36000;
+            }
+            if (roll > 36000) {
+                roll -= 36000;
+            }
+            if (yaw < 0) {
+                yaw += 36000;
+            }
+            if (yaw > 36000) {
+                yaw -= 36000;
+            }
+            r = r * 4;
+
+            m_tmp_data.emplace_back((unsigned short) r >> 8);
+            m_tmp_data.emplace_back((unsigned short) r);
+            m_tmp_data.emplace_back((unsigned short) roll >> 8);
+            m_tmp_data.emplace_back((unsigned short) roll);
+            m_tmp_data.emplace_back((unsigned short) yaw >> 8);
+            m_tmp_data.emplace_back((unsigned short) yaw);
+            m_tmp_data.emplace_back(msg->points[i].intensity);
+        }
+
+//        std::chrono::duration<double> diff = std::chrono::steady_clock::now() - m_capture_time;
+//        if (diff.count() * 1000 > 10) {
+//                int tmp_size = m_tmp_data.size() / 7;
+//                for (int i = tmp_size; i < 2880 * 10; i++) {
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                }
+            m_capture_time = std::chrono::steady_clock::now();
+            std::lock_guard<std::mutex> lck(m_cloud_mutex);
+            DLOG(INFO) << "save cloud data size: " << m_tmp_data.size() / 7;
+            m_lidar_data.emplace_back(m_tmp_data);
+            m_tmp_data.clear();
+            if (m_lidar_data.size() > 100) {
+                m_lidar_data.pop_front();
+            }
+//        }
+    }
+}
+
+void RslidarDevice::exceptionCallback(const Error &code) {
+
+}
+
+void RslidarDevice::packetCallback(const Packet &pkt) {
+    return;
+    if (!pkt.is_difop && m_timestamp != pkt.timestamp) {
+        bool ret = false;
+        m_decoder_ptr_->processMsopPkt(pkt.buf_.data(), pkt.buf_.size());
+        if (m_decoder_ptr_->point_cloud_->points.size() > 0) {
+            DLOG(INFO) << m_decoder_ptr_->point_cloud_->points.begin()->x << " " << m_decoder_ptr_->point_cloud_->points.begin()->y;
+            Packet packet;
+            int buf_size = m_decoder_ptr_->point_cloud_->points.size();
+            float x, y, z, x_y, r, roll, yaw;
+
+            for (int i = 0; i < buf_size; i++) {
+                // yaw旋转
+                double angle = (m_lidar_params.decoder_param.transform_param.yaw * M_PI) / 180.0f;
+                x = m_decoder_ptr_->point_cloud_->points[i].x * std::cos(angle) - m_decoder_ptr_->point_cloud_->points[i].y * std::sin(angle);
+                y = m_decoder_ptr_->point_cloud_->points[i].x * std::sin(angle) + m_decoder_ptr_->point_cloud_->points[i].y * std::cos(angle);
+                // roll旋转
+                angle = (m_lidar_params.decoder_param.transform_param.roll * M_PI) / 180.0f;
+                m_decoder_ptr_->point_cloud_->points[i].y = y;
+                y = m_decoder_ptr_->point_cloud_->points[i].y * std::cos(angle) - m_decoder_ptr_->point_cloud_->points[i].z * std::sin(angle);
+                z = m_decoder_ptr_->point_cloud_->points[i].y * std::sin(angle) + m_decoder_ptr_->point_cloud_->points[i].z * std::cos(angle);
+                // pitch旋转
+                angle = (m_lidar_params.decoder_param.transform_param.pitch * M_PI) / 180.0f;
+                m_decoder_ptr_->point_cloud_->points[i].x = x;
+                m_decoder_ptr_->point_cloud_->points[i].z = z;
+                x = m_decoder_ptr_->point_cloud_->points[i].x * std::cos(angle) - m_decoder_ptr_->point_cloud_->points[i].z * std::sin(angle);
+                z = m_decoder_ptr_->point_cloud_->points[i].x * std::sin(angle) + m_decoder_ptr_->point_cloud_->points[i].z * std::cos(angle);
+                x = x * 100.0f + m_lidar_params.decoder_param.transform_param.x ;
+                y = y * 100.0f + m_lidar_params.decoder_param.transform_param.y;
+                z = z * 100.0f + m_lidar_params.decoder_param.transform_param.z;
+                r = sqrt(x * x + y * y + z * z);
+                x_y = sqrt(x * x + y * y);
+                roll = atan2(z, x_y);
+                yaw = atan2(x, y);
+                roll = (roll * 18000) / 3.1415926535;
+                yaw = (yaw * 18000) / 3.1415926535;
+
+                if (roll < 0) {
+                    roll += 36000;
+                }
+                if (roll > 36000) {
+                    roll -= 36000;
+                }
+                if (yaw < 0) {
+                    yaw += 36000;
+                }
+                if (yaw > 36000) {
+                    yaw -= 36000;
+                }
+                r = r * 4;
+
+                m_tmp_data.emplace_back((unsigned short) r >> 8);
+                m_tmp_data.emplace_back((unsigned short) r);
+                m_tmp_data.emplace_back((unsigned short) roll >> 8);
+                m_tmp_data.emplace_back((unsigned short) roll);
+                m_tmp_data.emplace_back((unsigned short) yaw >> 8);
+                m_tmp_data.emplace_back((unsigned short) yaw);
+                m_tmp_data.emplace_back(m_decoder_ptr_->point_cloud_->points[i].intensity);
+            }
+
+            m_decoder_ptr_->point_cloud_->points.clear();
+            std::chrono::duration<double> diff = std::chrono::steady_clock::now() - m_capture_time;
+            if (diff.count() * 1000 > 10) {
+//                int tmp_size = m_tmp_data.size() / 7;
+//                for (int i = tmp_size; i < 2880 * 10; i++) {
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                    m_tmp_data.emplace_back(0);
+//                }
+                m_capture_time = std::chrono::steady_clock::now();
+                std::lock_guard<std::mutex> lck(m_cloud_mutex);
+                m_lidar_data.emplace_back(m_tmp_data);
+                m_tmp_data.clear();
+                if (m_lidar_data.size() > 100) {
+                    DLOG(INFO) << "pop data";
+                    m_lidar_data.pop_front();
+                }
+            }
+        }
+    } else {
+        m_decoder_ptr_->processDifopPkt(pkt.buf_.data(), pkt.buf_.size());
+    }
+}

Modules/DataToCloud/rslidar/rslidar_driver.h

@@ -0,0 +1,139 @@
+#pragma once
+
+#include <iostream>
+#include <thread>
+#include <mutex>
+#include <chrono>
+#include <functional>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <deque>
+
+#include <glog/logging.h>
+
+#include "rs_driver/api/lidar_driver.hpp"
+#include "rs_driver/driver/decoder/decoder_factory.hpp"
+#include "pcl/common/transforms.h"
+
+#ifdef ENABLE_PCL_POINTCLOUD
+#include <rs_driver/msg/pcl_point_cloud_msg.hpp>
+#else
+
+#include "rs_driver/msg/point_cloud_msg.hpp"
+
+#endif
+
+#include "error_code/error_code.hpp"
+#include "rslidar_mqtt_async.h"
+
+using namespace robosense::lidar;
+
+typedef PointXYZIRT PointT;
+typedef PointCloudT<PointT> PointCloudMsg;
+
+typedef int (*ArrivedCloudPoint)(std::shared_ptr<PointCloudMsg> msg);
+
+class RslidarDevice {
+public:
+    // rslidar status
+    enum RSLIDAR_STATUS {
+        E_UNKNOWN = 0,    //未知
+        E_READY = 1,    //正常待机
+        E_DISCONNECT = 2,    //断连
+        E_BUSY = 3,    //工作正忙
+        E_FAULT = 10,    //故障
+    };
+
+public:
+    // 无参构造函数
+    RslidarDevice();
+
+    // 析构函数
+    ~RslidarDevice();
+
+    // 初始化
+    Error_manager init();
+
+    Error_manager init(RSDriverParam &param);
+
+    Error_manager uninit();
+
+    Error_manager get_last_cloud(std::vector<uint8_t> &data);
+
+    bool updateRSTransformParam(RSTransformParam &transform_param);
+
+    //判断是否为待机，如果已经准备好，则可以执行任务。
+    bool is_ready();
+
+    RSLIDAR_STATUS status();
+
+    int id();
+
+private:
+    void update_status();
+
+    std::shared_ptr<PointCloudMsg> driverGetPointCloudFromCallerCallback(void);
+
+    void driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg);
+
+    void exceptionCallback(const Error &code);
+
+    void packetCallback(const Packet &pkt);
+
+    static void decoderexceptionCallback(const Error &code) {};
+
+    static void decoderCallback(uint16_t v_c, double v_b) {};
+
+    void processCloud() {
+        while (true) {
+            int buf_size = (rand() % 2000 + 1000) * 7 * 3;
+            m_tmp_data.resize(buf_size);
+            memset(m_tmp_data.data(), 0, buf_size);
+            std::chrono::duration<double> diff = std::chrono::steady_clock::now() - m_capture_time;
+            if (diff.count() * 1000 > 10) {
+                m_capture_time = std::chrono::steady_clock::now();
+                std::lock_guard<std::mutex> lck(m_cloud_mutex);
+                m_lidar_data.emplace_back(m_tmp_data);
+                if (m_lidar_data.size() > 100) {
+                    DLOG(INFO) << "pop data";
+                    m_lidar_data.pop_front();
+                }
+            }
+            usleep(1000 * 1);
+        }
+    }
+
+protected:
+    // 测试变量
+    std::thread *mp_cloud_handle_thread;
+    double m_timestamp;
+
+    //状态
+    RSLIDAR_STATUS m_rs_lidar_device_status;    //设备状态
+    RSLIDAR_STATUS m_rs_protocol_status;
+
+    // 雷达配置
+    RSDriverParam m_lidar_params;
+    LidarDriver<PointCloudMsg> m_lidar_driver;               ///< Declare the driver object
+
+    // // 空点云缓存
+    SyncQueue<std::shared_ptr<PointCloudMsg>> free_cloud_queue;
+    SyncQueue<std::shared_ptr<PointCloudMsg>> stuffed_cloud_queue;
+
+    std::mutex m_cloud_mutex;
+    // 完整可获取点云
+    std::vector<PointT> m_ring_cloud;
+    // 获取点云时间
+    std::chrono::steady_clock::time_point m_capture_time;
+
+    // 点云内参齐次矩阵
+    Eigen::Matrix4d m_calib_matrix;
+
+    std::deque<std::vector<unsigned char>> m_lidar_data;
+    std::vector<unsigned char> m_tmp_data;
+    std::string m_topic;
+    std::shared_ptr<robosense::lidar::Decoder<PointCloudMsg>> m_decoder_ptr_;
+    std::shared_ptr<PointCloudMsg> m_point_cloud_ = std::make_shared<PointCloudMsg>();
+};

Modules/DataToCloud/rslidar/rslidar_manager.cpp

@@ -0,0 +1,42 @@
+#include "rslidar_manager.h"
+
+Error_manager RslidarManager::init() {
+    RSDriverParam param;
+    m_config = new RsliadrJsonConfig(m_config_path);
+    std::vector<int> list;
+    m_config->getLidarIdList(list);
+
+    for (auto id:list) {
+        auto *lidar = new RslidarDevice();
+        if (m_config->getRSDriverParam(param, id)) {
+            if (lidar->init(param) == SUCCESS) {
+                m_lidars_map_.insert(std::pair<int, RslidarDevice*>(id, lidar));
+                LOG(INFO) << id;
+            }
+        }
+    }
+
+    return SUCCESS;
+}
+
+size_t RslidarManager::getLidarNumber() {
+    return m_lidars_map_.size();
+}
+
+bool RslidarManager::getIdList(std::vector<int> &list) {
+    return m_config->getLidarIdList(list);
+}
+
+bool RslidarManager::updateRSTransformParam(int id) {
+    RSTransformParam param;
+    if(!m_config->getRSTransformParam(param, id)) {
+        return false;
+    }
+
+    m_lidars_map_.at(id)->updateRSTransformParam(param);
+    return true;
+}
+
+RslidarDevice *RslidarManager::getRslidar(int id) {
+    return m_lidars_map_.at(id);
+}

Modules/DataToCloud/rslidar/rslidar_manager.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include "rslidar_driver.h"
+#include "rslidar_config.hpp"
+#include <vector>
+
+class RslidarManager {
+private:
+    /* data */
+public:
+    RslidarManager() = default;
+
+    ~RslidarManager() = default;
+
+    Error_manager init();
+
+    size_t getLidarNumber();
+
+    bool getIdList(std::vector<int> &list);
+
+    bool updateRSTransformParam();
+
+    bool updateRSTransformParam(int id);
+
+    RslidarDevice *getRslidar(int id);
+
+private:
+    std::string m_config_path = "../etc/relidar.json";
+    std::map<int, RslidarDevice *> m_lidars_map_;
+    RsliadrJsonConfig *m_config;
+};

Modules/DataToCloud/rslidar/rslidar_mqtt_async.cpp

@@ -0,0 +1,107 @@
+#include "rslidar_mqtt_async.h"
+
+void RslidarMqttAsyncClient::init() {
+    m_message_arrived_callback_ = CloudDataArrived;
+
+    MqttAsyncClient::init_from_proto("../etc/rslidar_mqtt_async.json");
+}
+
+bool RslidarMqttAsyncClient::setCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
+    if (cloud->empty()) {
+        printf("---Debug cloud size %ld\n", cloud->size());
+        return false;
+    }
+
+//    auto iter_cloud = m_cloud_.find(topic);
+    auto iter_full_cloud = m_full_cloud_.find(topic);
+//    auto iter_time = m_topic_time_.find(topic);
+    if (iter_full_cloud == m_full_cloud_.end()) {
+//        m_cloud_.insert(std::pair<std::string, pcl::PointCloud<pcl::PointALL>::Ptr>(topic,new pcl::PointCloud<pcl::PointALL>));
+        m_full_cloud_.insert(std::pair<std::string, pcl::PointCloud<pcl::PointALL>::Ptr>(topic,new pcl::PointCloud<pcl::PointALL>));
+//        m_topic_time_.insert(std::pair<std::string, std::chrono::steady_clock::time_point>(topic, std::chrono::steady_clock::now()));
+//        iter_cloud = m_cloud_.find(topic);
+        iter_full_cloud = m_full_cloud_.find(topic);
+//        iter_time = m_topic_time_.find(topic);
+    }
+
+//    std::chrono::duration<double> diff = std::chrono::steady_clock::now() - iter_time->second;
+//    if (diff.count() * 1000 > 100) {
+//        iter_time->second = std::chrono::steady_clock::now();
+//        iter_full_cloud->second->swap(*iter_cloud->second);
+//        iter_cloud->second->clear();
+//    }
+//
+//    iter_cloud->second->insert(iter_cloud->second->end(), cloud->begin(), cloud->end());
+//    LOG(INFO) << topic << "   " << iter_full_cloud->first;
+//    iter_full_cloud->second->points.insert(iter_full_cloud->second->end(), cloud->points.begin(), cloud->points.end());
+    {
+        std::lock_guard<std::mutex> lck(m_cloud_mutex);
+        iter_full_cloud->second->swap(*cloud);
+    }
+//    iter_cloud->second->clear();
+
+    return true;
+}
+
+bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter = m_full_cloud_.find(topic);
+    if (iter == m_full_cloud_.end()) {
+        LOG(WARNING) << topic << " can't find data.";
+        return false;
+    }
+
+    cloud->clear();
+    pcl::PointXYZI cloud_p;
+    for (auto &point:*iter->second) {
+        cloud_p.x = point.x;
+        cloud_p.y = point.y;
+        cloud_p.x = point.z;
+        cloud_p.intensity = point.intensity;
+        cloud->points.emplace_back(cloud_p);
+    }
+    return true;
+}
+
+bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter = m_full_cloud_.find(topic);
+    if (iter == m_full_cloud_.end()) {
+        return false;
+    }
+    cloud->clear();
+    for (auto &point: *iter->second) {
+        cloud->points.emplace_back(point.x, point.y, point.z);
+    }
+    return true;
+}
+
+bool RslidarMqttAsyncClient::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, double *color) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter = m_full_cloud_.find(topic);
+    if (iter == m_full_cloud_.end()) {
+        return false;
+    }
+    cloud->clear();
+    pcl::PointXYZRGB rgb;
+    for (auto &point: *iter->second) {
+        rgb.x = point.x;
+        rgb.y = point.y;
+        rgb.z = point.z;
+        rgb.r = color[0];
+        rgb.g = color[1];
+        rgb.b = color[2];
+        cloud->points.emplace_back(rgb);
+    }
+    return true;
+}
+
+int RslidarMqttAsyncClient::CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen,
+                                             MQTTAsync_message *message) {
+    pcl::PointCloud<pcl::PointALL>::Ptr cloud(new pcl::PointCloud<pcl::PointALL>);
+    CloudPointUnpacket unpacket;
+    unpacket.unpacket((unsigned char *) message->payload, message->payloadlen, cloud);
+    RslidarMqttAsyncClient *t_client = (RslidarMqttAsyncClient *) client;
+    t_client->setCloudData(topicName, cloud);
+    return 1;
+}

Modules/DataToCloud/rslidar/rslidar_mqtt_async.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "pahoc/mqtt_async.h"
+#include "message/unpacket.hpp"
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+class RslidarMqttAsyncClient : public MqttAsyncClient {
+public:
+    RslidarMqttAsyncClient() = default;
+
+    void init();
+
+    bool setCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, double *color);
+
+private:
+    static int CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen, MQTTAsync_message *message);
+
+public:
+
+private:
+    std::mutex m_cloud_mutex;
+    std::map<std::string, int> m_topics_;
+    std::map<std::string, pcl::PointCloud<pcl::PointALL>::Ptr> m_cloud_;
+    std::map<std::string, pcl::PointCloud<pcl::PointALL>::Ptr> m_full_cloud_;
+    std::map<std::string, std::chrono::steady_clock::time_point> m_topic_time_;
+};

Modules/LidarICP/CMakeLists.txt

@@ -0,0 +1,43 @@
+if(TEST)
+    add_definitions(-DETC_PATH="${CMAKE_CURRENT_LIST_DIR}")
+endif ()
+
+# 定义cmake的最低版本
+# 定义工程名称
+set(Project_Module
+        LidarICP
+        )
+
+# 将./src 下的所有文件名保存到DIR_SRCS变量
+set (Project_Module_Source
+        ${CMAKE_CURRENT_LIST_DIR}/main.cpp
+        ${CMAKE_CURRENT_LIST_DIR}/rslidar_mqtt_async.cpp
+        ${CMAKE_CURRENT_LIST_DIR}/rslidar_mqtt_async.h
+        )
+
+set (Project_Module_Libary
+        zx
+        )
+
+# 编译时打印
+message("-- add " ${Project_Module_Source})
+
+# 生成可执行文件
+add_executable(${Project_Module} ${Project_Module_Source})
+
+# 添加链接静态库
+target_link_libraries(${Project_Module}
+        ${Project_Module_Libary}
+)
+
+# 将库文件，可执行文件，头文件安装到指定目录
+install(TARGETS ${Project_Module}
+        LIBRARY DESTINATION lib  # 动态库安装路径
+        ARCHIVE DESTINATION lib  # 静态库安装路径
+        RUNTIME DESTINATION bin  # 可执行文件安装路径
+        PUBLIC_HEADER DESTINATION include  # 头文件安装路径
+)
+
+install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/etc
+        DESTINATION ${CMAKE_INSTALL_PREFIX}
+        )

Modules/LidarICP/etc/icp_mqtt_async.json

@@ -0,0 +1,13 @@
+{
+    "address": "mqtt://127.0.0.1:1883",
+    "sendMessage": [
+        {
+            "topic": "rslidar"
+        }
+    ],
+    "subscribe": [
+        {
+            "topic": "rslidar/#"
+        }
+    ]
+}

Modules/LidarICP/main.cpp

@@ -0,0 +1,173 @@
+/**
+  * @project Project
+  * @brief   TODO: add a brief
+  * @author  lz
+  * @data    2023/6/24
+ **/
+
+#include <iostream>
+#include <string>
+#include <pcl/io/ply_io.h>
+#include <pcl/point_types.h>
+#include <pcl/registration/icp.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/console/time.h>
+#include <vector>
+#include <omp.h>
+
+#include <map>
+#include "rslidar_mqtt_async.h"
+
+using namespace std;
+
+typedef pcl::PointXYZ PointT;
+typedef pcl::PointCloud<PointT> PointCloudT;
+bool next_iteration = false;
+
+void print4x4Matrix(const Eigen::Matrix4d &matrix) {
+    printf("Rotation matrix :\n");
+    printf("    | %6.6f %6.6f %6.6f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
+    printf("R = | %6.6f %6.6f %6.6f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
+    printf("    | %6.6f %6.6f %6.6f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
+    printf("Translation vector :\n");
+    printf("t = < %6.6f, %6.6f, %6.6f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
+}
+
+void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *nothing) {
+    if (event.getKeySym() == "space" && event.keyDown())
+        next_iteration = true;
+}
+
+void findcorrespondences(pcl::PointCloud<pcl::PointXYZ>::Ptr &source, pcl::PointCloud<pcl::PointXYZ>::Ptr &target,
+                         pcl::PointCloud<pcl::PointXYZ>::Ptr &P, pcl::PointCloud<pcl::PointXYZ>::Ptr &Q) {
+    P->clear();
+    Q->clear();
+    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
+    kdtree.setInputCloud(target);
+//#pragma omp parallel for num_threads(200)
+    for (int i = 0; i < source->points.size(); i++) {
+        vector<int> indx;
+        vector<float> dists;
+        kdtree.nearestKSearch(source->points[i], 1, indx, dists);
+        if (dists[0] < 0.02) {
+            P->push_back(source->points[i]);
+            Q->push_back(target->points[indx[0]]);
+        }
+    }
+}
+
+Eigen::Matrix<double, 4, 4> findRT(pcl::PointCloud<pcl::PointXYZ>::Ptr &P, pcl::PointCloud<pcl::PointXYZ>::Ptr &Q) {
+    int n = P->points.size();
+    Eigen::Matrix<double, 3, Eigen::Dynamic> matrixP(3, n), matrixQ(3, n);
+    for (int i = 0; i < n; i++) {
+        matrixP(0, i) = P->points[i].x;
+        matrixP(1, i) = P->points[i].y;
+        matrixP(2, i) = P->points[i].z;
+        matrixQ(0, i) = Q->points[i].x;
+        matrixQ(1, i) = Q->points[i].y;
+        matrixQ(2, i) = Q->points[i].z;
+    }
+    Eigen::Matrix4d RT = Eigen::umeyama(matrixP, matrixQ);
+    print4x4Matrix(RT);
+    return RT;
+}
+
+void myICP(pcl::PointCloud<pcl::PointXYZ>::Ptr &source, pcl::PointCloud<pcl::PointXYZ>::Ptr &target,
+           Eigen::Matrix<double, 4, 4> &RT) {
+    pcl::PointCloud<pcl::PointXYZ>::Ptr P(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::PointCloud<pcl::PointXYZ>::Ptr Q(new pcl::PointCloud<pcl::PointXYZ>);
+
+    pcl::visualization::PCLVisualizer viewer("viewer");
+
+    pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_source(source, 255, 0, 0), cloud_target(target, 255,
+                                                                                                           255, 255);
+
+    viewer.addPointCloud(source, cloud_source, "source");
+    viewer.addPointCloud(target, cloud_target, "target");
+
+    viewer.registerKeyboardCallback(&keyboardEventOccurred, (void *) nullptr);
+    while (!viewer.wasStopped()) {
+        viewer.spinOnce();
+        if (next_iteration) {
+            findcorrespondences(source, target, P, Q);
+            RT = findRT(P, Q);
+            pcl::transformPointCloud(*source, *source, RT);
+            viewer.updatePointCloud(source, cloud_source, "source");
+        }
+        next_iteration = false;
+    }
+}
+
+
+int main() {
+    CloudDataMqttAsync mqtt_client;
+    mqtt_client.init();
+
+    std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_map;
+    cloud_map.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>("rslidar/6311",
+                                                                                 new pcl::PointCloud<pcl::PointXYZ>));
+    cloud_map.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>("rslidar/6312",
+                                                                                 new pcl::PointCloud<pcl::PointXYZ>));
+    cloud_map.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>("rslidar/6313",
+                                                                                 new pcl::PointCloud<pcl::PointXYZ>));
+    cloud_map.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>("rslidar/6314",
+                                                                                 new pcl::PointCloud<pcl::PointXYZ>));
+    cloud_map.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>("rslidar/6315",
+                                                                                 new pcl::PointCloud<pcl::PointXYZ>));
+    cloud_map.insert(std::pair<std::string, pcl::PointCloud<pcl::PointXYZ>::Ptr>("rslidar/6316",
+                                                                                 new pcl::PointCloud<pcl::PointXYZ>));
+
+    std::map<std::string, Eigen::Matrix<double, 4, 4>> rt_map;
+    rt_map.insert(std::pair<std::string, Eigen::Matrix<double, 4, 4>>("rslidar/6311", Eigen::Matrix<double, 4, 4>()));
+    rt_map.insert(std::pair<std::string, Eigen::Matrix<double, 4, 4>>("rslidar/6312", Eigen::Matrix<double, 4, 4>()));
+    rt_map.insert(std::pair<std::string, Eigen::Matrix<double, 4, 4>>("rslidar/6313", Eigen::Matrix<double, 4, 4>()));
+    rt_map.insert(std::pair<std::string, Eigen::Matrix<double, 4, 4>>("rslidar/6314", Eigen::Matrix<double, 4, 4>()));
+    rt_map.insert(std::pair<std::string, Eigen::Matrix<double, 4, 4>>("rslidar/6315", Eigen::Matrix<double, 4, 4>()));
+    rt_map.insert(std::pair<std::string, Eigen::Matrix<double, 4, 4>>("rslidar/6316", Eigen::Matrix<double, 4, 4>()));
+
+    bool run = true;
+    while (run) {
+        for (auto &iter: cloud_map) {
+            mqtt_client.getCloudData(iter.first, iter.second);
+        }
+
+        if (cloud_map.find("rslidar/6314")->second->empty()) {
+            continue;
+        }
+
+        if (!cloud_map.find("rslidar/6311")->second->empty()) {
+            myICP(cloud_map.find("rslidar/6314")->second, cloud_map.find("rslidar/6311")->second,
+                  rt_map.find("rslidar/6311")->second);
+        }
+
+        break;
+
+        if (!cloud_map.find("rslidar/6312")->second->empty()) {
+            myICP(cloud_map.find("rslidar/6314")->second, cloud_map.find("rslidar/6312")->second,
+                  rt_map.find("rslidar/6312")->second);
+
+        }
+        if (!cloud_map.find("rslidar/6313")->second->empty()) {
+            myICP(cloud_map.find("rslidar/6314")->second, cloud_map.find("rslidar/6313")->second,
+                  rt_map.find("rslidar/6313")->second);
+        }
+
+        if (!cloud_map.find("rslidar/6314")->second->empty()) {
+            myICP(cloud_map.find("rslidar/6314")->second, cloud_map.find("rslidar/6314")->second,
+                  rt_map.find("rslidar/6314")->second);
+        }
+
+        if (!cloud_map.find("rslidar/6315")->second->empty()) {
+            myICP(cloud_map.find("rslidar/6314")->second, cloud_map.find("rslidar/6315")->second,
+                  rt_map.find("rslidar/6315")->second);
+        }
+
+        if (!cloud_map.find("rslidar/6316")->second->empty()) {
+            myICP(cloud_map.find("rslidar/6314")->second, cloud_map.find("rslidar/6316")->second,
+                  rt_map.find("rslidar/6316")->second);
+        }
+
+        run = false;
+    }
+}
+

Modules/LidarICP/rslidar_mqtt_async.cpp

@@ -0,0 +1,168 @@
+/**
+  * @project ALG_WHeelData
+  * @brief   从mqtt服务器获取点云数据并进行处理，仅给外部一个获取数据的接口。
+  * @author  lz
+  * @data    2023/4/21
+ **/
+#include "rslidar_mqtt_async.h"
+
+bool CloudDataMqttAsync::init() {
+    m_message_arrived_callback_ = CloudDataArrived;
+
+    MqttAsyncClient::init_from_proto(MQTT_CONFIG_PATH);
+    return true;
+}
+
+int CloudDataMqttAsync::CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen,
+                                         MQTTAsync_message *message) {
+    std::string topic = topicName;
+    DLOG(INFO) << "received message from topic : " << topic << ".";
+
+    auto *m_client = dynamic_cast<CloudDataMqttAsync *>(client);
+    m_client->insertCloudData(topic, (unsigned char *) message->payload, message->payloadlen);
+
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    // 检测时间戳是否超时
+    auto iter_time = m_cloud_time_stamp_map.find(topic);
+    if (iter_time == m_cloud_time_stamp_map.end()) {
+        return false;
+    }
+    std::chrono::duration<double> diff = std::chrono::steady_clock::now() - iter_time->second;
+    if (diff.count() > 1) {
+        LOG(WARNING) << "cloud not update, please re-check lidar mqtt statu.";
+        return false;
+    }
+
+    // 检测点云数据是否存在
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        LOG(WARNING) << "find topic " << topic << " in m_cloud_map error, please re-check save data code.";
+        return false;
+    }
+
+    for (auto &point: iter->second->points) {
+        cloud->emplace_back(point.x, point.y, point.z);
+    }
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+
+    // 检测时间戳是否超时
+    auto iter_time = m_cloud_time_stamp_map.find(topic);
+    if (iter_time == m_cloud_time_stamp_map.end()) {
+        return false;
+    }
+    std::chrono::duration<double> diff = std::chrono::steady_clock::now() - iter_time->second;
+    if (diff.count() > 1) {
+        LOG(WARNING) << "cloud not update, please re-check lidar mqtt statu.";
+        return false;
+    }
+
+    // 检测点云数据是否存在
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        LOG(WARNING) << "find topic " << topic << " in m_cloud_map error, please re-check save data code.";
+        return false;
+    }
+
+    // 获取点云输据
+    pcl::PointXYZI pt;
+    for (auto &point: iter->second->points) {
+        pt.x = point.x;
+        pt.y = point.y;
+        pt.z = point.z;
+        pt.intensity = point.intensity;
+        cloud->emplace_back(pt);
+    }
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointRPRY>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    // 检测时间戳是否超时
+    auto iter_time = m_cloud_time_stamp_map.find(topic);
+    if (iter_time == m_cloud_time_stamp_map.end()) {
+        return false;
+    }
+    std::chrono::duration<double> diff = std::chrono::steady_clock::now() - iter_time->second;
+    if (diff.count() > 1) {
+        LOG(WARNING) << "cloud not update, please re-check lidar mqtt statu.";
+        return false;
+    }
+
+    // 检测点云数据是否存在
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        LOG(WARNING) << "find topic " << topic << " in m_cloud_map error, please re-check save data code.";
+        return false;
+    }
+
+    pcl::PointRPRY pt;
+    for (auto &point: iter->second->points) {
+        pt.pitch = point.pitch;
+        pt.yaw = point.yaw;
+        pt.roll = point.roll;
+        pt.r = point.r;
+        cloud->emplace_back(pt);
+    }
+    return true;
+}
+
+bool CloudDataMqttAsync::getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud) {
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    // 检测时间戳是否超时
+    auto iter_time = m_cloud_time_stamp_map.find(topic);
+    if (iter_time == m_cloud_time_stamp_map.end()) {
+        return false;
+    }
+    std::chrono::duration<double> diff = std::chrono::steady_clock::now() - iter_time->second;
+    if (diff.count() > 3) {
+
+    }
+    if (diff.count() > 1) {
+        LOG(WARNING) << "cloud not update, please re-check lidar mqtt statu.";
+        return false;
+    }
+
+    // 检测点云数据是否存在
+    auto iter = m_full_clouds_map.find(topic);
+    if (iter == m_full_clouds_map.end()) {
+        LOG(WARNING) << "find topic " << topic << " in m_cloud_map error, please re-check save data code.";
+        return false;
+    }
+
+    cloud->insert(cloud->end(), iter->second->points.begin(), iter->second->points.end());
+    return true;
+}
+
+bool CloudDataMqttAsync::insertCloudData(std::string &topic, unsigned char *cloud_data, int size) {
+    pcl::PointCloud<pcl::PointALL>::Ptr clouds(new pcl::PointCloud<pcl::PointALL>);
+
+    // 解压数据，获得点云
+    m_unpacket->unpacket((unsigned char *) cloud_data, size, clouds);
+    std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    auto iter_full_cloud = m_full_clouds_map.find(topic);
+    if (iter_full_cloud == m_full_clouds_map.end()) {
+        m_full_clouds_map.insert(
+                std::pair<std::string, pcl::PointCloud<pcl::PointALL>::Ptr>(topic, new pcl::PointCloud<pcl::PointALL>));
+        iter_full_cloud = m_full_clouds_map.find(topic);
+    }
+    auto iter_cloud_time_stamp = m_cloud_time_stamp_map.find(topic);
+    if (iter_cloud_time_stamp == m_cloud_time_stamp_map.end()) {
+        m_cloud_time_stamp_map.insert(
+                std::pair<std::string, std::chrono::steady_clock::time_point>(topic, std::chrono::steady_clock::now()));
+        iter_cloud_time_stamp = m_cloud_time_stamp_map.find(topic);
+    }
+
+    iter_full_cloud->second->swap(*clouds);
+    iter_cloud_time_stamp->second = std::chrono::steady_clock::now();
+
+    return true;
+}
+

Modules/LidarICP/rslidar_mqtt_async.h

@@ -0,0 +1,55 @@
+/**
+  * @project ALG_WHeelData
+  * @brief   从mqtt服务器获取点云数据并进行处理，仅给外部一个获取数据的接口。
+  * @author  lz
+  * @data    2023/4/21
+ **/
+#pragma once
+
+#include "pahoc/mqtt_async.h"
+#include "message/unpacket.hpp"
+#include "defines.hpp"
+#include <vector>
+#include <memory>
+#include <typeinfo>
+
+// TODO: 完善功能
+class CloudDataMqttAsync : public MqttAsyncClient {
+public:
+    CloudDataMqttAsync() = default;
+
+    ~CloudDataMqttAsync() = default;
+
+    static CloudDataMqttAsync *instance() {
+        static CloudDataMqttAsync *p = nullptr;
+        if (p == nullptr) {
+            p = new CloudDataMqttAsync();
+            p->init();
+        }
+        return p;
+    }
+
+    bool init();
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointRPRY>::Ptr &cloud);
+
+    bool getCloudData(const std::string &topic, pcl::PointCloud<pcl::PointALL>::Ptr &cloud);
+
+private:
+    static int CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen, MQTTAsync_message *message);
+
+    bool insertCloudData(std::string &topic, unsigned char *cloud_data, int size);
+
+private:
+    const char *MQTT_CONFIG_PATH = ETC_PATH"/etc/icp_mqtt_async.json";
+    CloudPointUnpacket *m_unpacket;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr m_clouds;
+
+    std::mutex m_cloud_mutex;
+    std::map<std::string, pcl::PointCloud<pcl::PointALL>::Ptr> m_full_clouds_map;
+    std::map<std::string, std::chrono::steady_clock::time_point> m_cloud_time_stamp_map;
+};

Modules/MeasureNode/CMakeLists.txt

@@ -0,0 +1,72 @@
+if(TEST)
+	add_definitions(-DETC_PATH="${CMAKE_CURRENT_LIST_DIR}")
+endif ()
+
+pkg_check_modules(nanomsg REQUIRED nanomsg)
+
+include_directories(
+)
+link_directories("/usr/local/lib")
+
+message(STATUS ${EXECUTABLE_OUTPUT_PATH})
+
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/error_code ERROR_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/message message_src )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/wanji_lidar WANJI_LIDAR_SRC )
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication COMMUNICATION_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/system SYSTEM_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY_SRC )
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/velodyne_lidar/match3d/common VELODYNE_LIDAR_COMMON)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/velodyne_lidar/match3d/ VELODYNE_LIDAR_MATCH)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/velodyne_lidar/velodyne_driver VELODYNE_LIDAR_DRIVER)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/velodyne_lidar VELODYNE_LIDAR)
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/rabbitmq RABBIT_MQ)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/rslidar RSLIDAR)
+
+add_executable(measure_wj
+		${CMAKE_CURRENT_LIST_DIR}/measure_main.cpp
+        ${ERROR_SRC}
+        ${message_src}
+
+		${WANJI_LIDAR_SRC}
+        ${TASK_MANAGER_SRC}
+        ${TOOL_SRC}
+        ${COMMUNICATION_SRC}
+        ${SYSTEM_SRC}
+		${VERIFY_SRC}
+		${VELODYNE_LIDAR_COMMON}
+		${VELODYNE_LIDAR_MATCH}
+		${VELODYNE_LIDAR_DRIVER}
+		${VELODYNE_LIDAR}
+		${RABBIT_MQ}
+		${RSLIDAR}
+		)
+
+target_link_libraries(measure_wj
+		zx
+        /usr/local/lib/librabbitmq.a
+        nnxx
+        nanomsg
+		${CERES_LIBRARIES}
+		${YAML_CPP_LIBRARIES}
+        -lpthread
+        )
+
+
+install(TARGETS measure_wj
+		LIBRARY DESTINATION lib  # 动态库安装路径
+		ARCHIVE DESTINATION lib  # 静态库安装路径
+		RUNTIME DESTINATION bin  # 可执行文件安装路径
+		PUBLIC_HEADER DESTINATION include  # 头文件安装路径
+		)
+
+install(DIRECTORY ${CMAKE_CURRENT_LIST_DIR}/setting
+		DESTINATION ${CMAKE_INSTALL_PREFIX}
+		)
+

Modules/MeasureNode/README.md

@@ -0,0 +1,23 @@
+# 1，简介
+
+##     1.1功能描述：
+    实时发送当前测量结果，
+    消息格式：状态消息,
+    发送端口：measure_N_statu_port
+##     1.2使用平台：
+     linux X64/linux arm 
+
+# 2，业务逻辑
+# 3，编译  
+###     linux下：
+    mkdir build
+    cd build
+    cmake ..
+    make -j4
+###     windows下
+    ......
+    
+# 4，部署与启动
+    执行命令：....
+
+

Modules/MeasureNode/communication/communication.pb.cc

@@ -0,0 +1,618 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: communication.proto
+
+#include "communication.pb.h"
+
+#include <algorithm>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/extension_set.h>
+#include <google/protobuf/wire_format_lite.h>
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/reflection_ops.h>
+#include <google/protobuf/wire_format.h>
+// @@protoc_insertion_point(includes)
+#include <google/protobuf/port_def.inc>
+extern PROTOBUF_INTERNAL_EXPORT_communication_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Communication_parameter_communication_2eproto;
+namespace Communication_proto {
+class Communication_parameterDefaultTypeInternal {
+ public:
+  ::PROTOBUF_NAMESPACE_ID::internal::ExplicitlyConstructed<Communication_parameter> _instance;
+} _Communication_parameter_default_instance_;
+class Communication_parameter_allDefaultTypeInternal {
+ public:
+  ::PROTOBUF_NAMESPACE_ID::internal::ExplicitlyConstructed<Communication_parameter_all> _instance;
+} _Communication_parameter_all_default_instance_;
+}  // namespace Communication_proto
+static void InitDefaultsscc_info_Communication_parameter_communication_2eproto() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  {
+    void* ptr = &::Communication_proto::_Communication_parameter_default_instance_;
+    new (ptr) ::Communication_proto::Communication_parameter();
+    ::PROTOBUF_NAMESPACE_ID::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::Communication_proto::Communication_parameter::InitAsDefaultInstance();
+}
+
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Communication_parameter_communication_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 0, 0, InitDefaultsscc_info_Communication_parameter_communication_2eproto}, {}};
+
+static void InitDefaultsscc_info_Communication_parameter_all_communication_2eproto() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  {
+    void* ptr = &::Communication_proto::_Communication_parameter_all_default_instance_;
+    new (ptr) ::Communication_proto::Communication_parameter_all();
+    ::PROTOBUF_NAMESPACE_ID::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::Communication_proto::Communication_parameter_all::InitAsDefaultInstance();
+}
+
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<1> scc_info_Communication_parameter_all_communication_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 1, 0, InitDefaultsscc_info_Communication_parameter_all_communication_2eproto}, {
+      &scc_info_Communication_parameter_communication_2eproto.base,}};
+
+static ::PROTOBUF_NAMESPACE_ID::Metadata file_level_metadata_communication_2eproto[2];
+static constexpr ::PROTOBUF_NAMESPACE_ID::EnumDescriptor const** file_level_enum_descriptors_communication_2eproto = nullptr;
+static constexpr ::PROTOBUF_NAMESPACE_ID::ServiceDescriptor const** file_level_service_descriptors_communication_2eproto = nullptr;
+
+const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_communication_2eproto::offsets[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
+  PROTOBUF_FIELD_OFFSET(::Communication_proto::Communication_parameter, _has_bits_),
+  PROTOBUF_FIELD_OFFSET(::Communication_proto::Communication_parameter, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  PROTOBUF_FIELD_OFFSET(::Communication_proto::Communication_parameter, bind_string_),
+  PROTOBUF_FIELD_OFFSET(::Communication_proto::Communication_parameter, connect_string_vector_),
+  0,
+  ~0u,
+  PROTOBUF_FIELD_OFFSET(::Communication_proto::Communication_parameter_all, _has_bits_),
+  PROTOBUF_FIELD_OFFSET(::Communication_proto::Communication_parameter_all, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  PROTOBUF_FIELD_OFFSET(::Communication_proto::Communication_parameter_all, communication_parameters_),
+  0,
+};
+static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
+  { 0, 7, sizeof(::Communication_proto::Communication_parameter)},
+  { 9, 15, sizeof(::Communication_proto::Communication_parameter_all)},
+};
+
+static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
+  reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::Communication_proto::_Communication_parameter_default_instance_),
+  reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::Communication_proto::_Communication_parameter_all_default_instance_),
+};
+
+const char descriptor_table_protodef_communication_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
+  "\n\023communication.proto\022\023Communication_pro"
+  "to\"M\n\027Communication_parameter\022\023\n\013bind_st"
+  "ring\030\001 \001(\t\022\035\n\025connect_string_vector\030\002 \003("
+  "\t\"m\n\033Communication_parameter_all\022N\n\030comm"
+  "unication_parameters\030\001 \001(\0132,.Communicati"
+  "on_proto.Communication_parameter"
+  ;
+static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_communication_2eproto_deps[1] = {
+};
+static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_communication_2eproto_sccs[2] = {
+  &scc_info_Communication_parameter_communication_2eproto.base,
+  &scc_info_Communication_parameter_all_communication_2eproto.base,
+};
+static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_communication_2eproto_once;
+const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_communication_2eproto = {
+  false, false, descriptor_table_protodef_communication_2eproto, "communication.proto", 232,
+  &descriptor_table_communication_2eproto_once, descriptor_table_communication_2eproto_sccs, descriptor_table_communication_2eproto_deps, 2, 0,
+  schemas, file_default_instances, TableStruct_communication_2eproto::offsets,
+  file_level_metadata_communication_2eproto, 2, file_level_enum_descriptors_communication_2eproto, file_level_service_descriptors_communication_2eproto,
+};
+
+// Force running AddDescriptors() at dynamic initialization time.
+static bool dynamic_init_dummy_communication_2eproto = (static_cast<void>(::PROTOBUF_NAMESPACE_ID::internal::AddDescriptors(&descriptor_table_communication_2eproto)), true);
+namespace Communication_proto {
+
+// ===================================================================
+
+void Communication_parameter::InitAsDefaultInstance() {
+}
+class Communication_parameter::_Internal {
+ public:
+  using HasBits = decltype(std::declval<Communication_parameter>()._has_bits_);
+  static void set_has_bind_string(HasBits* has_bits) {
+    (*has_bits)[0] |= 1u;
+  }
+};
+
+Communication_parameter::Communication_parameter(::PROTOBUF_NAMESPACE_ID::Arena* arena)
+  : ::PROTOBUF_NAMESPACE_ID::Message(arena),
+  connect_string_vector_(arena) {
+  SharedCtor();
+  RegisterArenaDtor(arena);
+  // @@protoc_insertion_point(arena_constructor:Communication_proto.Communication_parameter)
+}
+Communication_parameter::Communication_parameter(const Communication_parameter& from)
+  : ::PROTOBUF_NAMESPACE_ID::Message(),
+      _has_bits_(from._has_bits_),
+      connect_string_vector_(from.connect_string_vector_) {
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  bind_string_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (from._internal_has_bind_string()) {
+    bind_string_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_bind_string(),
+      GetArena());
+  }
+  // @@protoc_insertion_point(copy_constructor:Communication_proto.Communication_parameter)
+}
+
+void Communication_parameter::SharedCtor() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_Communication_parameter_communication_2eproto.base);
+  bind_string_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+}
+
+Communication_parameter::~Communication_parameter() {
+  // @@protoc_insertion_point(destructor:Communication_proto.Communication_parameter)
+  SharedDtor();
+  _internal_metadata_.Delete<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+void Communication_parameter::SharedDtor() {
+  GOOGLE_DCHECK(GetArena() == nullptr);
+  bind_string_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+}
+
+void Communication_parameter::ArenaDtor(void* object) {
+  Communication_parameter* _this = reinterpret_cast< Communication_parameter* >(object);
+  (void)_this;
+}
+void Communication_parameter::RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena*) {
+}
+void Communication_parameter::SetCachedSize(int size) const {
+  _cached_size_.Set(size);
+}
+const Communication_parameter& Communication_parameter::default_instance() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&::scc_info_Communication_parameter_communication_2eproto.base);
+  return *internal_default_instance();
+}
+
+
+void Communication_parameter::Clear() {
+// @@protoc_insertion_point(message_clear_start:Communication_proto.Communication_parameter)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  connect_string_vector_.Clear();
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    bind_string_.ClearNonDefaultToEmpty();
+  }
+  _has_bits_.Clear();
+  _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+const char* Communication_parameter::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) {
+#define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  _Internal::HasBits has_bits{};
+  ::PROTOBUF_NAMESPACE_ID::Arena* arena = GetArena(); (void)arena;
+  while (!ctx->Done(&ptr)) {
+    ::PROTOBUF_NAMESPACE_ID::uint32 tag;
+    ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
+    CHK_(ptr);
+    switch (tag >> 3) {
+      // optional string bind_string = 1;
+      case 1:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 10)) {
+          auto str = _internal_mutable_bind_string();
+          ptr = ::PROTOBUF_NAMESPACE_ID::internal::InlineGreedyStringParser(str, ptr, ctx);
+          #ifndef NDEBUG
+          ::PROTOBUF_NAMESPACE_ID::internal::VerifyUTF8(str, "Communication_proto.Communication_parameter.bind_string");
+          #endif  // !NDEBUG
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // repeated string connect_string_vector = 2;
+      case 2:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 18)) {
+          ptr -= 1;
+          do {
+            ptr += 1;
+            auto str = _internal_add_connect_string_vector();
+            ptr = ::PROTOBUF_NAMESPACE_ID::internal::InlineGreedyStringParser(str, ptr, ctx);
+            #ifndef NDEBUG
+            ::PROTOBUF_NAMESPACE_ID::internal::VerifyUTF8(str, "Communication_proto.Communication_parameter.connect_string_vector");
+            #endif  // !NDEBUG
+            CHK_(ptr);
+            if (!ctx->DataAvailable(ptr)) break;
+          } while (::PROTOBUF_NAMESPACE_ID::internal::ExpectTag<18>(ptr));
+        } else goto handle_unusual;
+        continue;
+      default: {
+      handle_unusual:
+        if ((tag & 7) == 4 || tag == 0) {
+          ctx->SetLastTag(tag);
+          goto success;
+        }
+        ptr = UnknownFieldParse(tag,
+            _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(),
+            ptr, ctx);
+        CHK_(ptr != nullptr);
+        continue;
+      }
+    }  // switch
+  }  // while
+success:
+  _has_bits_.Or(has_bits);
+  return ptr;
+failure:
+  ptr = nullptr;
+  goto success;
+#undef CHK_
+}
+
+::PROTOBUF_NAMESPACE_ID::uint8* Communication_parameter::_InternalSerialize(
+    ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const {
+  // @@protoc_insertion_point(serialize_to_array_start:Communication_proto.Communication_parameter)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional string bind_string = 1;
+  if (cached_has_bits & 0x00000001u) {
+    ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->_internal_bind_string().data(), static_cast<int>(this->_internal_bind_string().length()),
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::SERIALIZE,
+      "Communication_proto.Communication_parameter.bind_string");
+    target = stream->WriteStringMaybeAliased(
+        1, this->_internal_bind_string(), target);
+  }
+
+  // repeated string connect_string_vector = 2;
+  for (int i = 0, n = this->_internal_connect_string_vector_size(); i < n; i++) {
+    const auto& s = this->_internal_connect_string_vector(i);
+    ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::VerifyUTF8StringNamedField(
+      s.data(), static_cast<int>(s.length()),
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::SERIALIZE,
+      "Communication_proto.Communication_parameter.connect_string_vector");
+    target = stream->WriteString(2, s, target);
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:Communication_proto.Communication_parameter)
+  return target;
+}
+
+size_t Communication_parameter::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:Communication_proto.Communication_parameter)
+  size_t total_size = 0;
+
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  // repeated string connect_string_vector = 2;
+  total_size += 1 *
+      ::PROTOBUF_NAMESPACE_ID::internal::FromIntSize(connect_string_vector_.size());
+  for (int i = 0, n = connect_string_vector_.size(); i < n; i++) {
+    total_size += ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
+      connect_string_vector_.Get(i));
+  }
+
+  // optional string bind_string = 1;
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
+        this->_internal_bind_string());
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
+        _internal_metadata_, total_size, &_cached_size_);
+  }
+  int cached_size = ::PROTOBUF_NAMESPACE_ID::internal::ToCachedSize(total_size);
+  SetCachedSize(cached_size);
+  return total_size;
+}
+
+void Communication_parameter::MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:Communication_proto.Communication_parameter)
+  GOOGLE_DCHECK_NE(&from, this);
+  const Communication_parameter* source =
+      ::PROTOBUF_NAMESPACE_ID::DynamicCastToGenerated<Communication_parameter>(
+          &from);
+  if (source == nullptr) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:Communication_proto.Communication_parameter)
+    ::PROTOBUF_NAMESPACE_ID::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:Communication_proto.Communication_parameter)
+    MergeFrom(*source);
+  }
+}
+
+void Communication_parameter::MergeFrom(const Communication_parameter& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:Communication_proto.Communication_parameter)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  connect_string_vector_.MergeFrom(from.connect_string_vector_);
+  if (from._internal_has_bind_string()) {
+    _internal_set_bind_string(from._internal_bind_string());
+  }
+}
+
+void Communication_parameter::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:Communication_proto.Communication_parameter)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void Communication_parameter::CopyFrom(const Communication_parameter& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:Communication_proto.Communication_parameter)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Communication_parameter::IsInitialized() const {
+  return true;
+}
+
+void Communication_parameter::InternalSwap(Communication_parameter* other) {
+  using std::swap;
+  _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  connect_string_vector_.InternalSwap(&other->connect_string_vector_);
+  bind_string_.Swap(&other->bind_string_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+
+::PROTOBUF_NAMESPACE_ID::Metadata Communication_parameter::GetMetadata() const {
+  return GetMetadataStatic();
+}
+
+
+// ===================================================================
+
+void Communication_parameter_all::InitAsDefaultInstance() {
+  ::Communication_proto::_Communication_parameter_all_default_instance_._instance.get_mutable()->communication_parameters_ = const_cast< ::Communication_proto::Communication_parameter*>(
+      ::Communication_proto::Communication_parameter::internal_default_instance());
+}
+class Communication_parameter_all::_Internal {
+ public:
+  using HasBits = decltype(std::declval<Communication_parameter_all>()._has_bits_);
+  static const ::Communication_proto::Communication_parameter& communication_parameters(const Communication_parameter_all* msg);
+  static void set_has_communication_parameters(HasBits* has_bits) {
+    (*has_bits)[0] |= 1u;
+  }
+};
+
+const ::Communication_proto::Communication_parameter&
+Communication_parameter_all::_Internal::communication_parameters(const Communication_parameter_all* msg) {
+  return *msg->communication_parameters_;
+}
+Communication_parameter_all::Communication_parameter_all(::PROTOBUF_NAMESPACE_ID::Arena* arena)
+  : ::PROTOBUF_NAMESPACE_ID::Message(arena) {
+  SharedCtor();
+  RegisterArenaDtor(arena);
+  // @@protoc_insertion_point(arena_constructor:Communication_proto.Communication_parameter_all)
+}
+Communication_parameter_all::Communication_parameter_all(const Communication_parameter_all& from)
+  : ::PROTOBUF_NAMESPACE_ID::Message(),
+      _has_bits_(from._has_bits_) {
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  if (from._internal_has_communication_parameters()) {
+    communication_parameters_ = new ::Communication_proto::Communication_parameter(*from.communication_parameters_);
+  } else {
+    communication_parameters_ = nullptr;
+  }
+  // @@protoc_insertion_point(copy_constructor:Communication_proto.Communication_parameter_all)
+}
+
+void Communication_parameter_all::SharedCtor() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_Communication_parameter_all_communication_2eproto.base);
+  communication_parameters_ = nullptr;
+}
+
+Communication_parameter_all::~Communication_parameter_all() {
+  // @@protoc_insertion_point(destructor:Communication_proto.Communication_parameter_all)
+  SharedDtor();
+  _internal_metadata_.Delete<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+void Communication_parameter_all::SharedDtor() {
+  GOOGLE_DCHECK(GetArena() == nullptr);
+  if (this != internal_default_instance()) delete communication_parameters_;
+}
+
+void Communication_parameter_all::ArenaDtor(void* object) {
+  Communication_parameter_all* _this = reinterpret_cast< Communication_parameter_all* >(object);
+  (void)_this;
+}
+void Communication_parameter_all::RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena*) {
+}
+void Communication_parameter_all::SetCachedSize(int size) const {
+  _cached_size_.Set(size);
+}
+const Communication_parameter_all& Communication_parameter_all::default_instance() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&::scc_info_Communication_parameter_all_communication_2eproto.base);
+  return *internal_default_instance();
+}
+
+
+void Communication_parameter_all::Clear() {
+// @@protoc_insertion_point(message_clear_start:Communication_proto.Communication_parameter_all)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    GOOGLE_DCHECK(communication_parameters_ != nullptr);
+    communication_parameters_->Clear();
+  }
+  _has_bits_.Clear();
+  _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+const char* Communication_parameter_all::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) {
+#define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  _Internal::HasBits has_bits{};
+  ::PROTOBUF_NAMESPACE_ID::Arena* arena = GetArena(); (void)arena;
+  while (!ctx->Done(&ptr)) {
+    ::PROTOBUF_NAMESPACE_ID::uint32 tag;
+    ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
+    CHK_(ptr);
+    switch (tag >> 3) {
+      // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+      case 1:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 10)) {
+          ptr = ctx->ParseMessage(_internal_mutable_communication_parameters(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      default: {
+      handle_unusual:
+        if ((tag & 7) == 4 || tag == 0) {
+          ctx->SetLastTag(tag);
+          goto success;
+        }
+        ptr = UnknownFieldParse(tag,
+            _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(),
+            ptr, ctx);
+        CHK_(ptr != nullptr);
+        continue;
+      }
+    }  // switch
+  }  // while
+success:
+  _has_bits_.Or(has_bits);
+  return ptr;
+failure:
+  ptr = nullptr;
+  goto success;
+#undef CHK_
+}
+
+::PROTOBUF_NAMESPACE_ID::uint8* Communication_parameter_all::_InternalSerialize(
+    ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const {
+  // @@protoc_insertion_point(serialize_to_array_start:Communication_proto.Communication_parameter_all)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+  if (cached_has_bits & 0x00000001u) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        1, _Internal::communication_parameters(this), target, stream);
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:Communication_proto.Communication_parameter_all)
+  return target;
+}
+
+size_t Communication_parameter_all::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:Communication_proto.Communication_parameter_all)
+  size_t total_size = 0;
+
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *communication_parameters_);
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
+        _internal_metadata_, total_size, &_cached_size_);
+  }
+  int cached_size = ::PROTOBUF_NAMESPACE_ID::internal::ToCachedSize(total_size);
+  SetCachedSize(cached_size);
+  return total_size;
+}
+
+void Communication_parameter_all::MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:Communication_proto.Communication_parameter_all)
+  GOOGLE_DCHECK_NE(&from, this);
+  const Communication_parameter_all* source =
+      ::PROTOBUF_NAMESPACE_ID::DynamicCastToGenerated<Communication_parameter_all>(
+          &from);
+  if (source == nullptr) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:Communication_proto.Communication_parameter_all)
+    ::PROTOBUF_NAMESPACE_ID::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:Communication_proto.Communication_parameter_all)
+    MergeFrom(*source);
+  }
+}
+
+void Communication_parameter_all::MergeFrom(const Communication_parameter_all& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:Communication_proto.Communication_parameter_all)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  if (from._internal_has_communication_parameters()) {
+    _internal_mutable_communication_parameters()->::Communication_proto::Communication_parameter::MergeFrom(from._internal_communication_parameters());
+  }
+}
+
+void Communication_parameter_all::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:Communication_proto.Communication_parameter_all)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void Communication_parameter_all::CopyFrom(const Communication_parameter_all& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:Communication_proto.Communication_parameter_all)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool Communication_parameter_all::IsInitialized() const {
+  return true;
+}
+
+void Communication_parameter_all::InternalSwap(Communication_parameter_all* other) {
+  using std::swap;
+  _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  swap(communication_parameters_, other->communication_parameters_);
+}
+
+::PROTOBUF_NAMESPACE_ID::Metadata Communication_parameter_all::GetMetadata() const {
+  return GetMetadataStatic();
+}
+
+
+// @@protoc_insertion_point(namespace_scope)
+}  // namespace Communication_proto
+PROTOBUF_NAMESPACE_OPEN
+template<> PROTOBUF_NOINLINE ::Communication_proto::Communication_parameter* Arena::CreateMaybeMessage< ::Communication_proto::Communication_parameter >(Arena* arena) {
+  return Arena::CreateMessageInternal< ::Communication_proto::Communication_parameter >(arena);
+}
+template<> PROTOBUF_NOINLINE ::Communication_proto::Communication_parameter_all* Arena::CreateMaybeMessage< ::Communication_proto::Communication_parameter_all >(Arena* arena) {
+  return Arena::CreateMessageInternal< ::Communication_proto::Communication_parameter_all >(arena);
+}
+PROTOBUF_NAMESPACE_CLOSE
+
+// @@protoc_insertion_point(global_scope)
+#include <google/protobuf/port_undef.inc>

Modules/MeasureNode/communication/communication.pb.h

@@ -0,0 +1,666 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: communication.proto
+
+#ifndef GOOGLE_PROTOBUF_INCLUDED_communication_2eproto
+#define GOOGLE_PROTOBUF_INCLUDED_communication_2eproto
+
+#include <limits>
+#include <string>
+
+#include <google/protobuf/port_def.inc>
+#if PROTOBUF_VERSION < 4000000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers. Please update
+#error your headers.
+#endif
+#if 4000000 < PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers. Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/port_undef.inc>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_table_driven.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/inlined_string_field.h>
+#include <google/protobuf/metadata_lite.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>  // IWYU pragma: export
+#include <google/protobuf/extension_set.h>  // IWYU pragma: export
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+#include <google/protobuf/port_def.inc>
+#define PROTOBUF_INTERNAL_EXPORT_communication_2eproto
+PROTOBUF_NAMESPACE_OPEN
+namespace internal {
+class AnyMetadata;
+}  // namespace internal
+PROTOBUF_NAMESPACE_CLOSE
+
+// Internal implementation detail -- do not use these members.
+struct TableStruct_communication_2eproto {
+  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTableField entries[]
+    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
+  static const ::PROTOBUF_NAMESPACE_ID::internal::AuxiliaryParseTableField aux[]
+    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
+  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[2]
+    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
+  static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[];
+  static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[];
+  static const ::PROTOBUF_NAMESPACE_ID::uint32 offsets[];
+};
+extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_communication_2eproto;
+namespace Communication_proto {
+class Communication_parameter;
+class Communication_parameterDefaultTypeInternal;
+extern Communication_parameterDefaultTypeInternal _Communication_parameter_default_instance_;
+class Communication_parameter_all;
+class Communication_parameter_allDefaultTypeInternal;
+extern Communication_parameter_allDefaultTypeInternal _Communication_parameter_all_default_instance_;
+}  // namespace Communication_proto
+PROTOBUF_NAMESPACE_OPEN
+template<> ::Communication_proto::Communication_parameter* Arena::CreateMaybeMessage<::Communication_proto::Communication_parameter>(Arena*);
+template<> ::Communication_proto::Communication_parameter_all* Arena::CreateMaybeMessage<::Communication_proto::Communication_parameter_all>(Arena*);
+PROTOBUF_NAMESPACE_CLOSE
+namespace Communication_proto {
+
+// ===================================================================
+
+class Communication_parameter PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:Communication_proto.Communication_parameter) */ {
+ public:
+  inline Communication_parameter() : Communication_parameter(nullptr) {}
+  virtual ~Communication_parameter();
+
+  Communication_parameter(const Communication_parameter& from);
+  Communication_parameter(Communication_parameter&& from) noexcept
+    : Communication_parameter() {
+    *this = ::std::move(from);
+  }
+
+  inline Communication_parameter& operator=(const Communication_parameter& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline Communication_parameter& operator=(Communication_parameter&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  inline const ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance);
+  }
+  inline ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const Communication_parameter& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const Communication_parameter* internal_default_instance() {
+    return reinterpret_cast<const Communication_parameter*>(
+               &_Communication_parameter_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    0;
+
+  friend void swap(Communication_parameter& a, Communication_parameter& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(Communication_parameter* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(Communication_parameter* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline Communication_parameter* New() const final {
+    return CreateMaybeMessage<Communication_parameter>(nullptr);
+  }
+
+  Communication_parameter* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<Communication_parameter>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const Communication_parameter& from);
+  void MergeFrom(const Communication_parameter& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(Communication_parameter* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "Communication_proto.Communication_parameter";
+  }
+  protected:
+  explicit Communication_parameter(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_communication_2eproto);
+    return ::descriptor_table_communication_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kConnectStringVectorFieldNumber = 2,
+    kBindStringFieldNumber = 1,
+  };
+  // repeated string connect_string_vector = 2;
+  int connect_string_vector_size() const;
+  private:
+  int _internal_connect_string_vector_size() const;
+  public:
+  void clear_connect_string_vector();
+  const std::string& connect_string_vector(int index) const;
+  std::string* mutable_connect_string_vector(int index);
+  void set_connect_string_vector(int index, const std::string& value);
+  void set_connect_string_vector(int index, std::string&& value);
+  void set_connect_string_vector(int index, const char* value);
+  void set_connect_string_vector(int index, const char* value, size_t size);
+  std::string* add_connect_string_vector();
+  void add_connect_string_vector(const std::string& value);
+  void add_connect_string_vector(std::string&& value);
+  void add_connect_string_vector(const char* value);
+  void add_connect_string_vector(const char* value, size_t size);
+  const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField<std::string>& connect_string_vector() const;
+  ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField<std::string>* mutable_connect_string_vector();
+  private:
+  const std::string& _internal_connect_string_vector(int index) const;
+  std::string* _internal_add_connect_string_vector();
+  public:
+
+  // optional string bind_string = 1;
+  bool has_bind_string() const;
+  private:
+  bool _internal_has_bind_string() const;
+  public:
+  void clear_bind_string();
+  const std::string& bind_string() const;
+  void set_bind_string(const std::string& value);
+  void set_bind_string(std::string&& value);
+  void set_bind_string(const char* value);
+  void set_bind_string(const char* value, size_t size);
+  std::string* mutable_bind_string();
+  std::string* release_bind_string();
+  void set_allocated_bind_string(std::string* bind_string);
+  private:
+  const std::string& _internal_bind_string() const;
+  void _internal_set_bind_string(const std::string& value);
+  std::string* _internal_mutable_bind_string();
+  public:
+
+  // @@protoc_insertion_point(class_scope:Communication_proto.Communication_parameter)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  ::PROTOBUF_NAMESPACE_ID::internal::HasBits<1> _has_bits_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField<std::string> connect_string_vector_;
+  ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr bind_string_;
+  friend struct ::TableStruct_communication_2eproto;
+};
+// -------------------------------------------------------------------
+
+class Communication_parameter_all PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:Communication_proto.Communication_parameter_all) */ {
+ public:
+  inline Communication_parameter_all() : Communication_parameter_all(nullptr) {}
+  virtual ~Communication_parameter_all();
+
+  Communication_parameter_all(const Communication_parameter_all& from);
+  Communication_parameter_all(Communication_parameter_all&& from) noexcept
+    : Communication_parameter_all() {
+    *this = ::std::move(from);
+  }
+
+  inline Communication_parameter_all& operator=(const Communication_parameter_all& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline Communication_parameter_all& operator=(Communication_parameter_all&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  inline const ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance);
+  }
+  inline ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const Communication_parameter_all& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const Communication_parameter_all* internal_default_instance() {
+    return reinterpret_cast<const Communication_parameter_all*>(
+               &_Communication_parameter_all_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    1;
+
+  friend void swap(Communication_parameter_all& a, Communication_parameter_all& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(Communication_parameter_all* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(Communication_parameter_all* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline Communication_parameter_all* New() const final {
+    return CreateMaybeMessage<Communication_parameter_all>(nullptr);
+  }
+
+  Communication_parameter_all* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<Communication_parameter_all>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const Communication_parameter_all& from);
+  void MergeFrom(const Communication_parameter_all& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(Communication_parameter_all* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "Communication_proto.Communication_parameter_all";
+  }
+  protected:
+  explicit Communication_parameter_all(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_communication_2eproto);
+    return ::descriptor_table_communication_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kCommunicationParametersFieldNumber = 1,
+  };
+  // optional .Communication_proto.Communication_parameter communication_parameters = 1;
+  bool has_communication_parameters() const;
+  private:
+  bool _internal_has_communication_parameters() const;
+  public:
+  void clear_communication_parameters();
+  const ::Communication_proto::Communication_parameter& communication_parameters() const;
+  ::Communication_proto::Communication_parameter* release_communication_parameters();
+  ::Communication_proto::Communication_parameter* mutable_communication_parameters();
+  void set_allocated_communication_parameters(::Communication_proto::Communication_parameter* communication_parameters);
+  private:
+  const ::Communication_proto::Communication_parameter& _internal_communication_parameters() const;
+  ::Communication_proto::Communication_parameter* _internal_mutable_communication_parameters();
+  public:
+  void unsafe_arena_set_allocated_communication_parameters(
+      ::Communication_proto::Communication_parameter* communication_parameters);
+  ::Communication_proto::Communication_parameter* unsafe_arena_release_communication_parameters();
+
+  // @@protoc_insertion_point(class_scope:Communication_proto.Communication_parameter_all)
+ private:
+  class _Internal;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  ::PROTOBUF_NAMESPACE_ID::internal::HasBits<1> _has_bits_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  ::Communication_proto::Communication_parameter* communication_parameters_;
+  friend struct ::TableStruct_communication_2eproto;
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#endif  // __GNUC__
+// Communication_parameter
+
+// optional string bind_string = 1;
+inline bool Communication_parameter::_internal_has_bind_string() const {
+  bool value = (_has_bits_[0] & 0x00000001u) != 0;
+  return value;
+}
+inline bool Communication_parameter::has_bind_string() const {
+  return _internal_has_bind_string();
+}
+inline void Communication_parameter::clear_bind_string() {
+  bind_string_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline const std::string& Communication_parameter::bind_string() const {
+  // @@protoc_insertion_point(field_get:Communication_proto.Communication_parameter.bind_string)
+  return _internal_bind_string();
+}
+inline void Communication_parameter::set_bind_string(const std::string& value) {
+  _internal_set_bind_string(value);
+  // @@protoc_insertion_point(field_set:Communication_proto.Communication_parameter.bind_string)
+}
+inline std::string* Communication_parameter::mutable_bind_string() {
+  // @@protoc_insertion_point(field_mutable:Communication_proto.Communication_parameter.bind_string)
+  return _internal_mutable_bind_string();
+}
+inline const std::string& Communication_parameter::_internal_bind_string() const {
+  return bind_string_.Get();
+}
+inline void Communication_parameter::_internal_set_bind_string(const std::string& value) {
+  _has_bits_[0] |= 0x00000001u;
+  bind_string_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), value, GetArena());
+}
+inline void Communication_parameter::set_bind_string(std::string&& value) {
+  _has_bits_[0] |= 0x00000001u;
+  bind_string_.Set(
+    &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::move(value), GetArena());
+  // @@protoc_insertion_point(field_set_rvalue:Communication_proto.Communication_parameter.bind_string)
+}
+inline void Communication_parameter::set_bind_string(const char* value) {
+  GOOGLE_DCHECK(value != nullptr);
+  _has_bits_[0] |= 0x00000001u;
+  bind_string_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(value),
+              GetArena());
+  // @@protoc_insertion_point(field_set_char:Communication_proto.Communication_parameter.bind_string)
+}
+inline void Communication_parameter::set_bind_string(const char* value,
+    size_t size) {
+  _has_bits_[0] |= 0x00000001u;
+  bind_string_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(
+      reinterpret_cast<const char*>(value), size), GetArena());
+  // @@protoc_insertion_point(field_set_pointer:Communication_proto.Communication_parameter.bind_string)
+}
+inline std::string* Communication_parameter::_internal_mutable_bind_string() {
+  _has_bits_[0] |= 0x00000001u;
+  return bind_string_.Mutable(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline std::string* Communication_parameter::release_bind_string() {
+  // @@protoc_insertion_point(field_release:Communication_proto.Communication_parameter.bind_string)
+  if (!_internal_has_bind_string()) {
+    return nullptr;
+  }
+  _has_bits_[0] &= ~0x00000001u;
+  return bind_string_.ReleaseNonDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline void Communication_parameter::set_allocated_bind_string(std::string* bind_string) {
+  if (bind_string != nullptr) {
+    _has_bits_[0] |= 0x00000001u;
+  } else {
+    _has_bits_[0] &= ~0x00000001u;
+  }
+  bind_string_.SetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), bind_string,
+      GetArena());
+  // @@protoc_insertion_point(field_set_allocated:Communication_proto.Communication_parameter.bind_string)
+}
+
+// repeated string connect_string_vector = 2;
+inline int Communication_parameter::_internal_connect_string_vector_size() const {
+  return connect_string_vector_.size();
+}
+inline int Communication_parameter::connect_string_vector_size() const {
+  return _internal_connect_string_vector_size();
+}
+inline void Communication_parameter::clear_connect_string_vector() {
+  connect_string_vector_.Clear();
+}
+inline std::string* Communication_parameter::add_connect_string_vector() {
+  // @@protoc_insertion_point(field_add_mutable:Communication_proto.Communication_parameter.connect_string_vector)
+  return _internal_add_connect_string_vector();
+}
+inline const std::string& Communication_parameter::_internal_connect_string_vector(int index) const {
+  return connect_string_vector_.Get(index);
+}
+inline const std::string& Communication_parameter::connect_string_vector(int index) const {
+  // @@protoc_insertion_point(field_get:Communication_proto.Communication_parameter.connect_string_vector)
+  return _internal_connect_string_vector(index);
+}
+inline std::string* Communication_parameter::mutable_connect_string_vector(int index) {
+  // @@protoc_insertion_point(field_mutable:Communication_proto.Communication_parameter.connect_string_vector)
+  return connect_string_vector_.Mutable(index);
+}
+inline void Communication_parameter::set_connect_string_vector(int index, const std::string& value) {
+  // @@protoc_insertion_point(field_set:Communication_proto.Communication_parameter.connect_string_vector)
+  connect_string_vector_.Mutable(index)->assign(value);
+}
+inline void Communication_parameter::set_connect_string_vector(int index, std::string&& value) {
+  // @@protoc_insertion_point(field_set:Communication_proto.Communication_parameter.connect_string_vector)
+  connect_string_vector_.Mutable(index)->assign(std::move(value));
+}
+inline void Communication_parameter::set_connect_string_vector(int index, const char* value) {
+  GOOGLE_DCHECK(value != nullptr);
+  connect_string_vector_.Mutable(index)->assign(value);
+  // @@protoc_insertion_point(field_set_char:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline void Communication_parameter::set_connect_string_vector(int index, const char* value, size_t size) {
+  connect_string_vector_.Mutable(index)->assign(
+    reinterpret_cast<const char*>(value), size);
+  // @@protoc_insertion_point(field_set_pointer:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline std::string* Communication_parameter::_internal_add_connect_string_vector() {
+  return connect_string_vector_.Add();
+}
+inline void Communication_parameter::add_connect_string_vector(const std::string& value) {
+  connect_string_vector_.Add()->assign(value);
+  // @@protoc_insertion_point(field_add:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline void Communication_parameter::add_connect_string_vector(std::string&& value) {
+  connect_string_vector_.Add(std::move(value));
+  // @@protoc_insertion_point(field_add:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline void Communication_parameter::add_connect_string_vector(const char* value) {
+  GOOGLE_DCHECK(value != nullptr);
+  connect_string_vector_.Add()->assign(value);
+  // @@protoc_insertion_point(field_add_char:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline void Communication_parameter::add_connect_string_vector(const char* value, size_t size) {
+  connect_string_vector_.Add()->assign(reinterpret_cast<const char*>(value), size);
+  // @@protoc_insertion_point(field_add_pointer:Communication_proto.Communication_parameter.connect_string_vector)
+}
+inline const ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField<std::string>&
+Communication_parameter::connect_string_vector() const {
+  // @@protoc_insertion_point(field_list:Communication_proto.Communication_parameter.connect_string_vector)
+  return connect_string_vector_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::RepeatedPtrField<std::string>*
+Communication_parameter::mutable_connect_string_vector() {
+  // @@protoc_insertion_point(field_mutable_list:Communication_proto.Communication_parameter.connect_string_vector)
+  return &connect_string_vector_;
+}
+
+// -------------------------------------------------------------------
+
+// Communication_parameter_all
+
+// optional .Communication_proto.Communication_parameter communication_parameters = 1;
+inline bool Communication_parameter_all::_internal_has_communication_parameters() const {
+  bool value = (_has_bits_[0] & 0x00000001u) != 0;
+  PROTOBUF_ASSUME(!value || communication_parameters_ != nullptr);
+  return value;
+}
+inline bool Communication_parameter_all::has_communication_parameters() const {
+  return _internal_has_communication_parameters();
+}
+inline void Communication_parameter_all::clear_communication_parameters() {
+  if (communication_parameters_ != nullptr) communication_parameters_->Clear();
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline const ::Communication_proto::Communication_parameter& Communication_parameter_all::_internal_communication_parameters() const {
+  const ::Communication_proto::Communication_parameter* p = communication_parameters_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::Communication_proto::Communication_parameter*>(
+      &::Communication_proto::_Communication_parameter_default_instance_);
+}
+inline const ::Communication_proto::Communication_parameter& Communication_parameter_all::communication_parameters() const {
+  // @@protoc_insertion_point(field_get:Communication_proto.Communication_parameter_all.communication_parameters)
+  return _internal_communication_parameters();
+}
+inline void Communication_parameter_all::unsafe_arena_set_allocated_communication_parameters(
+    ::Communication_proto::Communication_parameter* communication_parameters) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(communication_parameters_);
+  }
+  communication_parameters_ = communication_parameters;
+  if (communication_parameters) {
+    _has_bits_[0] |= 0x00000001u;
+  } else {
+    _has_bits_[0] &= ~0x00000001u;
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:Communication_proto.Communication_parameter_all.communication_parameters)
+}
+inline ::Communication_proto::Communication_parameter* Communication_parameter_all::release_communication_parameters() {
+  _has_bits_[0] &= ~0x00000001u;
+  ::Communication_proto::Communication_parameter* temp = communication_parameters_;
+  communication_parameters_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::Communication_proto::Communication_parameter* Communication_parameter_all::unsafe_arena_release_communication_parameters() {
+  // @@protoc_insertion_point(field_release:Communication_proto.Communication_parameter_all.communication_parameters)
+  _has_bits_[0] &= ~0x00000001u;
+  ::Communication_proto::Communication_parameter* temp = communication_parameters_;
+  communication_parameters_ = nullptr;
+  return temp;
+}
+inline ::Communication_proto::Communication_parameter* Communication_parameter_all::_internal_mutable_communication_parameters() {
+  _has_bits_[0] |= 0x00000001u;
+  if (communication_parameters_ == nullptr) {
+    auto* p = CreateMaybeMessage<::Communication_proto::Communication_parameter>(GetArena());
+    communication_parameters_ = p;
+  }
+  return communication_parameters_;
+}
+inline ::Communication_proto::Communication_parameter* Communication_parameter_all::mutable_communication_parameters() {
+  // @@protoc_insertion_point(field_mutable:Communication_proto.Communication_parameter_all.communication_parameters)
+  return _internal_mutable_communication_parameters();
+}
+inline void Communication_parameter_all::set_allocated_communication_parameters(::Communication_proto::Communication_parameter* communication_parameters) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete communication_parameters_;
+  }
+  if (communication_parameters) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      ::PROTOBUF_NAMESPACE_ID::Arena::GetArena(communication_parameters);
+    if (message_arena != submessage_arena) {
+      communication_parameters = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, communication_parameters, submessage_arena);
+    }
+    _has_bits_[0] |= 0x00000001u;
+  } else {
+    _has_bits_[0] &= ~0x00000001u;
+  }
+  communication_parameters_ = communication_parameters;
+  // @@protoc_insertion_point(field_set_allocated:Communication_proto.Communication_parameter_all.communication_parameters)
+}
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic pop
+#endif  // __GNUC__
+// -------------------------------------------------------------------
+
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace Communication_proto
+
+// @@protoc_insertion_point(global_scope)
+
+#include <google/protobuf/port_undef.inc>
+#endif  // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_communication_2eproto

Modules/MeasureNode/communication/communication.proto

@@ -0,0 +1,14 @@
+syntax = "proto2";
+package Communication_proto;
+
+message Communication_parameter
+{
+    optional string bind_string = 1;
+    repeated string connect_string_vector = 2;
+
+}
+
+message Communication_parameter_all
+{
+    optional Communication_parameter        communication_parameters=1;
+}

Modules/MeasureNode/communication/communication_message.cpp

@@ -0,0 +1,93 @@
+//
+// Created by huli on 2020/6/29.
+//
+
+#include "communication_message.h"
+
+
+Communication_message::Communication_message()
+{
+	m_message_type = eBase_msg;
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(5000);		//超时默认5秒
+	m_sender = eEmpty;
+	m_receiver = eEmpty;
+//	m_message_buf = "";
+}
+
+Communication_message::Communication_message(std::string message_buf)
+{
+	m_message_type = eBase_msg;
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(5000);		//超时默认5秒
+	m_sender = eEmpty;
+	m_receiver = eEmpty;
+	m_message_buf = message_buf;
+}
+
+Communication_message::Communication_message(char* p_buf, int size)
+{
+	m_message_type = eBase_msg;
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(5000);		//超时默认5秒
+	m_sender = eEmpty;
+	m_receiver = eEmpty;
+	m_message_buf = std::string(p_buf, size);
+}
+
+
+
+Communication_message::~Communication_message()
+{
+
+}
+
+bool Communication_message::is_over_time()
+{
+	if ( std::chrono::system_clock::now() - m_receive_time > m_timeout_ms)
+	{
+	    return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+
+void Communication_message::reset(const message::Base_info& base_info, std::string receive_string)
+{
+	m_message_type = (Message_type)(base_info.msg_type());
+
+	m_receive_time = std::chrono::system_clock::now();
+	m_timeout_ms = std::chrono::milliseconds(base_info.timeout_ms());
+	m_sender = (Communicator)(base_info.sender());
+	m_receiver = (Communicator)(base_info.receiver());
+	m_message_buf = receive_string;
+}
+
+Communication_message::Message_type Communication_message::get_message_type()
+{
+	return m_message_type;
+}
+Communication_message::Communicator Communication_message::get_sender()
+{
+	return m_sender;
+}
+Communication_message::Communicator Communication_message::get_receiver()
+{
+	return m_receiver;
+}
+std::string& Communication_message::get_message_buf()
+{
+	return m_message_buf;
+}
+
+std::chrono::system_clock::time_point Communication_message::get_receive_time()
+{
+	return m_receive_time;
+}
+
+
+
+

Modules/MeasureNode/communication/communication_message.h

@@ -0,0 +1,148 @@
+//
+// Created by huli on 2020/6/29.
+//
+
+#ifndef NNXX_TESTS_COMMUNICATION_MESSAGE_H
+#define NNXX_TESTS_COMMUNICATION_MESSAGE_H
+
+#include "error_code/error_code.hpp"
+
+#include <time.h>
+#include <sys/time.h>
+#include <chrono>
+//#include <iosfwd>
+
+#include <string>
+#include "../message/message_base.pb.h"
+
+
+class Communication_message
+{
+public:
+	//消息类型定义,每个在网络上传输的消息必须含有这个属性
+	enum Message_type
+	{
+		eBase_msg=0x00,
+		eCommand_msg=0x01,                      //指令消息
+
+
+		eLocate_status_msg=0x11,                //定位模块状态消息
+		eLocate_request_msg=0x12,               //定位请求消息
+		eLocate_response_msg=0x13,              //定位反馈消息
+
+		eLocate_sift_request_msg = 0x14,            //预测算法请求消息
+		eLocate_sift_response_msg = 0x15,           //预测算法反馈消息
+
+		eDispatch_status_msg=0x21,                	//调度模块硬件状态消息
+		eDispatch_request_msg=0x22,              	//请求调度消息
+		eDispatch_response_msg=0x23,             	//调度结果反馈消息
+
+		eParkspace_allocation_status_msg=0x31,  		//车位分配模块状态消息，包括车位信息
+		eParkspace_allocation_request_msg=0x32, 		//请求分配车位消息
+		eParkspace_allocation_response_msg=0x33,		//分配车位结果反馈消息
+		eParkspace_search_request_msg = 0x34,			//查询车位请求消息
+		eParkspace_search_response_msg = 0x35,			//查询车位反馈消息
+		eParkspace_release_request_msg = 0x36,			//释放车位请求消息
+		eParkspace_release_response_msg = 0x37,			//释放车位反馈消息
+		eParkspace_force_update_request_msg = 0x38,		//手动修改车位消息
+		eParkspace_force_update_response_msg = 0x39,	//手动修改车位反馈消息
+		eParkspace_confirm_alloc_request_msg = 0x3A,	//确认分配车位请求消息
+		eParkspace_confirm_alloc_response_msg = 0x3B,	//确认分配车位反馈消息
+        eParkspace_refresh_request_msg=0x3C,        	//请求更新车位数据
+        eParkspace_allocation_data_response_msg=0x3D,	//车位数据反馈消息
+		eParkspace_manual_search_request_msg = 0x3E,	//手动查询车位请求消息
+		eParkspace_manual_search_response_msg = 0x3F,	//手动查询车位反馈消息
+
+		eStore_command_request_msg=0x41,        	//终端停车请求消息
+		eStore_command_response_msg=0x42,       	//停车请求反馈消息
+		ePickup_command_request_msg=0x43,       	//取车请求消息
+		ePickup_command_response_msg=0x44,       	//取车请求反馈消息
+		eTerminal_status_msg = 0x50,	 			//终端状态消息
+
+
+		eStoring_process_statu_msg=0x90,        //停车指令进度条消息
+		ePicking_process_statu_msg=0x91,        //取车指令进度消息
+
+
+		eCentral_controller_statu_msg=0xa0,     //中控系统状态消息
+
+		eEntrance_manual_operation_msg=0xb0,            //针对出入口状态操作的手动消息
+		eProcess_manual_operation_msg=0xb1,             //针对流程的手动消息
+
+		eNotify_request_msg=0xc0,               //取车等候区通知请求
+		eNotify_response_msg=0xc1,              //等候区反馈
+		eNotify_status_msg=0xc2,                //等候区通知节点状态
+		eUnNormalized_module_statu_msg = 0xd0, //非标节点状态
+
+		eDispatch_plan_request_msg          = 0xe0,     //调度总规划的请求(用于启动整个调度算法)(调度管理->调度算法)
+		eDispatch_plan_response_msg         = 0xe1,     //调度总规划的答复(调度算法->调度管理)
+		eDispatch_control_request_msg       = 0xe2,     //调度控制的任务请求(调度算法->调度管理)
+		eDispatch_control_response_msg      = 0xe3,     //调度控制的任务答复(调度管理->调度算法)
+		eDispatch_manager_status_msg        = 0xea,     //调度管理的设备状态消息(调度底下所有硬件设备状态的汇总)
+		eDispatch_manager_data_msg          = 0xeb,     //调度管理的设备详细的数据信息
+		
+		
+		eGround_detect_request_msg=0xf0,        //地面雷达测量请求消息
+    	eGround_detect_response_msg=0xf1,       //地面雷达测量反馈消息
+		eGround_status_msg=0xf2,                //地面雷达状态消息
+
+
+	};
+
+//通讯单元
+	enum Communicator
+	{
+		eEmpty=0x0000,
+		eMain=0x0001,    //主流程
+
+		eTerminor=0x0100,
+		//车位表
+		eParkspace=0x0200,
+		//测量单元
+		eMeasurer=0x0300,
+		//测量单元
+		eMeasurer_sift_server=0x0301,
+
+		//调度机构
+		eDispatch_mamager=0x0400,
+		//调度机构
+		eDispatch_control=0x0401,
+		//取车等候区通知节点
+		eNotify=0x0501,
+
+		//地面测量单元
+		eGround_measurer=0x0f00,
+	};
+public:
+	Communication_message();
+	Communication_message(std::string message_buf);
+	Communication_message(char* p_buf, int size);
+	Communication_message(const Communication_message& other)= default;
+	Communication_message& operator =(const Communication_message& other)= default;
+	~Communication_message();
+public://API functions
+	bool is_over_time();
+public://get or set member variable
+	void reset(const message::Base_info& base_info, std::string receive_string);
+
+	Message_type get_message_type();
+	Communicator get_sender();
+	Communicator get_receiver();
+	std::string& get_message_buf();
+	std::chrono::system_clock::time_point get_receive_time();
+
+protected://member variable
+	Message_type								m_message_type;				//消息类型
+	std::chrono::system_clock::time_point		m_receive_time;				//接收消息的时间点
+	std::chrono::milliseconds					m_timeout_ms;				//超时时间, 整个软件都统一为毫秒
+	Communicator								m_sender;					//发送者
+	Communicator								m_receiver;					//接受者
+
+	std::string									m_message_buf;				//消息数据
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_COMMUNICATION_MESSAGE_H

Modules/MeasureNode/communication/communication_message.h.orig

@@ -0,0 +1,128 @@
+//
+// Created by huli on 2020/6/29.
+//
+
+#ifndef NNXX_TESTS_COMMUNICATION_MESSAGE_H
+#define NNXX_TESTS_COMMUNICATION_MESSAGE_H
+
+#include "../error_code/error_code.h"
+
+#include <time.h>
+#include <sys/time.h>
+#include <chrono>
+//#include <iosfwd>
+
+#include <string>
+#include "../message/message_base.pb.h"
+
+
+class Communication_message
+{
+public:
+	//消息类型定义,每个在网络上传输的消息必须含有这个属性
+	enum Message_type
+	{
+		eBase_msg=0x00,
+		eCommand_msg=0x01,                      //指令消息
+
+
+		eLocate_status_msg=0x11,                //定位模块状态消息
+		eLocate_request_msg=0x12,               //定位请求消息
+		eLocate_response_msg=0x13,              //定位反馈消息
+
+		eLocate_sift_request_msg = 0x14,            //预测算法请求消息
+		eLocate_sift_response_msg = 0x15,           //预测算法反馈消息
+
+		eDispatch_status_msg=0x21,                //调度模块硬件状态消息
+		eDispatch_request_msg=0x22,              //请求调度消息
+		eDispatch_response_msg=0x23,             //调度结果反馈消息
+
+		eParkspace_allocation_status_msg=0x31,  //车位分配模块状态消息，包括车位信息
+		eParkspace_allocation_request_msg=0x32, //请求分配车位消息
+		eParkspace_allocation_response_msg=0x33,//分配车位结果反馈消息
+		eParkspace_search_request_msg = 0x34,		//查询车位请求消息
+		eParkspace_search_response_msg = 0x35,		//查询车位反馈消息
+		eParkspace_release_request_msg = 0x36,		//释放车位请求消息
+		eParkspace_release_response_msg = 0x37,		//释放车位反馈消息
+		eParkspace_force_update_request_msg = 0x38,	//手动修改车位消息
+		eParkspace_force_update_response_msg = 0x39,//手动修改车位反馈消息
+		eParkspace_confirm_alloc_request_msg = 0x3A,//确认分配车位请求消息
+		eParkspace_confirm_alloc_response_msg = 0x3B,//确认分配车位反馈消息
+
+
+		eStore_command_request_msg=0x41,        //终端停车请求消息
+		eStore_command_response_msg=0x42,       //停车请求反馈消息
+		ePickup_command_request_msg=0x43,       //取车请求消息
+		ePickup_command_response_msg=0x44,       //取车请求反馈消息
+
+
+
+		eStoring_process_statu_msg=0x90,        //停车指令进度条消息
+		ePicking_process_statu_msg=0x91,        //取车指令进度消息
+
+
+		eCentral_controller_statu_msg=0xa0,     //中控系统状态消息
+
+
+		eEntrance_manual_operation_msg=0xb0,            //针对出入口状态操作的手动消息
+		eProcess_manual_operation_msg=0xb1,             //针对流程的手动消息
+
+
+		eGround_detect_request_msg=0xf0,        //地面雷达测量请求消息
+    	eGround_detect_response_msg=0xf1,       //地面雷达测量反馈消息
+
+	};
+
+//通讯单元
+	enum Communicator
+	{
+		eEmpty=0x0000,
+		eMain=0x0001,    //主流程
+
+		eTerminor=0x0100,
+		//车位表
+		eParkspace=0x0200,
+		//测量单元
+		eMeasurer=0x0300,
+		//测量单元
+		eMeasurer_sift_server=0x0301,
+		//调度机构
+		eDispatch=0x0400,
+		//...
+
+		//地面测量单元
+		eGround_measurer=0x0f00,
+	};
+public:
+	Communication_message();
+	Communication_message(std::string message_buf);
+	Communication_message(char* p_buf, int size);
+	Communication_message(const Communication_message& other)= default;
+	Communication_message& operator =(const Communication_message& other)= default;
+	~Communication_message();
+public://API functions
+	bool is_over_time();
+public://get or set member variable
+	void reset(const message::Base_info& base_info, std::string receive_string);
+
+	Message_type get_message_type();
+	Communicator get_sender();
+	Communicator get_receiver();
+	std::string& get_message_buf();
+	std::chrono::system_clock::time_point get_receive_time();
+
+protected://member variable
+	Message_type								m_message_type;				//消息类型
+	std::chrono::system_clock::time_point		m_receive_time;				//接收消息的时间点
+	std::chrono::milliseconds					m_timeout_ms;				//超时时间, 整个软件都统一为毫秒
+	Communicator								m_sender;					//发送者
+	Communicator								m_receiver;					//接受者
+
+	std::string									m_message_buf;				//消息数据
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_COMMUNICATION_MESSAGE_H

Modules/MeasureNode/communication/communication_socket_base.cpp

@@ -0,0 +1,608 @@
+
+
+
+#include "communication_socket_base.h"
+#include "tool/proto_tool.h"
+
+Communication_socket_base::Communication_socket_base()
+{
+	m_communication_statu = COMMUNICATION_UNKNOW;
+
+	mp_receive_data_thread = NULL;
+	mp_analysis_data_thread = NULL;
+	mp_send_data_thread = NULL;
+	mp_encapsulate_data_thread = NULL;
+
+	m_analysis_cycle_time = 1000;//默认1000ms,就自动解析(接受list)
+	m_encapsulate_cycle_time = 1000;//默认1000ms,就自动发送一次状态信息
+}
+
+Communication_socket_base::~Communication_socket_base()
+{
+	communication_uninit();
+}
+
+//初始化 通信 模块。如下三选一
+Error_manager Communication_socket_base::communication_init()
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+
+	return  communication_init_from_protobuf(COMMUNICATION_PARAMETER_PATH);
+}
+
+//初始化 通信 模块。从文件读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(std::string prototxt_path)
+{
+	Communication_proto::Communication_parameter_all t_communication_parameter_all;
+	if(!  proto_tool::read_proto_param(prototxt_path,t_communication_parameter_all) )
+	{
+		return Error_manager(COMMUNICATION_READ_PROTOBUF_ERROR,MINOR_ERROR,
+		"Communication_socket_base read_proto_param  failed");
+	}
+	return communication_init_from_protobuf(t_communication_parameter_all);
+}
+
+//初始化 通信 模块。从protobuf读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init_from_protobuf() run--- "<< this;
+	Error_manager t_error;
+
+	if ( communication_parameter_all.communication_parameters().has_bind_string() )
+	{
+		t_error = communication_bind(communication_parameter_all.communication_parameters().bind_string());
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+//	std::cout << "communication_parameter_all.communication_parameters().connect_string_vector_size() " <<
+//		communication_parameter_all.communication_parameters().connect_string_vector_size()<< std::endl;
+	for(int i=0;i<communication_parameter_all.communication_parameters().connect_string_vector_size();++i)
+	{
+		t_error = communication_connect( communication_parameter_all.communication_parameters().connect_string_vector(i) );
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+
+//初始化
+Error_manager Communication_socket_base::communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+	Error_manager t_error;
+
+	t_error = communication_bind(bind_string);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	t_error = communication_connect(connect_string_vector);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+//bind
+Error_manager Communication_socket_base::communication_bind(std::string bind_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+
+	//m_socket 自己作为一个服务器, 绑定一个端口
+	t_socket_result = m_socket.bind(bind_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_BIND_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.bind error ");
+	}
+	LOG(INFO) << " ---Communication_socket_base::communication_bind() bind::  "<< bind_string << "  " << this;
+
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::vector<std::string>& connect_string_vector)
+{
+	Error_manager t_error;
+	for (auto iter = connect_string_vector.begin(); iter != connect_string_vector.end(); ++iter)
+	{
+		t_error = communication_connect(*iter);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::string connect_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+	//m_socket 和远端通信, 连接远端服务器的端口
+	t_socket_result = m_socket.connect(connect_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_CONNECT_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.connect error ");
+	}
+	LOG(INFO) << " ---Communication_socket_base::communication_connect() connect::  "<< connect_string << "  " << this;
+
+	return Error_code::SUCCESS;
+}
+//启动通信, run thread
+Error_manager Communication_socket_base::communication_run()
+{
+	m_communication_statu = COMMUNICATION_READY;
+	//启动4个线程。
+	//接受线程默认循环, 内部的nn_recv进行等待, 超时1ms
+	m_receive_condition.reset(false, false, false);
+	mp_receive_data_thread = new std::thread(&Communication_socket_base::receive_data_thread, this);
+	//解析线程默认等待, 需要接受线程去唤醒, 超时1ms, 超时后主动遍历m_receive_data_list
+	m_analysis_data_condition.reset(false, false, false);
+	mp_analysis_data_thread = new std::thread(&Communication_socket_base::analysis_data_thread, this);
+	//发送线程默认循环, 内部的wait_and_pop进行等待,
+	m_send_data_condition.reset(false, true, false);
+	mp_send_data_thread = new std::thread(&Communication_socket_base::send_data_thread, this);
+	//封装线程默认等待, ...., 超时1ms, 超时后主动 封装心跳和状态信息,
+	m_encapsulate_data_condition.reset(false, false, false);
+	mp_encapsulate_data_thread = new std::thread(&Communication_socket_base::encapsulate_data_thread, this);
+
+	return Error_code::SUCCESS;
+}
+
+
+//反初始化 通信 模块。
+Error_manager Communication_socket_base::communication_uninit()
+{
+	//终止list，防止 wait_and_pop 阻塞线程。
+	m_receive_data_list.termination_list();
+	m_send_data_list.termination_list();
+
+	//杀死4个线程，强制退出
+	if (mp_receive_data_thread)
+	{
+		m_receive_condition.kill_all();
+	}
+	if (mp_analysis_data_thread)
+	{
+		m_analysis_data_condition.kill_all();
+	}
+	if (mp_send_data_thread)
+	{
+		m_send_data_condition.kill_all();
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		m_encapsulate_data_condition.kill_all();
+	}
+	//回收4个线程的资源
+	if (mp_receive_data_thread)
+	{
+		mp_receive_data_thread->join();
+		delete mp_receive_data_thread;
+		mp_receive_data_thread = NULL;
+	}
+	if (mp_analysis_data_thread)
+	{
+		mp_analysis_data_thread->join();
+		delete mp_analysis_data_thread;
+		mp_analysis_data_thread = 0;
+	}
+	if (mp_send_data_thread)
+	{
+		mp_send_data_thread->join();
+		delete mp_send_data_thread;
+		mp_send_data_thread = NULL;
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		mp_encapsulate_data_thread->join();
+		delete mp_encapsulate_data_thread;
+		mp_encapsulate_data_thread = NULL;
+	}
+
+	//清空list
+	m_receive_data_list.clear_and_delete();
+	m_send_data_list.clear_and_delete();
+
+	m_communication_statu = COMMUNICATION_UNKNOW;
+	m_socket.close();
+
+	return Error_code::SUCCESS;
+}
+
+
+void Communication_socket_base::set_analysis_cycle_time(unsigned int analysis_cycle_time)
+{
+	m_analysis_cycle_time = analysis_cycle_time;
+}
+void Communication_socket_base::set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time)
+{
+	m_encapsulate_cycle_time = encapsulate_cycle_time;
+}
+
+
+
+
+//mp_receive_data_thread 接受线程执行函数，
+//receive_data_thread 内部线程负责接受消息
+void Communication_socket_base::receive_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::receive_data_thread start "<< this;
+
+	//通信接受线程, 负责接受socket消息, 并存入 m_receive_data_list
+	while (m_receive_condition.is_alive())
+	{
+		m_receive_condition.wait_for_ex(std::chrono::microseconds(1));
+		if ( m_receive_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			std::string t_receive_string;
+			{//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+				std::unique_lock<std::mutex> lk(m_mutex);
+				//flags为1, 非阻塞接受消息, 如果接收到消息, 那么接受数据长度大于0
+				t_receive_string = m_socket.recv<std::string>(1);
+			}
+			if ( t_receive_string.size()>0 )
+			{
+				//如果这里接受到了消息, 在这提前解析消息最前面的Base_msg (消息公共内容), 用于后续的check
+				message::Base_msg t_base_msg;
+				if( t_base_msg.ParseFromString(t_receive_string) )
+				{
+					//第一次解析之后转化为, Communication_message, 自定义的通信消息格式
+					Communication_message  * tp_communication_message = new Communication_message;
+					tp_communication_message->reset(t_base_msg.base_info(), t_receive_string);
+					//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+					if ( check_msg(tp_communication_message) == SUCCESS )
+					{
+						bool is_push = m_receive_data_list.push(tp_communication_message);
+						//push成功之后, tp_communication_message内存的管理权限交给链表, 如果失败就要回收内存
+						if ( is_push )
+						{
+							//唤醒解析线程一次,
+							m_analysis_data_condition.notify_all(false, true);
+						}
+						else
+						{
+//						push失败, 就要回收内存
+							delete(tp_communication_message);
+							tp_communication_message = NULL;
+//					return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//										 " m_receive_data_list.push error ");
+						}
+					}
+					else
+					{
+						delete(tp_communication_message);
+						tp_communication_message = NULL;
+					}
+
+
+				}
+				//解析失败, 就当做什么也没发生, 认为接收消息无效,
+			}
+			//没有接受到消息, 返回空字符串
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::receive_data_thread end "<< this;
+	return;
+}
+
+//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+Error_manager Communication_socket_base::check_msg(Communication_message*  p_msg)
+{
+	//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 判断这条消息是不是给我的.
+	//子类重载时, 增加自己模块的判断逻辑, 以后再写.
+	if ( p_msg->get_message_type() == Communication_message::Message_type::eBase_msg
+		 && p_msg->get_receiver() == Communication_message::Communicator::eMain )
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		//无效的消息,
+		return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
+							 " INVALID_MESSAGE error ");	}
+}
+
+//mp_analysis_data_thread 解析线程执行函数，
+//analysis_data_thread 内部线程负责解析消息
+void Communication_socket_base::analysis_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread start "<< this;
+
+	//通信解析线程, 负责巡检m_receive_data_list, 并解析和处理消息
+	while (m_analysis_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_analysis_data_condition.wait_for_millisecond(m_analysis_cycle_time);
+		if ( m_analysis_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果解析线程被主动唤醒, 那么就表示 收到新的消息, 那就遍历整个链表
+			if ( t_pass_flag )
+			{
+				analysis_receive_list();
+			}
+				//如果解析线程超时通过, 那么就定时处理链表残留的消息,
+			else
+			{
+				analysis_receive_list();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread end "<< this;
+	return;
+}
+
+//循环接受链表, 解析消息,
+Error_manager Communication_socket_base::analysis_receive_list()
+{
+	Error_manager t_error;
+	if ( m_receive_data_list.m_termination_flag )
+	{
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::analysis_receive_list error ");
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_receive_data_list.m_mutex);
+		for (auto iter = m_receive_data_list.m_data_list.begin(); iter != m_receive_data_list.m_data_list.end(); )
+		{
+			Communication_message* tp_msg = **iter;
+			if ( tp_msg == NULL )
+			{
+				iter = m_receive_data_list.m_data_list.erase(iter);
+				//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+			}
+			else
+			{
+				//检查消息是否可以被处理
+				t_error = check_executer(tp_msg);
+				if ( t_error == SUCCESS)
+				{
+					//处理消息
+					t_error = execute_msg(tp_msg);
+//				if ( t_error )
+//				{
+//					//执行结果不管
+//				}
+//				else
+//				{
+//					//执行结果不管
+//				}
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+				}
+				else if( t_error == COMMUNICATION_EXCUTER_IS_BUSY)
+				{
+					//处理器正忙, 那就不做处理, 直接处理下一个
+					//注:这条消息就被保留了下来, wait_for_millisecond 超时通过之后, 会循环检查残留的消息.
+					iter++;
+				}
+				else //if( t_error == COMMUNICATION_ANALYSIS_TIME_OUT )
+				{
+					//超时了就直接删除
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+
+					//注:消息删除之后, 不需要发送答复消息, 发送方也会有超时处理的,  只有 execute_msg 里面可以答复消息
+				}
+			}
+		}
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+Error_manager Communication_socket_base::check_executer(Communication_message*  p_msg)
+{
+	//检查对应模块的状态, 判断是否可以处理这条消息
+	//同时也要判断是否超时, 超时返回 COMMUNICATION_ANALYSIS_TIME_OUT
+	//如果处理器正在忙别的, 那么返回 COMMUNICATION_EXCUTER_IS_BUSY
+
+	if ( p_msg->is_over_time() )
+	{
+		std::cout << "Communication_socket_base::check_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+		std::cout << "Communication_socket_base::check_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+		std::cout << "COMMUNICATION_ANALYSIS_TIME_OUT , " << std::endl;
+		return Error_code::COMMUNICATION_ANALYSIS_TIME_OUT;
+	}
+	else
+	{
+		bool executer_is_ready = false;
+		//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 找到处理模块的实例对象, 查询执行人是否可以处理这条消息
+		//这里子类重载时, 增加判断逻辑, 以后再写.
+		executer_is_ready = true;
+
+		std::cout << "Communication_socket_base::check_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+		std::cout << "Communication_socket_base::check_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+
+		if ( executer_is_ready )
+		{
+			std::cout << "executer_is_ready , " << std::endl;
+			return Error_code::SUCCESS;
+		}
+		else
+		{
+			std::cout << "executer_is_busy , " << std::endl;
+			return Error_code::COMMUNICATION_EXCUTER_IS_BUSY;
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//处理消息
+Error_manager Communication_socket_base::execute_msg(Communication_message*  p_msg)
+{
+	//先将 p_msg 转化为 对应的格式, 使用对应模块的protobuf来二次解析
+	// 不能一直使用 Communication_message*  p_msg, 这个是要销毁的
+	//然后处理这个消息, 就是调用对应模块的 execute 接口函数
+	//执行结果不管, 如果需要答复, 那么对应模块 在自己内部 封装一条消息发送即可.
+	//子类重载, 需要完全重写, 以后再写.
+
+	//注注注注注意了, 本模块只是用来做通信,
+	//在做处理消息的时候, 可能会调用执行者的接口函数,
+	//这里不应该长时间阻塞或者处理复杂的逻辑,
+	//请执行者另开线程来处理任务.
+
+	std::cout << "Communication_socket_base::excute_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+	std::cout << "Communication_socket_base::excute_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+	return Error_code::SUCCESS;
+}
+
+//mp_send_data_thread 发送线程执行函数，
+//send_data_thread 内部线程负责发送消息
+void Communication_socket_base::send_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::send_data_thread start "<< this;
+
+	//通信发送线程, 负责巡检m_send_data_list, 并发送消息
+	while (m_send_data_condition.is_alive())
+	{
+		m_send_data_condition.wait();
+		if ( m_send_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			Communication_message* tp_msg = NULL;
+			//这里 wait_and_pop 会使用链表内部的 m_data_cond 条件变量来控制等待,
+			//封装线程使用push的时候, 会唤醒线程并通过等待, 此时 m_send_data_condition 是一直通过的.
+			//如果需要退出, 那么就要 m_send_data_list.termination_list();	和 m_send_data_condition.kill_all();
+			bool is_pop = m_send_data_list.wait_and_pop(tp_msg);
+			if ( is_pop )
+			{
+				if ( tp_msg != NULL )
+				{
+					{//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+						std::unique_lock<std::mutex> lk(m_mutex);
+						m_socket.send(tp_msg->get_message_buf());
+					}
+					delete(tp_msg);
+					tp_msg = NULL;
+				}
+			}
+			else
+			{
+				//没有取出, 那么应该就是 m_termination_flag 结束了
+//				return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//									 " Communication_socket_base::send_data_thread() error ");
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::send_data_thread end "<< this;
+	return;
+}
+
+//mp_encapsulate_data_thread 封装线程执行函数，
+//encapsulate_data_thread 内部线程负责封装消息
+void Communication_socket_base::encapsulate_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread start "<< this;
+
+	//通信封装线程, 负责定时封装消息, 并存入 m_send_data_list
+	while (m_encapsulate_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_encapsulate_data_condition.wait_for_millisecond(m_encapsulate_cycle_time);
+
+		if ( m_encapsulate_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果封装线程被主动唤醒, 那么就表示 需要主动发送消息,
+			if ( t_pass_flag )
+			{
+				//主动发送消息,
+			}
+				//如果封装线程超时通过, 那么就定时封装心跳和状态信息
+			else
+			{
+				encapsulate_send_data();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread end "<< this;
+	return;
+}
+
+
+//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_send_data()
+{
+//	char buf[256] = {0};
+//	static unsigned int t_heartbeat = 0;
+//	sprintf(buf, "Communication_socket_base, heartbeat = %d\0\0\0, test\0", t_heartbeat);
+//	t_heartbeat++;
+    return SUCCESS;
+	message::Base_msg t_base_msg;
+	t_base_msg.mutable_base_info()->set_msg_type(message::Message_type::eBase_msg);
+	t_base_msg.mutable_base_info()->set_timeout_ms(5000);
+	t_base_msg.mutable_base_info()->set_sender(message::Communicator::eMain);
+	t_base_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
+
+	Communication_message* tp_msg = new Communication_message(t_base_msg.SerializeAsString());
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(std::string message)
+{
+	Communication_message* tp_msg = new Communication_message(message);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(Communication_message* p_msg)
+{
+	Communication_message* tp_msg = new Communication_message(*p_msg);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}

Modules/MeasureNode/communication/communication_socket_base.cpp.orig

@@ -0,0 +1,610 @@
+
+
+
+#include "communication_socket_base.h"
+#include "../tool/proto_tool.h"
+
+Communication_socket_base::Communication_socket_base()
+{
+	m_communication_statu = COMMUNICATION_UNKNOW;
+
+	mp_receive_data_thread = NULL;
+	mp_analysis_data_thread = NULL;
+	mp_send_data_thread = NULL;
+	mp_encapsulate_data_thread = NULL;
+
+	m_analysis_cycle_time = 1000;//默认1000ms,就自动解析(接受list)
+	m_encapsulate_cycle_time = 1000;//默认1000ms,就自动发送一次状态信息
+}
+
+Communication_socket_base::~Communication_socket_base()
+{
+	communication_uninit();
+}
+
+//初始化 通信 模块。如下三选一
+Error_manager Communication_socket_base::communication_init()
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+
+	return  communication_init_from_protobuf(COMMUNICATION_PARAMETER_PATH);
+}
+
+//初始化 通信 模块。从文件读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(std::string prototxt_path)
+{
+	Communication_proto::Communication_parameter_all t_communication_parameter_all;
+	if(!  proto_tool::read_proto_param(prototxt_path,t_communication_parameter_all) )
+	{
+		return Error_manager(COMMUNICATION_READ_PROTOBUF_ERROR,MINOR_ERROR,
+		"Communication_socket_base read_proto_param  failed");
+	}
+
+	return communication_init_from_protobuf(t_communication_parameter_all);
+}
+
+//初始化 通信 模块。从protobuf读取
+Error_manager Communication_socket_base::communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init_from_protobuf() run--- "<< this;
+	Error_manager t_error;
+
+	if ( communication_parameter_all.communication_parameters().has_bind_string() )
+	{
+		t_error = communication_bind(communication_parameter_all.communication_parameters().bind_string());
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	std::cout << "communication_parameter_all.communication_parameters().connect_string_vector_size() " <<
+		communication_parameter_all.communication_parameters().connect_string_vector_size()<< std::endl;
+	for(int i=0;i<communication_parameter_all.communication_parameters().connect_string_vector_size();++i)
+	{
+		t_error = communication_connect( communication_parameter_all.communication_parameters().connect_string_vector(i) );
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+
+//初始化
+Error_manager Communication_socket_base::communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+	Error_manager t_error;
+
+	t_error = communication_bind(bind_string);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	t_error = communication_connect(connect_string_vector);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+//bind
+Error_manager Communication_socket_base::communication_bind(std::string bind_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+
+	//m_socket 自己作为一个服务器, 绑定一个端口
+	t_socket_result = m_socket.bind(bind_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_BIND_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.bind error ");
+	}
+	LOG(INFO) << " ---Communication_socket_base::communication_bind() bind::  "<< bind_string << "  " << this;
+
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::vector<std::string>& connect_string_vector)
+{
+	Error_manager t_error;
+	for (auto iter = connect_string_vector.begin(); iter != connect_string_vector.end(); ++iter)
+	{
+		t_error = communication_connect(*iter);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::string connect_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+	//m_socket 和远端通信, 连接远端服务器的端口
+	t_socket_result = m_socket.connect(connect_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_CONNECT_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.connect error ");
+	}
+	LOG(INFO) << " ---Communication_socket_base::communication_connect() connect::  "<< connect_string << "  " << this;
+
+	return Error_code::SUCCESS;
+}
+//启动通信, run thread
+Error_manager Communication_socket_base::communication_run()
+{
+	m_communication_statu = COMMUNICATION_READY;
+	//启动4个线程。
+	//接受线程默认循环, 内部的nn_recv进行等待, 超时1ms
+	m_receive_condition.reset(false, false, false);
+	mp_receive_data_thread = new std::thread(&Communication_socket_base::receive_data_thread, this);
+	//解析线程默认等待, 需要接受线程去唤醒, 超时1ms, 超时后主动遍历m_receive_data_list
+	m_analysis_data_condition.reset(false, false, false);
+	mp_analysis_data_thread = new std::thread(&Communication_socket_base::analysis_data_thread, this);
+	//发送线程默认循环, 内部的wait_and_pop进行等待,
+	m_send_data_condition.reset(false, true, false);
+	mp_send_data_thread = new std::thread(&Communication_socket_base::send_data_thread, this);
+	//封装线程默认等待, ...., 超时1ms, 超时后主动 封装心跳和状态信息,
+	m_encapsulate_data_condition.reset(false, false, false);
+	mp_encapsulate_data_thread = new std::thread(&Communication_socket_base::encapsulate_data_thread, this);
+
+	return Error_code::SUCCESS;
+}
+
+
+//反初始化 通信 模块。
+Error_manager Communication_socket_base::communication_uninit()
+{
+	//终止list，防止 wait_and_pop 阻塞线程。
+	m_receive_data_list.termination_list();
+	m_send_data_list.termination_list();
+
+	//杀死4个线程，强制退出
+	if (mp_receive_data_thread)
+	{
+		m_receive_condition.kill_all();
+	}
+	if (mp_analysis_data_thread)
+	{
+		m_analysis_data_condition.kill_all();
+	}
+	if (mp_send_data_thread)
+	{
+		m_send_data_condition.kill_all();
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		m_encapsulate_data_condition.kill_all();
+	}
+	//回收4个线程的资源
+	if (mp_receive_data_thread)
+	{
+		mp_receive_data_thread->join();
+		delete mp_receive_data_thread;
+		mp_receive_data_thread = NULL;
+	}
+	if (mp_analysis_data_thread)
+	{
+		mp_analysis_data_thread->join();
+		delete mp_analysis_data_thread;
+		mp_analysis_data_thread = 0;
+	}
+	if (mp_send_data_thread)
+	{
+		mp_send_data_thread->join();
+		delete mp_send_data_thread;
+		mp_send_data_thread = NULL;
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		mp_encapsulate_data_thread->join();
+		delete mp_encapsulate_data_thread;
+		mp_encapsulate_data_thread = NULL;
+	}
+
+	//清空list
+	m_receive_data_list.clear_and_delete();
+	m_send_data_list.clear_and_delete();
+
+	m_communication_statu = COMMUNICATION_UNKNOW;
+	m_socket.close();
+
+	return Error_code::SUCCESS;
+}
+
+
+void Communication_socket_base::set_analysis_cycle_time(unsigned int analysis_cycle_time)
+{
+	m_analysis_cycle_time = analysis_cycle_time;
+}
+void Communication_socket_base::set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time)
+{
+	m_encapsulate_cycle_time = encapsulate_cycle_time;
+}
+
+
+
+
+//mp_receive_data_thread 接受线程执行函数，
+//receive_data_thread 内部线程负责接受消息
+void Communication_socket_base::receive_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::receive_data_thread start "<< this;
+
+	//通信接受线程, 负责接受socket消息, 并存入 m_receive_data_list
+	while (m_receive_condition.is_alive())
+	{
+		m_receive_condition.wait_for_ex(std::chrono::microseconds(1));
+		if ( m_receive_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			std::string t_receive_string;
+			{//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+				std::unique_lock<std::mutex> lk(m_mutex);
+				//flags为1, 非阻塞接受消息, 如果接收到消息, 那么接受数据长度大于0
+				t_receive_string = m_socket.recv<std::string>(1);
+			}
+			if ( t_receive_string.size()>0 )
+			{
+				//如果这里接受到了消息, 在这提前解析消息最前面的Base_msg (消息公共内容), 用于后续的check
+				message::Base_msg t_base_msg;
+				if( t_base_msg.ParseFromString(t_receive_string) )
+				{
+					//第一次解析之后转化为, Communication_message, 自定义的通信消息格式
+					Communication_message  * tp_communication_message = new Communication_message;
+					tp_communication_message->reset(t_base_msg.base_info(), t_receive_string);
+					//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+					if ( check_msg(tp_communication_message) == SUCCESS )
+					{
+						bool is_push = m_receive_data_list.push(tp_communication_message);
+						//push成功之后, tp_communication_message内存的管理权限交给链表, 如果失败就要回收内存
+						if ( is_push )
+						{
+							//唤醒解析线程一次,
+							m_analysis_data_condition.notify_all(false, true);
+						}
+						else
+						{
+//						push失败, 就要回收内存
+							delete(tp_communication_message);
+							tp_communication_message = NULL;
+//					return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//										 " m_receive_data_list.push error ");
+						}
+					}
+					else
+					{
+						delete(tp_communication_message);
+						tp_communication_message = NULL;
+					}
+
+
+				}
+				//解析失败, 就当做什么也没发生, 认为接收消息无效,
+			}
+			//没有接受到消息, 返回空字符串
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::receive_data_thread end "<< this;
+	return;
+}
+
+//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+Error_manager Communication_socket_base::check_msg(Communication_message*  p_msg)
+{
+	//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 判断这条消息是不是给我的.
+	//子类重载时, 增加自己模块的判断逻辑, 以后再写.
+	if ( p_msg->get_message_type() == Communication_message::Message_type::eBase_msg
+		 && p_msg->get_receiver() == Communication_message::Communicator::eMain )
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		//无效的消息,
+		return Error_manager(Error_code::INVALID_MESSAGE, Error_level::NEGLIGIBLE_ERROR,
+							 " INVALID_MESSAGE error ");	}
+}
+
+//mp_analysis_data_thread 解析线程执行函数，
+//analysis_data_thread 内部线程负责解析消息
+void Communication_socket_base::analysis_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread start "<< this;
+
+	//通信解析线程, 负责巡检m_receive_data_list, 并解析和处理消息
+	while (m_analysis_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_analysis_data_condition.wait_for_millisecond(m_analysis_cycle_time);
+		if ( m_analysis_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果解析线程被主动唤醒, 那么就表示 收到新的消息, 那就遍历整个链表
+			if ( t_pass_flag )
+			{
+				analysis_receive_list();
+			}
+				//如果解析线程超时通过, 那么就定时处理链表残留的消息,
+			else
+			{
+				analysis_receive_list();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread end "<< this;
+	return;
+}
+
+//循环接受链表, 解析消息,
+Error_manager Communication_socket_base::analysis_receive_list()
+{
+	Error_manager t_error;
+	if ( m_receive_data_list.m_termination_flag )
+	{
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::analysis_receive_list error ");
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_receive_data_list.m_mutex);
+		for (auto iter = m_receive_data_list.m_data_list.begin(); iter != m_receive_data_list.m_data_list.end(); )
+		{
+			Communication_message* tp_msg = **iter;
+			if ( tp_msg == NULL )
+			{
+				iter = m_receive_data_list.m_data_list.erase(iter);
+				//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+			}
+			else
+			{
+				//检查消息是否可以被处理
+				t_error = check_executer(tp_msg);
+				if ( t_error == SUCCESS)
+				{
+					//处理消息
+					t_error = execute_msg(tp_msg);
+//				if ( t_error )
+//				{
+//					//执行结果不管
+//				}
+//				else
+//				{
+//					//执行结果不管
+//				}
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+				}
+				else if( t_error == COMMUNICATION_EXCUTER_IS_BUSY)
+				{
+					//处理器正忙, 那就不做处理, 直接处理下一个
+					//注:这条消息就被保留了下来, wait_for_millisecond 超时通过之后, 会循环检查残留的消息.
+					iter++;
+				}
+				else //if( t_error == COMMUNICATION_ANALYSIS_TIME_OUT )
+				{
+					//超时了就直接删除
+					delete(tp_msg);
+					tp_msg = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+
+					//注:消息删除之后, 不需要发送答复消息, 发送方也会有超时处理的,  只有 execute_msg 里面可以答复消息
+				}
+			}
+		}
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+Error_manager Communication_socket_base::check_executer(Communication_message*  p_msg)
+{
+	//检查对应模块的状态, 判断是否可以处理这条消息
+	//同时也要判断是否超时, 超时返回 COMMUNICATION_ANALYSIS_TIME_OUT
+	//如果处理器正在忙别的, 那么返回 COMMUNICATION_EXCUTER_IS_BUSY
+
+	if ( p_msg->is_over_time() )
+	{
+		std::cout << "Communication_socket_base::check_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+		std::cout << "Communication_socket_base::check_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+		std::cout << "COMMUNICATION_ANALYSIS_TIME_OUT , " << std::endl;
+		return Error_code::COMMUNICATION_ANALYSIS_TIME_OUT;
+	}
+	else
+	{
+		bool executer_is_ready = false;
+		//通过 p_msg->get_message_type() 和 p_msg->get_receiver() 找到处理模块的实例对象, 查询执行人是否可以处理这条消息
+		//这里子类重载时, 增加判断逻辑, 以后再写.
+		executer_is_ready = true;
+
+		std::cout << "Communication_socket_base::check_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+		std::cout << "Communication_socket_base::check_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+
+		if ( executer_is_ready )
+		{
+			std::cout << "executer_is_ready , " << std::endl;
+			return Error_code::SUCCESS;
+		}
+		else
+		{
+			std::cout << "executer_is_busy , " << std::endl;
+			return Error_code::COMMUNICATION_EXCUTER_IS_BUSY;
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//处理消息
+Error_manager Communication_socket_base::execute_msg(Communication_message*  p_msg)
+{
+	//先将 p_msg 转化为 对应的格式, 使用对应模块的protobuf来二次解析
+	// 不能一直使用 Communication_message*  p_msg, 这个是要销毁的
+	//然后处理这个消息, 就是调用对应模块的 execute 接口函数
+	//执行结果不管, 如果需要答复, 那么对应模块 在自己内部 封装一条消息发送即可.
+	//子类重载, 需要完全重写, 以后再写.
+
+	//注注注注注意了, 本模块只是用来做通信,
+	//在做处理消息的时候, 可能会调用执行者的接口函数,
+	//这里不应该长时间阻塞或者处理复杂的逻辑,
+	//请执行者另开线程来处理任务.
+
+	std::cout << "Communication_socket_base::excute_msg  p_buf =  " << p_msg->get_message_buf() << std::endl;
+	std::cout << "Communication_socket_base::excute_msg   size =  " << p_msg->get_message_buf().size() << std::endl;
+	return Error_code::SUCCESS;
+}
+
+//mp_send_data_thread 发送线程执行函数，
+//send_data_thread 内部线程负责发送消息
+void Communication_socket_base::send_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::send_data_thread start "<< this;
+
+	//通信发送线程, 负责巡检m_send_data_list, 并发送消息
+	while (m_send_data_condition.is_alive())
+	{
+		m_send_data_condition.wait();
+		if ( m_send_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			Communication_message* tp_msg = NULL;
+			//这里 wait_and_pop 会使用链表内部的 m_data_cond 条件变量来控制等待,
+			//封装线程使用push的时候, 会唤醒线程并通过等待, 此时 m_send_data_condition 是一直通过的.
+			//如果需要退出, 那么就要 m_send_data_list.termination_list();	和 m_send_data_condition.kill_all();
+			bool is_pop = m_send_data_list.wait_and_pop(tp_msg);
+			if ( is_pop )
+			{
+				if ( tp_msg != NULL )
+				{
+					{//这个大括号表示只对 recv 和 send 加锁, 不要因为后面的复杂逻辑影响通信效率
+						std::unique_lock<std::mutex> lk(m_mutex);
+						m_socket.send(tp_msg->get_message_buf());
+					}
+					delete(tp_msg);
+					tp_msg = NULL;
+				}
+			}
+			else
+			{
+				//没有取出, 那么应该就是 m_termination_flag 结束了
+//				return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//									 " Communication_socket_base::send_data_thread() error ");
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::send_data_thread end "<< this;
+	return;
+}
+
+//mp_encapsulate_data_thread 封装线程执行函数，
+//encapsulate_data_thread 内部线程负责封装消息
+void Communication_socket_base::encapsulate_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread start "<< this;
+
+	//通信封装线程, 负责定时封装消息, 并存入 m_send_data_list
+	while (m_encapsulate_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_encapsulate_data_condition.wait_for_millisecond(m_encapsulate_cycle_time);
+
+		if ( m_encapsulate_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果封装线程被主动唤醒, 那么就表示 需要主动发送消息,
+			if ( t_pass_flag )
+			{
+				//主动发送消息,
+			}
+				//如果封装线程超时通过, 那么就定时封装心跳和状态信息
+			else
+			{
+				encapsulate_send_data();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread end "<< this;
+	return;
+}
+
+
+//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_send_data()
+{
+//	char buf[256] = {0};
+//	static unsigned int t_heartbeat = 0;
+//	sprintf(buf, "Communication_socket_base, heartbeat = %d\0\0\0, test\0", t_heartbeat);
+//	t_heartbeat++;
+    return SUCCESS;
+	message::Base_msg t_base_msg;
+	t_base_msg.mutable_base_info()->set_msg_type(message::Message_type::eBase_msg);
+	t_base_msg.mutable_base_info()->set_timeout_ms(5000);
+	t_base_msg.mutable_base_info()->set_sender(message::Communicator::eMain);
+	t_base_msg.mutable_base_info()->set_receiver(message::Communicator::eMain);
+
+	Communication_message* tp_msg = new Communication_message(t_base_msg.SerializeAsString());
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(std::string message)
+{
+	Communication_message* tp_msg = new Communication_message(message);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(Communication_message* p_msg)
+{
+	Communication_message* tp_msg = new Communication_message(*p_msg);
+	bool is_push = m_send_data_list.push(tp_msg);
+	if ( is_push == false )
+	{
+		delete(tp_msg);
+		tp_msg = NULL;
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}

Modules/MeasureNode/communication/communication_socket_base.h

@@ -0,0 +1,162 @@
+
+
+/*
+ * communication_socket_base 通信模块的基类,
+ * 用户从这个基类继承, 初始化之后, 便可以自动进行通信
+ * 重载解析消息和封装消息,
+ *
+ *Thread_safe_list<Binary_buf*> 使用 Binary_buf , 而不是string
+ * 主要是为了支持直接发送数字0
+ * 
+ * 
+ * */
+
+#ifndef __COMMUNICATION_SOCKET_BASE__HH__
+#define __COMMUNICATION_SOCKET_BASE__HH__
+
+#include <mutex>
+#include <thread>
+#include <nnxx/message>
+#include <nnxx/socket.h>
+#include <nnxx/bus.h>
+
+#include <glog/logging.h>
+#include "error_code/error_code.hpp"
+#include "tool/binary_buf.h"
+#include "tool/thread_safe_list.hpp"
+#include "tool/thread_condition.h"
+
+#include "../communication/communication.pb.h"
+#include "../communication/communication_message.h"
+
+#include "../message/message_base.pb.h"
+//#include "../message/measure_message.pb.h"
+
+class Communication_socket_base
+{
+	//通信状态
+	enum Communication_statu
+	{
+		COMMUNICATION_UNKNOW		=0,	        //通信状态 未知
+		COMMUNICATION_READY			=1,			//通信状态 正常
+
+		COMMUNICATION_FAULT			=3,         //通信状态 错误
+	};
+
+public:
+	Communication_socket_base();
+	Communication_socket_base(const Communication_socket_base& other)= delete;
+	Communication_socket_base& operator =(const Communication_socket_base& other)= delete;
+	~Communication_socket_base();
+public://API functions
+	//初始化 通信 模块。如下三选一
+	virtual Error_manager communication_init();
+	//初始化 通信 模块。从文件读取
+	Error_manager communication_init_from_protobuf(std::string prototxt_path);
+	//初始化 通信 模块。从protobuf读取
+	Error_manager communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all);
+
+	//初始化
+	virtual Error_manager communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector);
+	//bind
+	virtual Error_manager communication_bind(std::string bind_string);
+	//connect
+	virtual Error_manager communication_connect(std::vector<std::string>& connect_string_vector);
+	//connect
+	virtual Error_manager communication_connect(std::string connect_string);
+	//启动通信, run thread
+	virtual Error_manager communication_run();
+
+	//反初始化 通信 模块。
+	virtual Error_manager communication_uninit();
+	
+	
+public://get or set member variable
+	void set_analysis_cycle_time(unsigned int analysis_cycle_time);
+	void set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time);
+
+protected:
+	//mp_receive_data_thread 接受线程执行函数，
+	//receive_data_thread 内部线程负责接受消息
+	void receive_data_thread();
+
+	//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+	virtual Error_manager check_msg(Communication_message* p_msg);
+
+	//mp_analysis_data_thread 解析线程执行函数，
+	//analysis_data_thread 内部线程负责解析消息
+	void analysis_data_thread();
+
+	//遍历接受链表, 解析消息,
+	Error_manager analysis_receive_list();
+
+	//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+	virtual Error_manager check_executer(Communication_message* p_msg);
+
+	//处理消息, 需要子类重载
+	virtual Error_manager execute_msg(Communication_message* p_msg);
+
+	//mp_send_data_thread 发送线程执行函数，
+	//send_data_thread 内部线程负责发送消息
+	void send_data_thread();
+
+	//mp_encapsulate_data_thread 封装线程执行函数，
+	//encapsulate_data_thread 内部线程负责封装消息
+	void encapsulate_data_thread();
+
+	//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+	virtual Error_manager encapsulate_send_data();
+
+public:
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(std::string message);
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(Communication_message* p_msg);
+
+protected://member variable
+
+	//通用的网络编程接口, 默认使用总线模式, (网状结构)
+	nnxx::socket 						m_socket { nnxx::SP, nnxx::BUS };
+	std::mutex 							m_mutex;				//m_socket的锁
+
+	//通信状态
+	Communication_statu 				m_communication_statu;			//通信状态
+
+	//接受模块,
+	Thread_safe_list<Communication_message*>		m_receive_data_list; 			//接受的list容器
+	std::thread*						mp_receive_data_thread;    		//接受的线程指针
+	Thread_condition					m_receive_condition;			//接受的条件变量
+	std::thread*						mp_analysis_data_thread;    	//解析的线程指针
+	Thread_condition					m_analysis_data_condition;		//解析的条件变量
+	unsigned int 						m_analysis_cycle_time;			//自动解析的时间周期
+
+	//发送模块,
+	Thread_safe_list<Communication_message*>		m_send_data_list;				//发送的list容器
+	std::thread*						mp_send_data_thread;    		//发送的线程指针
+	Thread_condition					m_send_data_condition;			//发送的条件变量
+	std::thread*						mp_encapsulate_data_thread;    	//封装的线程指针
+	Thread_condition					m_encapsulate_data_condition;	//封装的条件变量
+	unsigned int 						m_encapsulate_cycle_time;		//自动封装的时间周期
+
+
+
+private:
+    const char *COMMUNICATION_PARAMETER_PATH = ETC_PATH"/setting/communication.prototxt";
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //__COMMUNICATION_SOCKET_BASE__HH__

Modules/MeasureNode/communication/communication_socket_base.h.orig

@@ -0,0 +1,166 @@
+
+
+/*
+ * communication_socket_base 通信模块的基类,
+ * 用户从这个基类继承, 初始化之后, 便可以自动进行通信
+ * 重载解析消息和封装消息,
+ *
+ *Thread_safe_list<Binary_buf*> 使用 Binary_buf , 而不是string
+ * 主要是为了支持直接发送数字0
+ * 
+ * 
+ * */
+
+#ifndef __COMMUNICATION_SOCKET_BASE__HH__
+#define __COMMUNICATION_SOCKET_BASE__HH__
+
+#include <mutex>
+#include <thread>
+#include <nnxx/message>
+#include <nnxx/socket.h>
+#include <nnxx/bus.h>
+
+#include <glog/logging.h>
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_list.h"
+#include "../tool/thread_condition.h"
+
+#include "../communication/communication.pb.h"
+#include "../communication/communication_message.h"
+
+#include "../message/message_base.pb.h"
+#include "../message/measure_message.pb.h"
+
+
+
+#define COMMUNICATION_PARAMETER_PATH "../setting/communication.prototxt"
+
+class Communication_socket_base
+{
+	//通信状态
+	enum Communication_statu
+	{
+		COMMUNICATION_UNKNOW		=0,	        //通信状态 未知
+		COMMUNICATION_READY			=1,			//通信状态 正常
+
+		COMMUNICATION_FAULT			=3,         //通信状态 错误
+	};
+
+public:
+	Communication_socket_base();
+	Communication_socket_base(const Communication_socket_base& other)= delete;
+	Communication_socket_base& operator =(const Communication_socket_base& other)= delete;
+	~Communication_socket_base();
+public://API functions
+	//初始化 通信 模块。如下三选一
+	virtual Error_manager communication_init();
+	//初始化 通信 模块。从文件读取
+	Error_manager communication_init_from_protobuf(std::string prototxt_path);
+	//初始化 通信 模块。从protobuf读取
+	Error_manager communication_init_from_protobuf(Communication_proto::Communication_parameter_all& communication_parameter_all);
+
+	//初始化
+	virtual Error_manager communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector);
+	//bind
+	virtual Error_manager communication_bind(std::string bind_string);
+	//connect
+	virtual Error_manager communication_connect(std::vector<std::string>& connect_string_vector);
+	//connect
+	virtual Error_manager communication_connect(std::string connect_string);
+	//启动通信, run thread
+	virtual Error_manager communication_run();
+
+	//反初始化 通信 模块。
+	virtual Error_manager communication_uninit();
+	
+	
+public://get or set member variable
+	void set_analysis_cycle_time(unsigned int analysis_cycle_time);
+	void set_encapsulate_cycle_time(unsigned int encapsulate_cycle_time);
+
+protected:
+	//mp_receive_data_thread 接受线程执行函数，
+	//receive_data_thread 内部线程负责接受消息
+	void receive_data_thread();
+
+	//检查消息是否有效, 主要检查消息类型和接受者, 判断这条消息是不是给我的.
+	virtual Error_manager check_msg(Communication_message* p_msg);
+
+	//mp_analysis_data_thread 解析线程执行函数，
+	//analysis_data_thread 内部线程负责解析消息
+	void analysis_data_thread();
+
+	//遍历接受链表, 解析消息,
+	Error_manager analysis_receive_list();
+
+	//检查执行者的状态, 判断能否处理这条消息, 需要子类重载
+	virtual Error_manager check_executer(Communication_message* p_msg);
+
+	//处理消息, 需要子类重载
+	virtual Error_manager execute_msg(Communication_message* p_msg);
+
+	//mp_send_data_thread 发送线程执行函数，
+	//send_data_thread 内部线程负责发送消息
+	void send_data_thread();
+
+	//mp_encapsulate_data_thread 封装线程执行函数，
+	//encapsulate_data_thread 内部线程负责封装消息
+	void encapsulate_data_thread();
+
+	//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+	virtual Error_manager encapsulate_send_data();
+
+public:
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(std::string message);
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(Communication_message* p_msg);
+
+protected://member variable
+
+	//通用的网络编程接口, 默认使用总线模式, (网状结构)
+	nnxx::socket 						m_socket { nnxx::SP, nnxx::BUS };
+	std::mutex 							m_mutex;				//m_socket的锁
+
+	//通信状态
+	Communication_statu 				m_communication_statu;			//通信状态
+
+	//接受模块,
+	Thread_safe_list<Communication_message*>		m_receive_data_list; 			//接受的list容器
+	std::thread*						mp_receive_data_thread;    		//接受的线程指针
+	Thread_condition					m_receive_condition;			//接受的条件变量
+	std::thread*						mp_analysis_data_thread;    	//解析的线程指针
+	Thread_condition					m_analysis_data_condition;		//解析的条件变量
+	unsigned int 						m_analysis_cycle_time;			//自动解析的时间周期
+
+	//发送模块,
+	Thread_safe_list<Communication_message*>		m_send_data_list;				//发送的list容器
+	std::thread*						mp_send_data_thread;    		//发送的线程指针
+	Thread_condition					m_send_data_condition;			//发送的条件变量
+	std::thread*						mp_encapsulate_data_thread;    	//封装的线程指针
+	Thread_condition					m_encapsulate_data_condition;	//封装的条件变量
+	unsigned int 						m_encapsulate_cycle_time;		//自动封装的时间周期
+
+
+
+private:
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //__COMMUNICATION_SOCKET_BASE__HH__

Modules/MeasureNode/measure_main.cpp

@@ -0,0 +1,193 @@
+//
+// Created by zx on 2020/6/18.
+//
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <iostream>
+#include "error_code/error_code.hpp"
+#include <glog/logging.h>
+
+#include "./communication/communication_socket_base.h"
+
+#include "./tool/thread_pool.h"
+#include "./system/system_communication.h"
+#include "./system/system_executor.h"
+
+//#define LIVOX_NUMBER         2
+// #define POINT_DEBUG
+
+#ifdef POINT_DEBUG
+std::shared_ptr<pcl::visualization::PCLVisualizer> gp_visualizer;
+#endif
+
+void shut_down_logging(const char *data, size_t size) {
+    time_t tt;
+    time(&tt);
+    tt = tt + 8 * 3600;  // transform the time zone
+    tm *t = gmtime(&tt);
+    char buf[255] = {0};
+    sprintf(buf, "./%d%02d%02d-%02d%02d%02d-dump.txt",
+            t->tm_year + 1900,
+            t->tm_mon + 1,
+            t->tm_mday,
+            t->tm_hour,
+            t->tm_min,
+            t->tm_sec);
+
+    FILE *tp_file = fopen(buf, "w");
+    fprintf(tp_file, data, strlen(data));
+    fclose(tp_file);
+
+}
+
+void test_whole_process() {
+    System_executor::get_instance_references().execute_for_measure("key_for_test_only", 0,
+                                                                   std::chrono::system_clock::now());
+}
+
+int main(int argc, char *argv[]) {
+    std::cout << " measure：： " << " xxxxxxxxxxxxxxxxx " << std::endl;
+
+
+    Error_manager t_error;
+    Error_manager t_result;
+    Error_manager ec;
+
+#ifdef POINT_DEBUG
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+    pcl::visualization::PCLVisualizer viewer("Viewer");      //创建viewer对象
+    gp_visualizer = std::shared_ptr<pcl::visualization::PCLVisualizer>(&viewer);
+    gp_visualizer->addCoordinateSystem();
+    gp_visualizer->setBackgroundColor(0, 0, 0);
+    gp_visualizer->initCameraParameters();
+    gp_visualizer->addPointCloud(t_cloud, "region");
+#endif
+
+    const char *logPath = "./log/";
+    google::InitGoogleLogging("LidarMeasurement");
+    google::SetStderrLogging(google::INFO);
+    google::SetLogDestination(0, logPath);
+    google::SetLogFilenameExtension("zxlog");
+    google::InstallFailureSignalHandler();
+    google::InstallFailureWriter(&shut_down_logging);
+    FLAGS_colorlogtostderr = true;        // Set log color
+    FLAGS_logbufsecs = 0;                // Set log output speed(s)
+    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
+    FLAGS_stop_logging_if_full_disk = true;
+
+
+//huli test
+//    ec = System_communication_mq::get_instance_references().rabbitmq_init();
+//    if(ec != SUCCESS)
+//    {
+//        LOG(ERROR) << "system communication mq init failed: " << ec.to_string();
+//        return -1;
+//    }
+//
+//    while (true)
+//    {}
+//
+//    return 0;
+
+
+    // std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;
+
+    // 定义服务的终端id
+    int t_terminal_id = 0;
+    if (argc == 2) {
+        std::cout << " huli test :::: " << " argv[1] = " << argv[1] << std::endl;
+        t_terminal_id = atoi(argv[1]);
+    }
+    std::cout << " huli test :::: " << " t_terminal_id = " << t_terminal_id << std::endl;
+
+//	Error_manager ec;
+
+    // 初始化
+//    if (WJ_VELO == 0 || WJ_VELO == 2) {
+//        Wanji_manager::get_instance_references().wanji_manager_init(t_terminal_id);
+//        std::cout << "wanji_manager = " << Wanji_manager::get_instance_references().check_status().to_string()
+//                  << std::endl;
+//        if (ec != SUCCESS) {
+//            LOG(ERROR) << "wanji_manager init failed: " << ec.to_string();
+//            return -1;
+//        }
+//    }
+    if (WJ_VELO == 1 || WJ_VELO == 2) {
+        ec = Velodyne_manager::get_instance_references().velodyne_manager_init(t_terminal_id);
+
+        if (ec != SUCCESS) {
+            std::cout << "veodyne_manager = " << ec.to_string() << std::endl;
+            std::cout << "veodyne_manager = " << Velodyne_manager::get_instance_references().check_status().to_string()
+                      << std::endl;
+            LOG(ERROR) << "velodyne_manager init failed: " << ec.to_string();
+            return -1;
+        }
+    }
+
+    ec = System_executor::get_instance_references().system_executor_init(4, t_terminal_id);
+    std::cout << "System_executor = " << System_executor::get_instance_references().get_system_executor_status()
+              << std::endl;
+    if (ec != SUCCESS) {
+        LOG(ERROR) << "system executor init failed: " << ec.to_string();
+        return -1;
+    }
+
+    // usleep(1000 * 500);
+
+    ec = System_communication::get_instance_references().communication_init();
+    if (ec != SUCCESS) {
+        LOG(ERROR) << "system communication init failed: " << ec.to_string();
+        return -1;
+    }
+    System_communication::get_instance_references().set_encapsulate_cycle_time(110);
+
+    LOG(INFO) << "begain init rabbitmq.";
+    ec = System_communication_mq::get_instance_references().rabbitmq_init();
+    if (ec != SUCCESS) {
+        LOG(ERROR) << "system communication mq init failed: " << ec.to_string();
+        return -1;
+    }
+    System_communication_mq::get_instance_references().set_encapsulate_status_cycle_time(100);
+
+    // prev_test_pred_task();
+    // test_whole_process();
+//    usleep(1000*5000);
+    char ch = 'x';
+    pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(
+            new pcl::PointCloud<pcl::PointXYZ>);
+    while (ch != 'q') {
+
+#ifdef POINT_DEBUG
+        std::map<int, Ground_region *> t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map();
+        for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
+        {
+            if (iter->first == DISP_TERM_ID)
+            {
+                t_region_cloud->clear();
+                // 获取区域点云
+                iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
+                // gp_visualizer->showCloud(t_region_cloud);
+                gp_visualizer->updatePointCloud(t_region_cloud, "region");
+                // LOG(INFO) << "pcl cloud updated." <<t_region_cloud->size();
+            }
+        }
+        gp_visualizer->spinOnce();
+        usleep(100);
+#else
+        std::cout << "please input q to quit system." << std::endl;
+        std::cin >> ch;
+#endif
+    }
+
+    // 反初始化
+    System_communication::get_instance_references().communication_uninit();
+    System_communication_mq::get_instance_references().rabbitmq_uninit();
+
+    if (WJ_VELO == 1 || WJ_VELO == 2)
+        Velodyne_manager::get_instance_references().Velodyne_manager_uninit();
+    System_executor::get_instance_references().system_executor_uninit();
+    if (WJ_VELO == 0 || WJ_VELO == 2)
+        Wanji_manager::get_instance_references().wanji_manager_uninit();
+
+    return 0;
+}

Modules/MeasureNode/message/UnNormalized_module_message.pb.cc

@@ -0,0 +1,505 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: UnNormalized_module_message.proto
+
+#include "UnNormalized_module_message.pb.h"
+
+#include <algorithm>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/extension_set.h>
+#include <google/protobuf/wire_format_lite.h>
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/reflection_ops.h>
+#include <google/protobuf/wire_format.h>
+// @@protoc_insertion_point(includes)
+#include <google/protobuf/port_def.inc>
+extern PROTOBUF_INTERNAL_EXPORT_message_5fbase_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Base_info_message_5fbase_2eproto;
+extern PROTOBUF_INTERNAL_EXPORT_message_5fbase_2eproto ::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_Error_manager_message_5fbase_2eproto;
+namespace message {
+class UnNormalized_module_statu_msgDefaultTypeInternal {
+ public:
+  ::PROTOBUF_NAMESPACE_ID::internal::ExplicitlyConstructed<UnNormalized_module_statu_msg> _instance;
+} _UnNormalized_module_statu_msg_default_instance_;
+}  // namespace message
+static void InitDefaultsscc_info_UnNormalized_module_statu_msg_UnNormalized_5fmodule_5fmessage_2eproto() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+  {
+    void* ptr = &::message::_UnNormalized_module_statu_msg_default_instance_;
+    new (ptr) ::message::UnNormalized_module_statu_msg();
+    ::PROTOBUF_NAMESPACE_ID::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::message::UnNormalized_module_statu_msg::InitAsDefaultInstance();
+}
+
+::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<2> scc_info_UnNormalized_module_statu_msg_UnNormalized_5fmodule_5fmessage_2eproto =
+    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 2, 0, InitDefaultsscc_info_UnNormalized_module_statu_msg_UnNormalized_5fmodule_5fmessage_2eproto}, {
+      &scc_info_Base_info_message_5fbase_2eproto.base,
+      &scc_info_Error_manager_message_5fbase_2eproto.base,}};
+
+static ::PROTOBUF_NAMESPACE_ID::Metadata file_level_metadata_UnNormalized_5fmodule_5fmessage_2eproto[1];
+static constexpr ::PROTOBUF_NAMESPACE_ID::EnumDescriptor const** file_level_enum_descriptors_UnNormalized_5fmodule_5fmessage_2eproto = nullptr;
+static constexpr ::PROTOBUF_NAMESPACE_ID::ServiceDescriptor const** file_level_service_descriptors_UnNormalized_5fmodule_5fmessage_2eproto = nullptr;
+
+const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_UnNormalized_5fmodule_5fmessage_2eproto::offsets[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
+  PROTOBUF_FIELD_OFFSET(::message::UnNormalized_module_statu_msg, _has_bits_),
+  PROTOBUF_FIELD_OFFSET(::message::UnNormalized_module_statu_msg, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  PROTOBUF_FIELD_OFFSET(::message::UnNormalized_module_statu_msg, base_info_),
+  PROTOBUF_FIELD_OFFSET(::message::UnNormalized_module_statu_msg, id_),
+  PROTOBUF_FIELD_OFFSET(::message::UnNormalized_module_statu_msg, statu_code_),
+  PROTOBUF_FIELD_OFFSET(::message::UnNormalized_module_statu_msg, car_license_),
+  1,
+  3,
+  2,
+  0,
+};
+static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
+  { 0, 9, sizeof(::message::UnNormalized_module_statu_msg)},
+};
+
+static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
+  reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::message::_UnNormalized_module_statu_msg_default_instance_),
+};
+
+const char descriptor_table_protodef_UnNormalized_5fmodule_5fmessage_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
+  "\n!UnNormalized_module_message.proto\022\007mes"
+  "sage\032\022message_base.proto\"\223\001\n\035UnNormalize"
+  "d_module_statu_msg\022%\n\tbase_info\030\001 \002(\0132\022."
+  "message.Base_info\022\n\n\002id\030\002 \002(\005\022*\n\nstatu_c"
+  "ode\030\003 \002(\0132\026.message.Error_manager\022\023\n\013car"
+  "_license\030\004 \001(\t"
+  ;
+static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto_deps[1] = {
+  &::descriptor_table_message_5fbase_2eproto,
+};
+static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto_sccs[1] = {
+  &scc_info_UnNormalized_module_statu_msg_UnNormalized_5fmodule_5fmessage_2eproto.base,
+};
+static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto_once;
+const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto = {
+  false, false, descriptor_table_protodef_UnNormalized_5fmodule_5fmessage_2eproto, "UnNormalized_module_message.proto", 214,
+  &descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto_once, descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto_sccs, descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto_deps, 1, 1,
+  schemas, file_default_instances, TableStruct_UnNormalized_5fmodule_5fmessage_2eproto::offsets,
+  file_level_metadata_UnNormalized_5fmodule_5fmessage_2eproto, 1, file_level_enum_descriptors_UnNormalized_5fmodule_5fmessage_2eproto, file_level_service_descriptors_UnNormalized_5fmodule_5fmessage_2eproto,
+};
+
+// Force running AddDescriptors() at dynamic initialization time.
+static bool dynamic_init_dummy_UnNormalized_5fmodule_5fmessage_2eproto = (static_cast<void>(::PROTOBUF_NAMESPACE_ID::internal::AddDescriptors(&descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto)), true);
+namespace message {
+
+// ===================================================================
+
+void UnNormalized_module_statu_msg::InitAsDefaultInstance() {
+  ::message::_UnNormalized_module_statu_msg_default_instance_._instance.get_mutable()->base_info_ = const_cast< ::message::Base_info*>(
+      ::message::Base_info::internal_default_instance());
+  ::message::_UnNormalized_module_statu_msg_default_instance_._instance.get_mutable()->statu_code_ = const_cast< ::message::Error_manager*>(
+      ::message::Error_manager::internal_default_instance());
+}
+class UnNormalized_module_statu_msg::_Internal {
+ public:
+  using HasBits = decltype(std::declval<UnNormalized_module_statu_msg>()._has_bits_);
+  static const ::message::Base_info& base_info(const UnNormalized_module_statu_msg* msg);
+  static void set_has_base_info(HasBits* has_bits) {
+    (*has_bits)[0] |= 2u;
+  }
+  static void set_has_id(HasBits* has_bits) {
+    (*has_bits)[0] |= 8u;
+  }
+  static const ::message::Error_manager& statu_code(const UnNormalized_module_statu_msg* msg);
+  static void set_has_statu_code(HasBits* has_bits) {
+    (*has_bits)[0] |= 4u;
+  }
+  static void set_has_car_license(HasBits* has_bits) {
+    (*has_bits)[0] |= 1u;
+  }
+  static bool MissingRequiredFields(const HasBits& has_bits) {
+    return ((has_bits[0] & 0x0000000e) ^ 0x0000000e) != 0;
+  }
+};
+
+const ::message::Base_info&
+UnNormalized_module_statu_msg::_Internal::base_info(const UnNormalized_module_statu_msg* msg) {
+  return *msg->base_info_;
+}
+const ::message::Error_manager&
+UnNormalized_module_statu_msg::_Internal::statu_code(const UnNormalized_module_statu_msg* msg) {
+  return *msg->statu_code_;
+}
+void UnNormalized_module_statu_msg::clear_base_info() {
+  if (base_info_ != nullptr) base_info_->Clear();
+  _has_bits_[0] &= ~0x00000002u;
+}
+void UnNormalized_module_statu_msg::clear_statu_code() {
+  if (statu_code_ != nullptr) statu_code_->Clear();
+  _has_bits_[0] &= ~0x00000004u;
+}
+UnNormalized_module_statu_msg::UnNormalized_module_statu_msg(::PROTOBUF_NAMESPACE_ID::Arena* arena)
+  : ::PROTOBUF_NAMESPACE_ID::Message(arena) {
+  SharedCtor();
+  RegisterArenaDtor(arena);
+  // @@protoc_insertion_point(arena_constructor:message.UnNormalized_module_statu_msg)
+}
+UnNormalized_module_statu_msg::UnNormalized_module_statu_msg(const UnNormalized_module_statu_msg& from)
+  : ::PROTOBUF_NAMESPACE_ID::Message(),
+      _has_bits_(from._has_bits_) {
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  car_license_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (from._internal_has_car_license()) {
+    car_license_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), from._internal_car_license(),
+      GetArena());
+  }
+  if (from._internal_has_base_info()) {
+    base_info_ = new ::message::Base_info(*from.base_info_);
+  } else {
+    base_info_ = nullptr;
+  }
+  if (from._internal_has_statu_code()) {
+    statu_code_ = new ::message::Error_manager(*from.statu_code_);
+  } else {
+    statu_code_ = nullptr;
+  }
+  id_ = from.id_;
+  // @@protoc_insertion_point(copy_constructor:message.UnNormalized_module_statu_msg)
+}
+
+void UnNormalized_module_statu_msg::SharedCtor() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&scc_info_UnNormalized_module_statu_msg_UnNormalized_5fmodule_5fmessage_2eproto.base);
+  car_license_.UnsafeSetDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  ::memset(&base_info_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&id_) -
+      reinterpret_cast<char*>(&base_info_)) + sizeof(id_));
+}
+
+UnNormalized_module_statu_msg::~UnNormalized_module_statu_msg() {
+  // @@protoc_insertion_point(destructor:message.UnNormalized_module_statu_msg)
+  SharedDtor();
+  _internal_metadata_.Delete<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+void UnNormalized_module_statu_msg::SharedDtor() {
+  GOOGLE_DCHECK(GetArena() == nullptr);
+  car_license_.DestroyNoArena(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited());
+  if (this != internal_default_instance()) delete base_info_;
+  if (this != internal_default_instance()) delete statu_code_;
+}
+
+void UnNormalized_module_statu_msg::ArenaDtor(void* object) {
+  UnNormalized_module_statu_msg* _this = reinterpret_cast< UnNormalized_module_statu_msg* >(object);
+  (void)_this;
+}
+void UnNormalized_module_statu_msg::RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena*) {
+}
+void UnNormalized_module_statu_msg::SetCachedSize(int size) const {
+  _cached_size_.Set(size);
+}
+const UnNormalized_module_statu_msg& UnNormalized_module_statu_msg::default_instance() {
+  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&::scc_info_UnNormalized_module_statu_msg_UnNormalized_5fmodule_5fmessage_2eproto.base);
+  return *internal_default_instance();
+}
+
+
+void UnNormalized_module_statu_msg::Clear() {
+// @@protoc_insertion_point(message_clear_start:message.UnNormalized_module_statu_msg)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000007u) {
+    if (cached_has_bits & 0x00000001u) {
+      car_license_.ClearNonDefaultToEmpty();
+    }
+    if (cached_has_bits & 0x00000002u) {
+      GOOGLE_DCHECK(base_info_ != nullptr);
+      base_info_->Clear();
+    }
+    if (cached_has_bits & 0x00000004u) {
+      GOOGLE_DCHECK(statu_code_ != nullptr);
+      statu_code_->Clear();
+    }
+  }
+  id_ = 0;
+  _has_bits_.Clear();
+  _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+}
+
+const char* UnNormalized_module_statu_msg::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) {
+#define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
+  _Internal::HasBits has_bits{};
+  ::PROTOBUF_NAMESPACE_ID::Arena* arena = GetArena(); (void)arena;
+  while (!ctx->Done(&ptr)) {
+    ::PROTOBUF_NAMESPACE_ID::uint32 tag;
+    ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
+    CHK_(ptr);
+    switch (tag >> 3) {
+      // required .message.Base_info base_info = 1;
+      case 1:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 10)) {
+          ptr = ctx->ParseMessage(_internal_mutable_base_info(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // required int32 id = 2;
+      case 2:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
+          _Internal::set_has_id(&has_bits);
+          id_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // required .message.Error_manager statu_code = 3;
+      case 3:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 26)) {
+          ptr = ctx->ParseMessage(_internal_mutable_statu_code(), ptr);
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      // optional string car_license = 4;
+      case 4:
+        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 34)) {
+          auto str = _internal_mutable_car_license();
+          ptr = ::PROTOBUF_NAMESPACE_ID::internal::InlineGreedyStringParser(str, ptr, ctx);
+          #ifndef NDEBUG
+          ::PROTOBUF_NAMESPACE_ID::internal::VerifyUTF8(str, "message.UnNormalized_module_statu_msg.car_license");
+          #endif  // !NDEBUG
+          CHK_(ptr);
+        } else goto handle_unusual;
+        continue;
+      default: {
+      handle_unusual:
+        if ((tag & 7) == 4 || tag == 0) {
+          ctx->SetLastTag(tag);
+          goto success;
+        }
+        ptr = UnknownFieldParse(tag,
+            _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(),
+            ptr, ctx);
+        CHK_(ptr != nullptr);
+        continue;
+      }
+    }  // switch
+  }  // while
+success:
+  _has_bits_.Or(has_bits);
+  return ptr;
+failure:
+  ptr = nullptr;
+  goto success;
+#undef CHK_
+}
+
+::PROTOBUF_NAMESPACE_ID::uint8* UnNormalized_module_statu_msg::_InternalSerialize(
+    ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const {
+  // @@protoc_insertion_point(serialize_to_array_start:message.UnNormalized_module_statu_msg)
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // required .message.Base_info base_info = 1;
+  if (cached_has_bits & 0x00000002u) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        1, _Internal::base_info(this), target, stream);
+  }
+
+  // required int32 id = 2;
+  if (cached_has_bits & 0x00000008u) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_id(), target);
+  }
+
+  // required .message.Error_manager statu_code = 3;
+  if (cached_has_bits & 0x00000004u) {
+    target = stream->EnsureSpace(target);
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::
+      InternalWriteMessage(
+        3, _Internal::statu_code(this), target, stream);
+  }
+
+  // optional string car_license = 4;
+  if (cached_has_bits & 0x00000001u) {
+    ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::VerifyUTF8StringNamedField(
+      this->_internal_car_license().data(), static_cast<int>(this->_internal_car_license().length()),
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::SERIALIZE,
+      "message.UnNormalized_module_statu_msg.car_license");
+    target = stream->WriteStringMaybeAliased(
+        4, this->_internal_car_license(), target);
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:message.UnNormalized_module_statu_msg)
+  return target;
+}
+
+size_t UnNormalized_module_statu_msg::RequiredFieldsByteSizeFallback() const {
+// @@protoc_insertion_point(required_fields_byte_size_fallback_start:message.UnNormalized_module_statu_msg)
+  size_t total_size = 0;
+
+  if (_internal_has_base_info()) {
+    // required .message.Base_info base_info = 1;
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *base_info_);
+  }
+
+  if (_internal_has_statu_code()) {
+    // required .message.Error_manager statu_code = 3;
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *statu_code_);
+  }
+
+  if (_internal_has_id()) {
+    // required int32 id = 2;
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_id());
+  }
+
+  return total_size;
+}
+size_t UnNormalized_module_statu_msg::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:message.UnNormalized_module_statu_msg)
+  size_t total_size = 0;
+
+  if (((_has_bits_[0] & 0x0000000e) ^ 0x0000000e) == 0) {  // All required fields are present.
+    // required .message.Base_info base_info = 1;
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *base_info_);
+
+    // required .message.Error_manager statu_code = 3;
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::MessageSize(
+        *statu_code_);
+
+    // required int32 id = 2;
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
+        this->_internal_id());
+
+  } else {
+    total_size += RequiredFieldsByteSizeFallback();
+  }
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  // optional string car_license = 4;
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 0x00000001u) {
+    total_size += 1 +
+      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::StringSize(
+        this->_internal_car_license());
+  }
+
+  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
+    return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
+        _internal_metadata_, total_size, &_cached_size_);
+  }
+  int cached_size = ::PROTOBUF_NAMESPACE_ID::internal::ToCachedSize(total_size);
+  SetCachedSize(cached_size);
+  return total_size;
+}
+
+void UnNormalized_module_statu_msg::MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:message.UnNormalized_module_statu_msg)
+  GOOGLE_DCHECK_NE(&from, this);
+  const UnNormalized_module_statu_msg* source =
+      ::PROTOBUF_NAMESPACE_ID::DynamicCastToGenerated<UnNormalized_module_statu_msg>(
+          &from);
+  if (source == nullptr) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:message.UnNormalized_module_statu_msg)
+    ::PROTOBUF_NAMESPACE_ID::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:message.UnNormalized_module_statu_msg)
+    MergeFrom(*source);
+  }
+}
+
+void UnNormalized_module_statu_msg::MergeFrom(const UnNormalized_module_statu_msg& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:message.UnNormalized_module_statu_msg)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
+  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = from._has_bits_[0];
+  if (cached_has_bits & 0x0000000fu) {
+    if (cached_has_bits & 0x00000001u) {
+      _internal_set_car_license(from._internal_car_license());
+    }
+    if (cached_has_bits & 0x00000002u) {
+      _internal_mutable_base_info()->::message::Base_info::MergeFrom(from._internal_base_info());
+    }
+    if (cached_has_bits & 0x00000004u) {
+      _internal_mutable_statu_code()->::message::Error_manager::MergeFrom(from._internal_statu_code());
+    }
+    if (cached_has_bits & 0x00000008u) {
+      id_ = from.id_;
+    }
+    _has_bits_[0] |= cached_has_bits;
+  }
+}
+
+void UnNormalized_module_statu_msg::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:message.UnNormalized_module_statu_msg)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void UnNormalized_module_statu_msg::CopyFrom(const UnNormalized_module_statu_msg& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:message.UnNormalized_module_statu_msg)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool UnNormalized_module_statu_msg::IsInitialized() const {
+  if (_Internal::MissingRequiredFields(_has_bits_)) return false;
+  if (_internal_has_base_info()) {
+    if (!base_info_->IsInitialized()) return false;
+  }
+  if (_internal_has_statu_code()) {
+    if (!statu_code_->IsInitialized()) return false;
+  }
+  return true;
+}
+
+void UnNormalized_module_statu_msg::InternalSwap(UnNormalized_module_statu_msg* other) {
+  using std::swap;
+  _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  car_license_.Swap(&other->car_license_, &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
+      PROTOBUF_FIELD_OFFSET(UnNormalized_module_statu_msg, id_)
+      + sizeof(UnNormalized_module_statu_msg::id_)
+      - PROTOBUF_FIELD_OFFSET(UnNormalized_module_statu_msg, base_info_)>(
+          reinterpret_cast<char*>(&base_info_),
+          reinterpret_cast<char*>(&other->base_info_));
+}
+
+::PROTOBUF_NAMESPACE_ID::Metadata UnNormalized_module_statu_msg::GetMetadata() const {
+  return GetMetadataStatic();
+}
+
+
+// @@protoc_insertion_point(namespace_scope)
+}  // namespace message
+PROTOBUF_NAMESPACE_OPEN
+template<> PROTOBUF_NOINLINE ::message::UnNormalized_module_statu_msg* Arena::CreateMaybeMessage< ::message::UnNormalized_module_statu_msg >(Arena* arena) {
+  return Arena::CreateMessageInternal< ::message::UnNormalized_module_statu_msg >(arena);
+}
+PROTOBUF_NAMESPACE_CLOSE
+
+// @@protoc_insertion_point(global_scope)
+#include <google/protobuf/port_undef.inc>

Modules/MeasureNode/message/UnNormalized_module_message.pb.h

@@ -0,0 +1,564 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: UnNormalized_module_message.proto
+
+#ifndef GOOGLE_PROTOBUF_INCLUDED_UnNormalized_5fmodule_5fmessage_2eproto
+#define GOOGLE_PROTOBUF_INCLUDED_UnNormalized_5fmodule_5fmessage_2eproto
+
+#include <limits>
+#include <string>
+
+#include <google/protobuf/port_def.inc>
+#if PROTOBUF_VERSION < 4000000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers. Please update
+#error your headers.
+#endif
+#if 4000000 < PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers. Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/port_undef.inc>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_table_driven.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/inlined_string_field.h>
+#include <google/protobuf/metadata_lite.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>  // IWYU pragma: export
+#include <google/protobuf/extension_set.h>  // IWYU pragma: export
+#include <google/protobuf/unknown_field_set.h>
+#include "message_base.pb.h"
+// @@protoc_insertion_point(includes)
+#include <google/protobuf/port_def.inc>
+#define PROTOBUF_INTERNAL_EXPORT_UnNormalized_5fmodule_5fmessage_2eproto
+PROTOBUF_NAMESPACE_OPEN
+namespace internal {
+class AnyMetadata;
+}  // namespace internal
+PROTOBUF_NAMESPACE_CLOSE
+
+// Internal implementation detail -- do not use these members.
+struct TableStruct_UnNormalized_5fmodule_5fmessage_2eproto {
+  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTableField entries[]
+    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
+  static const ::PROTOBUF_NAMESPACE_ID::internal::AuxiliaryParseTableField aux[]
+    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
+  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[1]
+    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
+  static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[];
+  static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[];
+  static const ::PROTOBUF_NAMESPACE_ID::uint32 offsets[];
+};
+extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto;
+namespace message {
+class UnNormalized_module_statu_msg;
+class UnNormalized_module_statu_msgDefaultTypeInternal;
+extern UnNormalized_module_statu_msgDefaultTypeInternal _UnNormalized_module_statu_msg_default_instance_;
+}  // namespace message
+PROTOBUF_NAMESPACE_OPEN
+template<> ::message::UnNormalized_module_statu_msg* Arena::CreateMaybeMessage<::message::UnNormalized_module_statu_msg>(Arena*);
+PROTOBUF_NAMESPACE_CLOSE
+namespace message {
+
+// ===================================================================
+
+class UnNormalized_module_statu_msg PROTOBUF_FINAL :
+    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:message.UnNormalized_module_statu_msg) */ {
+ public:
+  inline UnNormalized_module_statu_msg() : UnNormalized_module_statu_msg(nullptr) {}
+  virtual ~UnNormalized_module_statu_msg();
+
+  UnNormalized_module_statu_msg(const UnNormalized_module_statu_msg& from);
+  UnNormalized_module_statu_msg(UnNormalized_module_statu_msg&& from) noexcept
+    : UnNormalized_module_statu_msg() {
+    *this = ::std::move(from);
+  }
+
+  inline UnNormalized_module_statu_msg& operator=(const UnNormalized_module_statu_msg& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  inline UnNormalized_module_statu_msg& operator=(UnNormalized_module_statu_msg&& from) noexcept {
+    if (GetArena() == from.GetArena()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+
+  inline const ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance);
+  }
+  inline ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
+  }
+
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
+    return GetDescriptor();
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
+    return GetMetadataStatic().descriptor;
+  }
+  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
+    return GetMetadataStatic().reflection;
+  }
+  static const UnNormalized_module_statu_msg& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const UnNormalized_module_statu_msg* internal_default_instance() {
+    return reinterpret_cast<const UnNormalized_module_statu_msg*>(
+               &_UnNormalized_module_statu_msg_default_instance_);
+  }
+  static constexpr int kIndexInFileMessages =
+    0;
+
+  friend void swap(UnNormalized_module_statu_msg& a, UnNormalized_module_statu_msg& b) {
+    a.Swap(&b);
+  }
+  inline void Swap(UnNormalized_module_statu_msg* other) {
+    if (other == this) return;
+    if (GetArena() == other->GetArena()) {
+      InternalSwap(other);
+    } else {
+      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
+    }
+  }
+  void UnsafeArenaSwap(UnNormalized_module_statu_msg* other) {
+    if (other == this) return;
+    GOOGLE_DCHECK(GetArena() == other->GetArena());
+    InternalSwap(other);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline UnNormalized_module_statu_msg* New() const final {
+    return CreateMaybeMessage<UnNormalized_module_statu_msg>(nullptr);
+  }
+
+  UnNormalized_module_statu_msg* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
+    return CreateMaybeMessage<UnNormalized_module_statu_msg>(arena);
+  }
+  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
+  void CopyFrom(const UnNormalized_module_statu_msg& from);
+  void MergeFrom(const UnNormalized_module_statu_msg& from);
+  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
+  bool IsInitialized() const final;
+
+  size_t ByteSizeLong() const final;
+  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
+  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
+      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
+  int GetCachedSize() const final { return _cached_size_.Get(); }
+
+  private:
+  inline void SharedCtor();
+  inline void SharedDtor();
+  void SetCachedSize(int size) const final;
+  void InternalSwap(UnNormalized_module_statu_msg* other);
+  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
+  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
+    return "message.UnNormalized_module_statu_msg";
+  }
+  protected:
+  explicit UnNormalized_module_statu_msg(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  private:
+  static void ArenaDtor(void* object);
+  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
+  public:
+
+  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
+  private:
+  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
+    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto);
+    return ::descriptor_table_UnNormalized_5fmodule_5fmessage_2eproto.file_level_metadata[kIndexInFileMessages];
+  }
+
+  public:
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  enum : int {
+    kCarLicenseFieldNumber = 4,
+    kBaseInfoFieldNumber = 1,
+    kStatuCodeFieldNumber = 3,
+    kIdFieldNumber = 2,
+  };
+  // optional string car_license = 4;
+  bool has_car_license() const;
+  private:
+  bool _internal_has_car_license() const;
+  public:
+  void clear_car_license();
+  const std::string& car_license() const;
+  void set_car_license(const std::string& value);
+  void set_car_license(std::string&& value);
+  void set_car_license(const char* value);
+  void set_car_license(const char* value, size_t size);
+  std::string* mutable_car_license();
+  std::string* release_car_license();
+  void set_allocated_car_license(std::string* car_license);
+  private:
+  const std::string& _internal_car_license() const;
+  void _internal_set_car_license(const std::string& value);
+  std::string* _internal_mutable_car_license();
+  public:
+
+  // required .message.Base_info base_info = 1;
+  bool has_base_info() const;
+  private:
+  bool _internal_has_base_info() const;
+  public:
+  void clear_base_info();
+  const ::message::Base_info& base_info() const;
+  ::message::Base_info* release_base_info();
+  ::message::Base_info* mutable_base_info();
+  void set_allocated_base_info(::message::Base_info* base_info);
+  private:
+  const ::message::Base_info& _internal_base_info() const;
+  ::message::Base_info* _internal_mutable_base_info();
+  public:
+  void unsafe_arena_set_allocated_base_info(
+      ::message::Base_info* base_info);
+  ::message::Base_info* unsafe_arena_release_base_info();
+
+  // required .message.Error_manager statu_code = 3;
+  bool has_statu_code() const;
+  private:
+  bool _internal_has_statu_code() const;
+  public:
+  void clear_statu_code();
+  const ::message::Error_manager& statu_code() const;
+  ::message::Error_manager* release_statu_code();
+  ::message::Error_manager* mutable_statu_code();
+  void set_allocated_statu_code(::message::Error_manager* statu_code);
+  private:
+  const ::message::Error_manager& _internal_statu_code() const;
+  ::message::Error_manager* _internal_mutable_statu_code();
+  public:
+  void unsafe_arena_set_allocated_statu_code(
+      ::message::Error_manager* statu_code);
+  ::message::Error_manager* unsafe_arena_release_statu_code();
+
+  // required int32 id = 2;
+  bool has_id() const;
+  private:
+  bool _internal_has_id() const;
+  public:
+  void clear_id();
+  ::PROTOBUF_NAMESPACE_ID::int32 id() const;
+  void set_id(::PROTOBUF_NAMESPACE_ID::int32 value);
+  private:
+  ::PROTOBUF_NAMESPACE_ID::int32 _internal_id() const;
+  void _internal_set_id(::PROTOBUF_NAMESPACE_ID::int32 value);
+  public:
+
+  // @@protoc_insertion_point(class_scope:message.UnNormalized_module_statu_msg)
+ private:
+  class _Internal;
+
+  // helper for ByteSizeLong()
+  size_t RequiredFieldsByteSizeFallback() const;
+
+  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
+  typedef void InternalArenaConstructable_;
+  typedef void DestructorSkippable_;
+  ::PROTOBUF_NAMESPACE_ID::internal::HasBits<1> _has_bits_;
+  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
+  ::PROTOBUF_NAMESPACE_ID::internal::ArenaStringPtr car_license_;
+  ::message::Base_info* base_info_;
+  ::message::Error_manager* statu_code_;
+  ::PROTOBUF_NAMESPACE_ID::int32 id_;
+  friend struct ::TableStruct_UnNormalized_5fmodule_5fmessage_2eproto;
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#endif  // __GNUC__
+// UnNormalized_module_statu_msg
+
+// required .message.Base_info base_info = 1;
+inline bool UnNormalized_module_statu_msg::_internal_has_base_info() const {
+  bool value = (_has_bits_[0] & 0x00000002u) != 0;
+  PROTOBUF_ASSUME(!value || base_info_ != nullptr);
+  return value;
+}
+inline bool UnNormalized_module_statu_msg::has_base_info() const {
+  return _internal_has_base_info();
+}
+inline const ::message::Base_info& UnNormalized_module_statu_msg::_internal_base_info() const {
+  const ::message::Base_info* p = base_info_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::message::Base_info*>(
+      &::message::_Base_info_default_instance_);
+}
+inline const ::message::Base_info& UnNormalized_module_statu_msg::base_info() const {
+  // @@protoc_insertion_point(field_get:message.UnNormalized_module_statu_msg.base_info)
+  return _internal_base_info();
+}
+inline void UnNormalized_module_statu_msg::unsafe_arena_set_allocated_base_info(
+    ::message::Base_info* base_info) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(base_info_);
+  }
+  base_info_ = base_info;
+  if (base_info) {
+    _has_bits_[0] |= 0x00000002u;
+  } else {
+    _has_bits_[0] &= ~0x00000002u;
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:message.UnNormalized_module_statu_msg.base_info)
+}
+inline ::message::Base_info* UnNormalized_module_statu_msg::release_base_info() {
+  _has_bits_[0] &= ~0x00000002u;
+  ::message::Base_info* temp = base_info_;
+  base_info_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::message::Base_info* UnNormalized_module_statu_msg::unsafe_arena_release_base_info() {
+  // @@protoc_insertion_point(field_release:message.UnNormalized_module_statu_msg.base_info)
+  _has_bits_[0] &= ~0x00000002u;
+  ::message::Base_info* temp = base_info_;
+  base_info_ = nullptr;
+  return temp;
+}
+inline ::message::Base_info* UnNormalized_module_statu_msg::_internal_mutable_base_info() {
+  _has_bits_[0] |= 0x00000002u;
+  if (base_info_ == nullptr) {
+    auto* p = CreateMaybeMessage<::message::Base_info>(GetArena());
+    base_info_ = p;
+  }
+  return base_info_;
+}
+inline ::message::Base_info* UnNormalized_module_statu_msg::mutable_base_info() {
+  // @@protoc_insertion_point(field_mutable:message.UnNormalized_module_statu_msg.base_info)
+  return _internal_mutable_base_info();
+}
+inline void UnNormalized_module_statu_msg::set_allocated_base_info(::message::Base_info* base_info) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete reinterpret_cast< ::PROTOBUF_NAMESPACE_ID::MessageLite*>(base_info_);
+  }
+  if (base_info) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(base_info)->GetArena();
+    if (message_arena != submessage_arena) {
+      base_info = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, base_info, submessage_arena);
+    }
+    _has_bits_[0] |= 0x00000002u;
+  } else {
+    _has_bits_[0] &= ~0x00000002u;
+  }
+  base_info_ = base_info;
+  // @@protoc_insertion_point(field_set_allocated:message.UnNormalized_module_statu_msg.base_info)
+}
+
+// required int32 id = 2;
+inline bool UnNormalized_module_statu_msg::_internal_has_id() const {
+  bool value = (_has_bits_[0] & 0x00000008u) != 0;
+  return value;
+}
+inline bool UnNormalized_module_statu_msg::has_id() const {
+  return _internal_has_id();
+}
+inline void UnNormalized_module_statu_msg::clear_id() {
+  id_ = 0;
+  _has_bits_[0] &= ~0x00000008u;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 UnNormalized_module_statu_msg::_internal_id() const {
+  return id_;
+}
+inline ::PROTOBUF_NAMESPACE_ID::int32 UnNormalized_module_statu_msg::id() const {
+  // @@protoc_insertion_point(field_get:message.UnNormalized_module_statu_msg.id)
+  return _internal_id();
+}
+inline void UnNormalized_module_statu_msg::_internal_set_id(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _has_bits_[0] |= 0x00000008u;
+  id_ = value;
+}
+inline void UnNormalized_module_statu_msg::set_id(::PROTOBUF_NAMESPACE_ID::int32 value) {
+  _internal_set_id(value);
+  // @@protoc_insertion_point(field_set:message.UnNormalized_module_statu_msg.id)
+}
+
+// required .message.Error_manager statu_code = 3;
+inline bool UnNormalized_module_statu_msg::_internal_has_statu_code() const {
+  bool value = (_has_bits_[0] & 0x00000004u) != 0;
+  PROTOBUF_ASSUME(!value || statu_code_ != nullptr);
+  return value;
+}
+inline bool UnNormalized_module_statu_msg::has_statu_code() const {
+  return _internal_has_statu_code();
+}
+inline const ::message::Error_manager& UnNormalized_module_statu_msg::_internal_statu_code() const {
+  const ::message::Error_manager* p = statu_code_;
+  return p != nullptr ? *p : *reinterpret_cast<const ::message::Error_manager*>(
+      &::message::_Error_manager_default_instance_);
+}
+inline const ::message::Error_manager& UnNormalized_module_statu_msg::statu_code() const {
+  // @@protoc_insertion_point(field_get:message.UnNormalized_module_statu_msg.statu_code)
+  return _internal_statu_code();
+}
+inline void UnNormalized_module_statu_msg::unsafe_arena_set_allocated_statu_code(
+    ::message::Error_manager* statu_code) {
+  if (GetArena() == nullptr) {
+    delete reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(statu_code_);
+  }
+  statu_code_ = statu_code;
+  if (statu_code) {
+    _has_bits_[0] |= 0x00000004u;
+  } else {
+    _has_bits_[0] &= ~0x00000004u;
+  }
+  // @@protoc_insertion_point(field_unsafe_arena_set_allocated:message.UnNormalized_module_statu_msg.statu_code)
+}
+inline ::message::Error_manager* UnNormalized_module_statu_msg::release_statu_code() {
+  _has_bits_[0] &= ~0x00000004u;
+  ::message::Error_manager* temp = statu_code_;
+  statu_code_ = nullptr;
+  if (GetArena() != nullptr) {
+    temp = ::PROTOBUF_NAMESPACE_ID::internal::DuplicateIfNonNull(temp);
+  }
+  return temp;
+}
+inline ::message::Error_manager* UnNormalized_module_statu_msg::unsafe_arena_release_statu_code() {
+  // @@protoc_insertion_point(field_release:message.UnNormalized_module_statu_msg.statu_code)
+  _has_bits_[0] &= ~0x00000004u;
+  ::message::Error_manager* temp = statu_code_;
+  statu_code_ = nullptr;
+  return temp;
+}
+inline ::message::Error_manager* UnNormalized_module_statu_msg::_internal_mutable_statu_code() {
+  _has_bits_[0] |= 0x00000004u;
+  if (statu_code_ == nullptr) {
+    auto* p = CreateMaybeMessage<::message::Error_manager>(GetArena());
+    statu_code_ = p;
+  }
+  return statu_code_;
+}
+inline ::message::Error_manager* UnNormalized_module_statu_msg::mutable_statu_code() {
+  // @@protoc_insertion_point(field_mutable:message.UnNormalized_module_statu_msg.statu_code)
+  return _internal_mutable_statu_code();
+}
+inline void UnNormalized_module_statu_msg::set_allocated_statu_code(::message::Error_manager* statu_code) {
+  ::PROTOBUF_NAMESPACE_ID::Arena* message_arena = GetArena();
+  if (message_arena == nullptr) {
+    delete reinterpret_cast< ::PROTOBUF_NAMESPACE_ID::MessageLite*>(statu_code_);
+  }
+  if (statu_code) {
+    ::PROTOBUF_NAMESPACE_ID::Arena* submessage_arena =
+      reinterpret_cast<::PROTOBUF_NAMESPACE_ID::MessageLite*>(statu_code)->GetArena();
+    if (message_arena != submessage_arena) {
+      statu_code = ::PROTOBUF_NAMESPACE_ID::internal::GetOwnedMessage(
+          message_arena, statu_code, submessage_arena);
+    }
+    _has_bits_[0] |= 0x00000004u;
+  } else {
+    _has_bits_[0] &= ~0x00000004u;
+  }
+  statu_code_ = statu_code;
+  // @@protoc_insertion_point(field_set_allocated:message.UnNormalized_module_statu_msg.statu_code)
+}
+
+// optional string car_license = 4;
+inline bool UnNormalized_module_statu_msg::_internal_has_car_license() const {
+  bool value = (_has_bits_[0] & 0x00000001u) != 0;
+  return value;
+}
+inline bool UnNormalized_module_statu_msg::has_car_license() const {
+  return _internal_has_car_license();
+}
+inline void UnNormalized_module_statu_msg::clear_car_license() {
+  car_license_.ClearToEmpty(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline const std::string& UnNormalized_module_statu_msg::car_license() const {
+  // @@protoc_insertion_point(field_get:message.UnNormalized_module_statu_msg.car_license)
+  return _internal_car_license();
+}
+inline void UnNormalized_module_statu_msg::set_car_license(const std::string& value) {
+  _internal_set_car_license(value);
+  // @@protoc_insertion_point(field_set:message.UnNormalized_module_statu_msg.car_license)
+}
+inline std::string* UnNormalized_module_statu_msg::mutable_car_license() {
+  // @@protoc_insertion_point(field_mutable:message.UnNormalized_module_statu_msg.car_license)
+  return _internal_mutable_car_license();
+}
+inline const std::string& UnNormalized_module_statu_msg::_internal_car_license() const {
+  return car_license_.Get();
+}
+inline void UnNormalized_module_statu_msg::_internal_set_car_license(const std::string& value) {
+  _has_bits_[0] |= 0x00000001u;
+  car_license_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), value, GetArena());
+}
+inline void UnNormalized_module_statu_msg::set_car_license(std::string&& value) {
+  _has_bits_[0] |= 0x00000001u;
+  car_license_.Set(
+    &::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::move(value), GetArena());
+  // @@protoc_insertion_point(field_set_rvalue:message.UnNormalized_module_statu_msg.car_license)
+}
+inline void UnNormalized_module_statu_msg::set_car_license(const char* value) {
+  GOOGLE_DCHECK(value != nullptr);
+  _has_bits_[0] |= 0x00000001u;
+  car_license_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(value),
+              GetArena());
+  // @@protoc_insertion_point(field_set_char:message.UnNormalized_module_statu_msg.car_license)
+}
+inline void UnNormalized_module_statu_msg::set_car_license(const char* value,
+    size_t size) {
+  _has_bits_[0] |= 0x00000001u;
+  car_license_.Set(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), ::std::string(
+      reinterpret_cast<const char*>(value), size), GetArena());
+  // @@protoc_insertion_point(field_set_pointer:message.UnNormalized_module_statu_msg.car_license)
+}
+inline std::string* UnNormalized_module_statu_msg::_internal_mutable_car_license() {
+  _has_bits_[0] |= 0x00000001u;
+  return car_license_.Mutable(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline std::string* UnNormalized_module_statu_msg::release_car_license() {
+  // @@protoc_insertion_point(field_release:message.UnNormalized_module_statu_msg.car_license)
+  if (!_internal_has_car_license()) {
+    return nullptr;
+  }
+  _has_bits_[0] &= ~0x00000001u;
+  return car_license_.ReleaseNonDefault(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), GetArena());
+}
+inline void UnNormalized_module_statu_msg::set_allocated_car_license(std::string* car_license) {
+  if (car_license != nullptr) {
+    _has_bits_[0] |= 0x00000001u;
+  } else {
+    _has_bits_[0] &= ~0x00000001u;
+  }
+  car_license_.SetAllocated(&::PROTOBUF_NAMESPACE_ID::internal::GetEmptyStringAlreadyInited(), car_license,
+      GetArena());
+  // @@protoc_insertion_point(field_set_allocated:message.UnNormalized_module_statu_msg.car_license)
+}
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic pop
+#endif  // __GNUC__
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace message
+
+// @@protoc_insertion_point(global_scope)
+
+#include <google/protobuf/port_undef.inc>
+#endif  // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_UnNormalized_5fmodule_5fmessage_2eproto

Modules/MeasureNode/message/UnNormalized_module_message.proto

@@ -0,0 +1,11 @@
+syntax = "proto2";
+ package message;
+ import "message_base.proto";
+ //地面定位模块测量结果 
+message UnNormalized_module_statu_msg
+ {
+	required Base_info base_info=1; //消息类型 
+	required int32 id=2; 
+	required Error_manager statu_code=3; //模块状态 
+	optional string car_license=4; //号牌数据 
+}