init

.gitignore

@@ -0,0 +1,33 @@
+
+#Ignore thumbnails created by Windows
+Thumbs.db
+#Ignore files built by Visual Studio
+*.obj
+*.exe
+*.pdb
+*.user
+*.aps
+*.pch
+*.vspscc
+*_i.c
+*_p.c
+*.ncb
+*.suo
+*.tlb
+*.tlh
+*.bak
+*.cache
+*.ilk
+*.log
+[Bb]in
+[Dd]ebug*/
+*.lib
+*.sbr
+obj/
+[Rr]elease*/
+_ReSharper*/
+[Tt]est[Rr]esult*
+.vs/
+#Nuget packages folder
+packages/
+.vscode/

CMakeLists.txt

@@ -0,0 +1,118 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(wj_716_lidar)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+add_compile_options(-std=c++11)
+
+find_package(PCL REQUIRED)
+find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${EIGEN3_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  /usr/local/include
+  ./src
+)
+
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/src WJ_LIDAR_SRC)
+
+add_executable(wj_716_lidar ${WJ_LIDAR_SRC} )
+target_link_libraries(wj_716_lidar ${PCL_LIBRARIES} ${Boost_LIBRARIES} ${PROTOBUF_LIBRARIES})
+
+#add_dependencies(wj_716_lidar ${PROJECT_NAME}_gencfg)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/wj_716_lidar.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/wj_716_lidar_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wj_716_lidar.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/error_code/error_code.cpp

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

src/error_code/error_code.h

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

src/task/task_command_manager.cpp

@@ -0,0 +1,44 @@
+//
+// Created by zx on 2019/12/28.
+//
+
+#include "task_command_manager.h"
+#include "../error.h"
+
+Task_Base::Task_Base()
+{
+}
+Task_Base::~Task_Base()
+{
+
+}
+//初始化任务单,初始任务单类型为 UNKONW_TASK
+int Task_Base::init()
+{
+    m_task_type=UNKNOW_TASK;
+    return SUCCESS;
+}
+//更新任务单
+//task_statu: 任务状态
+//statu_information:状态说明
+int Task_Base::update_statu(Task_statu task_statu,std::string statu_information)
+{
+    m_task_statu=task_statu;
+    m_task_statu_information=statu_information;
+    return SUCCESS;
+}
+//获取任务类型
+Task_type Task_Base::get_task_type()
+{
+    return m_task_type;
+}
+//获取任务单状态
+Task_statu  Task_Base::get_statu()
+{
+    return m_task_statu;
+}
+//获取状态说明
+std::string Task_Base::get_statu_information()
+{
+    return m_task_statu_information;
+}

src/task/task_command_manager.h

@@ -0,0 +1,51 @@
+//
+// Created by zx on 2019/12/28.
+//
+
+#ifndef TASK_COMAND_MANAGER_H
+#define TASK_COMAND_MANAGER_H
+#include <string>
+
+//任务类型
+enum Task_type
+{
+    LASER_TASK=0,           //雷达扫描任务
+    LOCATE_TASK,            //测量任务
+    PLC_TASK,                //上传PLC任务
+    UNKNOW_TASK             //未知任务单/初始化
+};
+//任务状态
+enum Task_statu
+{
+    TASK_CREATED=0,     //创建状态
+    TASK_SIGNED,        //已签收
+    TASK_WORKING,       //处理中
+    TASK_OVER           //已结束
+};
+
+//
+class Task_Base
+{
+public:
+    Task_Base();
+    ~Task_Base();
+    //初始化任务单,初始任务单类型为 UNKONW_TASK
+    virtual int init();
+    //更新任务单
+    //task_statu: 任务状态
+    //statu_information:状态说明
+    int update_statu(Task_statu task_statu,std::string statu_information="");
+    //获取任务类型
+    Task_type   get_task_type();
+    //获取任务单状态
+    Task_statu  get_statu();
+    //获取状态说明
+    std::string get_statu_information();
+
+protected:
+    Task_type                   m_task_type;
+    Task_statu                  m_task_statu;               //任务状态
+    std::string                 m_task_statu_information;        //任务状态说明
+};
+
+#endif //TASK_COMAND_MANAGER_H

src/tool/MeasureTopicPublisher.cpp

@@ -0,0 +1,36 @@
+//
+// Created by zx on 2019/11/25.
+//
+
+#include "MeasureTopicPublisher.h"
+#include "sys/time.h"
+#include "unistd.h"
+#include "math.h"
+MeasureTopicPublisher* MeasureTopicPublisher::g_publisher=0;
+MeasureTopicPublisher* MeasureTopicPublisher::GetInstance()
+{
+    if(g_publisher==0)
+    {
+        g_publisher=new MeasureTopicPublisher(CONNECTSTRING);
+    }
+    return g_publisher;
+}
+MeasureTopicPublisher::MeasureTopicPublisher(std::string connectStr)
+{
+    m_sock.bind(connectStr);
+    gettimeofday(&m_last_time,0);
+}
+
+bool MeasureTopicPublisher::Publish(std::string data)
+{
+    std::lock_guard<std::mutex> lk(m_lock);
+    /*double tick=0;
+    do{
+        struct timeval tm;
+        gettimeofday(&tm, 0);
+        tick=fabs((tm.tv_sec-m_last_time.tv_sec)*1000.0+(tm.tv_usec-m_last_time.tv_usec)/1000.0);
+        m_last_time=tm;
+    }while(tick<10);*/
+    usleep(100);
+    return m_sock.send(data)==data.length();
+}

src/tool/MeasureTopicPublisher.h

@@ -0,0 +1,37 @@
+//
+// Created by zx on 2019/11/25.
+//
+
+#ifndef MEASURETOPICSERVER_H
+#define MEASURETOPICSERVER_H
+
+#include <nnxx/message.h>
+#include <nnxx/message_control.h>
+#include <nnxx/socket.h>
+#include <nnxx/pubsub.h>
+#include <nnxx/timeout.h>
+#include <nnxx/error.h>
+#include <string>
+#include <iostream>
+#include <mutex>
+
+#define CONNECTSTRING "tcp://127.0.0.1:10080"
+
+class MeasureTopicPublisher
+{
+public:
+    static MeasureTopicPublisher* GetInstance();
+    bool Publish(std::string data);
+private:
+    MeasureTopicPublisher(std::string connectStr);
+
+private:
+    struct timeval         m_last_time;
+    nnxx::socket m_sock{nnxx::SP, nnxx::PUB};
+
+    std::mutex  m_lock;
+    static MeasureTopicPublisher* g_publisher;
+};
+
+
+#endif //MEASURETOPICSERVER_H

src/tool/StdCondition.cpp

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

src/tool/StdCondition.h

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

src/wj_lidar/async_client.cpp

@@ -0,0 +1,131 @@
+#include "async_client.h"
+
+Async_Client::Async_Client(boost::asio::io_service &io_service, ip::tcp::endpoint endpoint, fundata_t fundata_, void *p)
+    : iosev(io_service),
+      socket(iosev),
+      m_ep(endpoint),
+      mb_connected(0),
+      mb_initialized(0),
+      mb_with_reconnection(0),
+      m_fundata(fundata_),
+      mp_handle(p)
+{
+}
+void Async_Client::client_async_write(char *buf, int len)
+{
+  boost::asio::async_write(socket,
+                           boost::asio::buffer(buf, len),
+                           boost::bind(&Async_Client::handle_write, this,
+                                       boost::asio::placeholders::error));
+}
+void Async_Client::client_async_read()
+{
+  socket.async_read_some(boost::asio::buffer(data_, MAX_LENGTH),
+                         boost::bind(&Async_Client::handle_read, this,
+                                     boost::asio::placeholders::error,
+                                     boost::asio::placeholders::bytes_transferred));
+}
+
+/**
+ * 返回连接状态
+ * */
+bool Async_Client::client_connection_status()
+{
+  return mb_connected;
+}
+
+/**
+ * 异步连接回调
+ * */
+void Async_Client::handle_connect(const boost::system::error_code &error)
+{
+  if (!error)
+  {
+    std::cout << " Connection Success! " << m_ep.address().to_string() << std::endl;
+    mb_connected = true;
+    // 连接成功后开启异步读
+    this->client_async_read();
+  }
+  else
+  {
+    std::cout << boost::system::system_error(error).what() << " start reconnect" << std::endl;
+    mb_connected = false;
+    socket_close();
+    socket_connect();
+  }
+}
+
+/**
+ * 异步读取回调
+ * */
+void Async_Client::handle_read(const boost::system::error_code &error,
+                               size_t bytes_transferred)
+{
+  if (!error)
+  {
+    //printf("From %s Received data Len:%d\n",this->ep.address().to_string().c_str(),
+    // bytes_transferred);
+    if (mp_handle != 0)
+    {
+      // 调用外部回调处理函数后再次进入异步读
+      (*m_fundata)(this->m_ep.address().to_string().c_str(), this->m_ep.port(), data_, bytes_transferred, mp_handle);
+      this->client_async_read();
+    }
+    else
+    {
+      std::cout << "async client, handle null pointer" << std::endl;
+    }
+  }
+  else
+  {
+    std::cout << boost::system::system_error(error).what() << " start reconnect" << std::endl;
+    mb_connected = false;
+    socket_close();
+    socket_connect();
+  }
+}
+
+/**
+ * 异步写入回调
+ * */
+void Async_Client::handle_write(const boost::system::error_code &error)
+{
+  if (!error)
+  {
+  }
+  else
+  {
+    printf("%s. start reconnect", error.message().c_str());
+    mb_connected = false;
+    socket_close();
+    socket_connect();
+  }
+}
+
+/**
+ * 关闭连接
+ * */
+void Async_Client::socket_close()
+{
+  socket.close();
+}
+
+/**
+ * 开始连接
+ * */
+bool Async_Client::socket_connect()
+{
+  socket.async_connect(m_ep,
+                       boost::bind(&Async_Client::handle_connect, this, boost::asio::placeholders::error));
+}
+
+/**
+ * 初始化, 参数为是否开启重连功能
+ * */
+bool Async_Client::initialize(bool with_reconnection)
+{
+  mb_with_reconnection = with_reconnection;
+  socket_connect();
+  mb_initialized = true;
+  return mb_initialized;
+}

src/wj_lidar/async_client.h

@@ -0,0 +1,67 @@
+#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 <string>
+#include <unistd.h>
+
+// using namespace std ;
+using boost::asio::ip::udp;
+using boost::asio::ip::tcp;
+using namespace boost::asio;
+#define MAX_LENGTH 50000
+typedef void (*fundata_t)(const char* addr,int port,const char* data,const int len, void *p);
+
+/**
+ * 异步tcp通信客户端
+ * */
+class Async_Client
+{
+public:
+  // 无参构造
+  Async_Client();
+  // 有参构造
+  Async_Client(boost::asio::io_service& io_service,ip::tcp::endpoint endpoint,fundata_t fundata_ , void *p);
+  // 异步写入
+  void client_async_write(char *buf,int len);
+  // 异步读取
+  void client_async_read();
+  // 连接状态返回
+  bool client_connection_status();
+  // 初始化
+  bool initialize(bool with_reconnection=true);
+  // 连接
+  bool socket_connect();
+  // 关闭连接
+  void socket_close();
+
+private:
+  // 异步读回调
+  void handle_read(const boost::system::error_code& error,
+                   size_t bytes_transferred);
+
+  // 异步写回调
+  void handle_write(const boost::system::error_code& error);
+
+  // 异步连接回调
+  void handle_connect(const boost::system::error_code& error);
+
+  io_service &iosev;                      // 异步控制服务
+  ip::tcp::socket socket;                 // socket句柄
+  ip::tcp::endpoint m_ep;                 // 连接参数
+  boost::system::error_code m_ec;         // 错误码
+
+  char        data_[MAX_LENGTH];          // 原始数据
+  bool        mb_with_reconnection;       // 开启自动重连 
+  bool        mb_connected;               // 连接状态
+  bool        mb_initialized;             // 初始化是否成功
+  fundata_t   m_fundata ;                 // 数据处理回调
+  void*       mp_handle;                  // 外部句柄
+
+};
+
+#endif // ASYNC_CLIENT_H

src/wj_lidar/wj_716_lidar_01.cpp

@@ -0,0 +1,145 @@
+#include <ros/ros.h>
+#include "async_client.h"
+#include "wj_716_lidar_protocol.h"
+using namespace wj_lidar;
+
+int InitTcpConnection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
+{
+
+  io_service iosev;
+
+  ip::tcp::endpoint ep(ip::address_v4::from_string(addr),port);
+  *client_ = new Async_Client(iosev,ep,fundata_);
+
+  iosev.run();
+
+  return 1 ;
+}
+
+int boost_tcp_init_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_)
+{
+  int timecnt=0 ;
+  *client_ = NULL ;
+
+  boost::thread tmp(&InitTcpConnection,addr,port,client_,fundata_);
+  tmp.detach() ;
+
+  while(timecnt<50){
+    timecnt++ ;
+    usleep(20000); //20 ms
+    if((*client_)->client_return_status()){
+      return 0 ;
+    }
+  }
+  *client_ = NULL ;
+  return -1 ;
+
+}
+
+int boost_tcp_sync_send(Async_Client *client_ ,const char* msg,const int len)
+{
+  if(client_==NULL || client_->client_return_status()==0 ){
+    printf("not connected , please connect first \n");
+    return -1 ;
+  }
+  else{
+    client_->client_async_write((char*)msg,len);
+    return 0 ;
+  }
+
+  return 1;
+}
+
+int boost_tcp_sync_read(Async_Client *client_ )
+{
+  if(client_==NULL || client_->client_return_status()==0 ){
+    printf("not connected , please connect first \n");
+    return -1 ;
+  }
+  else{
+    client_->client_async_read();
+    return 0 ;
+  }
+  return 1;
+}
+
+/* ------------------------------------------------------------------------------------------
+ *  show demo --
+ * ------------------------------------------------------------------------------------------ */
+wj_716_lidar_protocol *protocol;
+Async_Client *client;
+void CallBackRead(const char* addr,int port,const char* data,const int len)
+{
+  protocol->data_process(data,len);
+}
+
+void callback(wj_716_lidar::wj_716_lidarConfig &config,uint32_t level)
+{
+  protocol->setConfig(config,level);
+}
+
+void timerCallback(const ros::TimerEvent&)
+{
+//  char heartframe[34]={0xFF,0xAA,0x00,0x1E,0x00,0x00,0x00,0x00,
+//                       0x00,0x00,0x01,0x01,0x00,0x05,0x00,0x00,
+//                       0x00,0x00,0x00,0x00,0x00,0x00,0x05,0x04,
+//                       0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x1A,
+//                       0xEE,0xEE};
+
+//  //cout << "20ms" <<endl;
+//  boost_tcp_sync_send(client,heartframe,34);
+
+  if(protocol->heartstate)
+  {
+    protocol->heartstate=false;
+    cout <<"once heart cycle "<<endl;
+  }
+  else
+  {
+    client->socket_close();
+    client=NULL;
+    ros::NodeHandle nh("~");
+    std::string hostname;
+    nh.getParam("hostname",hostname);
+    std::string port;
+    nh.getParam("port",port);
+
+
+    //InitTcpConnection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+   //boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+    boost_tcp_sync_read(client);
+
+    cout <<"try to reconect "<<endl;
+    cout << hostname.c_str()<<endl;
+    cout << port.c_str()<<endl;
+    //ROS_INFO("Hello wj_716_lidar!");
+  }
+
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "wj_716_lidar_01");
+  ros::NodeHandle nh("~");
+//  ros::Timer timer;
+  std::string hostname;
+  nh.getParam("hostname",hostname);
+  cout << hostname <<endl;
+  std::string port;
+  nh.getParam("port",port);
+
+  protocol = new wj_716_lidar_protocol();
+  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig> server;
+  dynamic_reconfigure::Server<wj_716_lidar::wj_716_lidarConfig>::CallbackType f;
+  f = boost::bind(&callback,_1,_2);
+  server.setCallback(f);
+  client=NULL;
+
+  boost_tcp_init_connection(hostname.c_str(),atoi(port.c_str()),&client,&CallBackRead);
+//  timer= nh.createTimer(ros::Duration(2), timerCallback);
+  boost_tcp_sync_read(client);
+
+  ROS_INFO("Hello wj_716_lidar!");
+  ros::spin();
+
+}

src/wj_lidar/wj_716_lidar_protocol.cpp

@@ -0,0 +1,460 @@
+#include "wj_716_lidar_protocol.h"
+#include <iostream>
+
+namespace wj_lidar
+{
+
+/**
+ * 扫描点云转换为欧式坐标,并根据标定参数移动
+ * */
+Error_manager scan_transform(wj::wjLidarParams params, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
+{
+  if (!params.has_net_config() || !params.has_transform() || !params.has_scan_limit()){
+    return Error_manager(Error_code::PARAMETER_ERROR);
+  }
+
+  float angle = params.angle_min();
+  cloud_out->clear();
+  for (size_t i = 0; i < cloud_in->size(); ++i)
+  {
+    const float first_echo = cloud_in->points[i].x;
+    // within the border
+    if (params.range_min() <= first_echo && first_echo <= params.range_max() && first_echo <= params.scan_limit().dist_limit())
+    {
+      const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
+      Eigen::MatrixXf mat = rotation * (first_echo * Eigen::Vector3f::UnitX());
+      /*Eigen::Matrix<float,4,1>  e_transform;
+            e_transform<<mat,1.0;
+            Eigen::MatrixXf  new_pos=tf_btol*e_transform;*/
+      float x = mat(0, 0);
+      float y = mat(1, 0);
+
+      if (x < params.scan_limit().minx() || x > params.scan_limit().maxx() || y < params.scan_limit().miny() || y > params.scan_limit().maxy())
+      {
+        angle += params.angle_increment();
+        continue;
+      }
+      float newx = x * params.transform().m00() + y * params.transform().m01() + params.transform().m02();
+      float newy = x * params.transform().m10() + y * params.transform().m11() + params.transform().m12();
+      pcl::PointXYZ point(newx, newy, 0);
+      cloud_out->push_back(point);
+      // cloud_in->points[i].x = newx;
+      // cloud_in->points[i].y = newy;
+    }
+    angle += params.angle_increment();
+  }
+  return Error_manager(Error_code::SUCCESS);
+}
+
+/**
+ * 设置参数
+ * */
+void Wj_716_lidar_protocol::set_config(wj::wjLidarParams params)
+{
+  m_lidar_params.CopyFrom(params);
+  LOG(INFO) << m_lidar_params.DebugString();
+  // LOG(INFO) << "min_ang:" << m_lidar_params.angle_min();
+  // LOG(INFO) << "max_ang:" << m_lidar_params.angle_max();
+  // LOG(INFO) << "angle_increment:" << m_lidar_params.angle_increment();
+  // LOG(INFO) << "time_increment:" << m_lidar_params.time_increment();
+  // LOG(INFO) << "range_min:" << m_lidar_params.range_min();
+  // LOG(INFO) << "range_max:" << m_lidar_params.range_max();
+}
+
+/**
+ * 获取更新扫描点云时间
+ * */
+std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_scan_time(){
+  return m_scan_cloud_time;
+}
+
+/**
+ * 获取更新二次回波时间
+ * */
+std::chrono::steady_clock::time_point Wj_716_lidar_protocol::get_two_echo_time(){
+  return m_two_echo_cloud_time;
+}
+
+/**
+ * 获取扫描点云
+ * */
+Error_manager Wj_716_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out){
+  Error_manager result;
+  m_scan_mutex.lock();
+  result = scan_transform(m_lidar_params, mp_scan_points, cloud_out);
+  m_scan_mutex.unlock();
+  if(result!=SUCCESS){
+    LOG(WARNING)<<"获取点云失败，参数错误";
+  }
+  return result;
+}
+
+/**
+ * 获取二次回波点云
+ * */
+Error_manager Wj_716_lidar_protocol::get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out){
+  Error_manager result;
+  m_two_echo_mutex.lock();
+  result = scan_transform(m_lidar_params, mp_two_echo_points, cloud_out);
+  m_two_echo_mutex.unlock();
+  if(result!=SUCCESS){
+    LOG(WARNING)<<"获取点云失败，参数错误";
+  }
+  return result;
+}
+
+/**
+ * 万集协议栈初始化
+ * */
+Error_manager Wj_716_lidar_protocol::initialize(wj::wjLidarParams params)
+{
+  memset(&m_sdata, 0, sizeof(m_sdata));
+
+  m_g_u32PreFrameNo = 0;
+  mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+  mp_two_echo_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+  mp_scan_points->resize(811);
+  mp_two_echo_points->resize(811);
+
+  m_lidar_params.set_angle_min(-2.35619449);
+  m_lidar_params.set_angle_max(2.35619449);
+  m_lidar_params.set_angle_increment(0.00582);
+  m_lidar_params.set_time_increment(0.0667 / 1081);
+  m_lidar_params.set_range_min(0);
+  m_lidar_params.set_range_max(30);
+
+  m_scan_cloud_time = std::chrono::steady_clock::now();
+  m_two_echo_cloud_time = std::chrono::steady_clock::now();
+
+  set_config(params);
+  LOG(INFO) << "wj_716_lidar_protocol start success";
+  return Error_manager(Error_code::SUCCESS);
+}
+
+Wj_716_lidar_protocol::Wj_716_lidar_protocol()
+{
+}
+
+/**
+ * 数据处理主函数
+ **/
+Error_manager Wj_716_lidar_protocol::data_process(const char *data, const int reclen)
+{
+  if (reclen > MAX_LENGTH_DATA_PROCESS)
+  {
+    return Error_manager(Error_code::WJ_PROTOCOL_EXCEED_MAX_SIZE);
+  }
+
+  if (m_sdata.m_u32in + reclen > MAX_LENGTH_DATA_PROCESS)
+  {
+    memset(&m_sdata, 0, sizeof(m_sdata));
+    return Error_manager(Error_code::WJ_PROTOCOL_EXCEED_MAX_SIZE);
+  }
+  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] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out + 1] == 0x02 &&
+        m_sdata.m_acdata[m_sdata.m_u32out + 2] == 0x02 && m_sdata.m_acdata[m_sdata.m_u32out + 3] == 0x02)
+    {
+      unsigned l_u32reallen = (m_sdata.m_acdata[m_sdata.m_u32out + 4] << 24) |
+                              (m_sdata.m_acdata[m_sdata.m_u32out + 5] << 16) |
+                              (m_sdata.m_acdata[m_sdata.m_u32out + 6] << 8) |
+                              (m_sdata.m_acdata[m_sdata.m_u32out + 7] << 0);
+      l_u32reallen = l_u32reallen + 9;
+
+      if (l_u32reallen <= (m_sdata.m_u32in - m_sdata.m_u32out + 1))
+      {
+        Error_manager code = on_recv_process(&m_sdata.m_acdata[m_sdata.m_u32out], l_u32reallen);
+        if (code == SUCCESS)
+        {
+          m_sdata.m_u32out += l_u32reallen;
+        }
+        else
+        {
+          LOG(INFO) << "continuous frame";
+          int i;
+          for (i == 1; i < l_u32reallen; i++)
+          {
+            if ((m_sdata.m_acdata[m_sdata.m_u32out + i] == 0x02) &&
+                (m_sdata.m_acdata[m_sdata.m_u32out + i + 1] == 0x02) &&
+                (m_sdata.m_acdata[m_sdata.m_u32out + i + 2] == 0x02) &&
+                (m_sdata.m_acdata[m_sdata.m_u32out + i + 3] == 0x02))
+            {
+              m_sdata.m_u32out += i;
+              break;
+            }
+            if (i == l_u32reallen)
+            {
+              m_sdata.m_u32out += l_u32reallen;
+            }
+          }
+        }
+      }
+      else if (l_u32reallen >= MAX_LENGTH_DATA_PROCESS)
+      {
+        LOG(INFO) << "l_u32reallen >= MAX_LENGTH_DATA_PROCESS";
+        LOG(INFO) << "reallen: " << l_u32reallen;
+        memset(&m_sdata, 0, sizeof(m_sdata));
+      }
+      else
+      {
+        //cout<<"reallen: "<<l_u32reallen<<" indata: "<<m_sdata.m_u32in<<" outdata: "<<m_sdata.m_u32out<<endl;
+        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)
+  {
+    memset(&m_sdata, 0, sizeof(m_sdata));
+  }
+  return Error_manager(Error_code::SUCCESS);
+}
+
+/**
+ * 获取消息处理
+ * */
+Error_manager Wj_716_lidar_protocol::on_recv_process(char *data, int len)
+{
+  if (len > 0)
+  {
+    Error_manager code = check_xor(data, len);
+    if (code==SUCCESS)
+    {
+      code = protocol(data, len);
+      if(code!=SUCCESS){
+        LOG(WARNING)<< "万集雷达协议解析失败";
+      }
+    }else{
+      LOG(WARNING)<< "万集雷达信息完整性验证失败";
+    }
+  }
+  else
+  {
+    return Error_manager(Error_code::WJ_PROTOCOL_EMPTY_PACKAGE);
+  }
+  return Error_manager(Error_code::SUCCESS);
+}
+
+/**
+ * 协议栈解析
+ * */
+Error_manager Wj_716_lidar_protocol::protocol(const char *data, const int len)
+{
+  if ((data[8] == 0x73 && data[9] == 0x52) || (data[8] == 0x73 && data[9] == 0x53)) //command type:0x73 0x52/0X53
+  {
+    static int s_n32ScanIndex;
+    int l_n32PackageNo = (data[50] << 8) + data[51];                                              //shuju bao xu hao
+    unsigned int l_u32FrameNo = (data[46] << 24) + (data[47] << 16) + (data[48] << 8) + data[49]; //quan hao
+
+    if (l_n32PackageNo == 1)
+    {
+      s_n32ScanIndex = 0;
+      m_g_u32PreFrameNo = l_u32FrameNo;
+
+      for (int j = 0; j < 810; j = j + 2)
+      {
+        m_scandata[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
+        m_scandata[s_n32ScanIndex] /= 1000.0;
+        if (m_scandata[s_n32ScanIndex] >= 25 || m_scandata[s_n32ScanIndex] == 0)
+        {
+          m_scandata[s_n32ScanIndex] = NAN;
+        }
+        s_n32ScanIndex++;
+      }
+      //cout << "quan hao1: " << g_u32PreFrameNo << "  danzhen quanhao1: " << l_u32FrameNo<< endl;
+    }
+    else if (l_n32PackageNo == 2)
+    {
+      if (m_g_u32PreFrameNo == l_u32FrameNo)
+      {
+        m_scan_mutex.lock();
+        //  m_scan_points.clear();
+        for (int j = 0; j < 812; j = j + 2)
+        {
+          m_scandata[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
+          m_scandata[s_n32ScanIndex] /= 1000.0;
+          if (m_scandata[s_n32ScanIndex] >= 25 || m_scandata[s_n32ScanIndex] == 0)
+          {
+            m_scandata[s_n32ScanIndex] = NAN;
+          }
+          // m_scan_points[s_n32ScanIndex].intensity = 0;
+          //  scan.intensities[s_n32ScanIndex] = 0;
+          s_n32ScanIndex++;
+        }
+        m_scan_mutex.unlock();
+      }
+      else
+      {
+        s_n32ScanIndex = 0;
+        //cout << "quan hao2: " << g_u32PreFrameNo << "  danzhen quanhao2: " << l_u32FrameNo<< endl;
+        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
+      }
+    }
+    else if (l_n32PackageNo == 3)
+    {
+      s_n32ScanIndex = 0;
+      if (m_g_u32PreFrameNo == l_u32FrameNo)
+      {
+        for (int j = 0; j < 810; j = j + 2)
+        {
+          scaninden[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
+          s_n32ScanIndex++;
+        }
+      }
+      else
+      {
+        s_n32ScanIndex = 0;
+        //cout << "quan hao3: " << g_u32PreFrameNo << "  danzhen quanhao3: " << l_u32FrameNo<< endl;
+        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
+      }
+    }
+    else if (l_n32PackageNo == 4)
+    {
+      if (m_g_u32PreFrameNo == l_u32FrameNo)
+      {
+        for (int j = 0; j < 812; j = j + 2)
+        {
+          scaninden[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
+          s_n32ScanIndex++;
+        }
+
+        // adjust angle_min to min_ang config param
+        int index_min = (m_lidar_params.angle_min() + 2.35619449) / m_lidar_params.angle_increment();
+        // adjust angle_max to max_ang config param
+        int index_max = 811 - ((2.35619449 - m_lidar_params.angle_max()) / m_lidar_params.angle_increment());
+        m_scan_mutex.lock();
+        mp_scan_points->clear();
+        mp_scan_points->resize(index_max - index_min);
+        //  scan.ranges.resize(index_max-index_min);
+        //  scan.intensities.resize(index_max-index_min);
+
+        for (int j = index_min; j <= index_max; ++j)
+        {
+          mp_scan_points->points[j - index_min].x = m_scandata[j];
+          mp_scan_points->points[j - index_min].y = 0;
+          mp_scan_points->points[j - index_min].z = 0;
+          // m_scan_points[j - index_min].intensity = scaninden[j];
+          //  scan.ranges[j - index_min] = scandata[j];
+          //  scan.intensities[j - index_min]=scaninden[j];
+        }
+        m_scan_mutex.unlock();
+        m_scan_cloud_time = std::chrono::steady_clock::now();
+      }
+      else
+      {
+        s_n32ScanIndex = 0;
+        //cout << "quan hao4: " << g_u32PreFrameNo << "  danzhen quanhao4: " << l_u32FrameNo<< endl;
+        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
+      }
+    }
+    else if (l_n32PackageNo == 5)
+    {
+      s_n32ScanIndex = 0;
+      if (m_g_u32PreFrameNo == l_u32FrameNo)
+      {
+        for (int j = 0; j < 810; j = j + 2)
+        {
+          m_scandata_te[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
+          m_scandata_te[s_n32ScanIndex] /= 1000.0;
+          if (m_scandata_te[s_n32ScanIndex] >= 55 || m_scandata_te[s_n32ScanIndex] == 0)
+          {
+            m_scandata_te[s_n32ScanIndex] = NAN;
+          }
+          s_n32ScanIndex++;
+        }
+      }
+      else
+      {
+        s_n32ScanIndex = 0;
+        //cout << "quan hao5: " << g_u32PreFrameNo << "  danzhen quanhao5: " << l_u32FrameNo<< endl;
+        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
+      }
+    }
+    else if (l_n32PackageNo == 6)
+    {
+      if (m_g_u32PreFrameNo == l_u32FrameNo)
+      {
+        for (int j = 0; j < 812; j = j + 2)
+        {
+          m_scandata_te[s_n32ScanIndex] = (((unsigned char)data[85 + j]) << 8) + ((unsigned char)data[86 + j]);
+          m_scandata_te[s_n32ScanIndex] /= 1000.0;
+          if (m_scandata_te[s_n32ScanIndex] >= 55 || m_scandata_te[s_n32ScanIndex] == 0)
+          {
+            m_scandata_te[s_n32ScanIndex] = NAN;
+          }
+          s_n32ScanIndex++;
+        }
+
+        int index_min = (m_lidar_params.angle_min() + 2.35619449) / m_lidar_params.angle_increment();
+        // adjust angle_max to max_ang config param
+        int index_max = 811 - ((2.35619449 - m_lidar_params.angle_max()) / m_lidar_params.angle_increment());
+        m_two_echo_mutex.lock();
+        mp_two_echo_points->clear();
+        mp_two_echo_points->resize(index_max - index_min);
+        //  scan_TwoEcho.ranges.resize(index_max-index_min);
+        //  scan_TwoEcho.intensities.resize(index_max-index_min);
+
+        for (int j = index_min; j <= index_max; ++j)
+        {
+          mp_two_echo_points->points[j - index_min].x = m_scandata_te[j];
+          mp_two_echo_points->points[j - index_min].y = 0;
+          mp_two_echo_points->points[j - index_min].z = 0;
+          // m_two_echo_points[j - index_min].intensity = 0;
+          //  scan_TwoEcho.ranges[j - index_min] = scandata_te[j];
+          //  scan_TwoEcho.intensities[j - index_min] =0;
+        }
+        m_two_echo_mutex.unlock();
+        m_two_echo_cloud_time = std::chrono::steady_clock::now();
+      }
+      else
+      {
+        s_n32ScanIndex = 0;
+        //cout << "quan hao6: " << g_u32PreFrameNo << "  danzhen quanhao6: " << l_u32FrameNo<< endl;
+        return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
+      }
+    }
+    return Error_manager(Error_code::SUCCESS);;
+  }
+  else
+  {
+    return Error_manager(Error_code::WJ_PROTOCOL_PARSE_FAILED);
+  }
+}
+
+/**
+ * 数据包完整性检查 
+ **/
+Error_manager Wj_716_lidar_protocol::check_xor(char *recvbuf, int recvlen)
+{
+  int i = 0;
+  char check = 0;
+  char *p = recvbuf;
+  int len;
+  if (*p == (char)0x02)
+  {
+    p = p + 8;
+    len = recvlen - 9;
+    for (i = 0; i < len; i++)
+    {
+      check ^= *p++;
+    }
+    if (check == *p)
+    {
+      return Error_manager(Error_code::SUCCESS);
+    }
+    else
+      return Error_manager(Error_code::WJ_PROTOCOL_INTEGRITY_ERROR);
+  }
+  else
+  {
+    return Error_manager(Error_code::WJ_PROTOCOL_INTEGRITY_ERROR);
+  }
+}
+
+} // namespace wj_lidar

src/wj_lidar/wj_716_lidar_protocol.h

@@ -0,0 +1,93 @@
+#ifndef WJ_716_LIDAR_PROTOCOL_H
+#define WJ_716_LIDAR_PROTOCOL_H
+#include <iostream>
+#include "string.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 <mutex>
+#include <chrono>
+
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_types.h>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/conversions.h>
+#include <pcl/common/common.h>
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::Message;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+
+#include "glog/logging.h"
+
+#include "wj_lidar_conf.pb.h"
+
+#include "../error_code/error_code.h"
+
+// using namespace std ;
+namespace wj_lidar
+{
+#define MAX_LENGTH_DATA_PROCESS 200000
+typedef struct tag_data_cache
+{
+  char m_acdata[MAX_LENGTH_DATA_PROCESS];
+  unsigned int m_u32in;
+  unsigned int m_u32out;
+} data_cache;
+class Wj_716_lidar_protocol
+{
+public:
+  Wj_716_lidar_protocol();
+  // 初始化
+  Error_manager initialize(wj::wjLidarParams params);
+  // 数据处理
+  Error_manager data_process(const char *data, const int reclen);
+  // 万集通信协议
+  Error_manager protocol(const char *data, const int len);
+  // 接收消息
+  Error_manager on_recv_process(char *data, int len);
+  // 消息完整性检查
+  Error_manager check_xor(char *recvbuf, int recvlen);
+  // 获取扫描点云
+  Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out);
+  Error_manager get_two_echo_points(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out);
+  // 获取扫描点云更新时间点
+  std::chrono::steady_clock::time_point get_scan_time();
+  std::chrono::steady_clock::time_point get_two_echo_time();
+
+private:
+  // 设置参数
+  void set_config(wj::wjLidarParams params);
+
+private:
+  // 万集雷达参数
+  wj::wjLidarParams m_lidar_params;
+  data_cache m_sdata;
+  // 扫描获取点云
+  pcl::PointCloud<pcl::PointXYZ>::Ptr mp_scan_points;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr mp_two_echo_points;
+  // 点云获取互斥锁
+  std::mutex m_scan_mutex;
+  std::mutex m_two_echo_mutex;
+
+  unsigned int m_g_u32PreFrameNo;
+  float m_scandata[811];
+  float m_scandata_te[811];
+  float scaninden[811];
+
+  // 扫描点云更新时间点
+  std::chrono::steady_clock::time_point m_scan_cloud_time;
+  std::chrono::steady_clock::time_point m_two_echo_cloud_time;
+};
+
+} // namespace wj_lidar
+#endif // WJ_716_LIDAR_PROTOCOL_H

src/wj_lidar/wj_lidar_conf.proto

@@ -0,0 +1,40 @@
+syntax = "proto2";
+package wj;
+
+message wjLidarParams
+{
+	optional float angle_min=1 [default=-2.35619449];
+	optional float angle_max=2 [default=2.35619449];
+	optional float angle_increment=3 [default=0.00582];
+	optional float time_increment=4 [default=0.000062];
+	optional int32 range_min=5 [default=0];
+	optional int32 range_max=6 [default=30];
+	optional netConfig net_config=7;
+    optional Transform2d transform=8;
+	optional scanLimit scan_limit=9;
+}
+
+message netConfig
+{
+	optional string ip_address=1 [default=""];
+	optional int32 port=2 [default=8000];
+}
+
+message Transform2d
+{
+    optional float m00=1 [default=1.0];
+    optional float m01=2 [default=0.0];
+    optional float m02=3 [default=0.0];
+    optional float m10=4 [default=0.0];
+    optional float m11=5 [default=1.0];
+    optional float m12=6 [default=0.0];
+}
+
+message scanLimit
+{
+	optional float dist_limit=1 [default=8.0];
+    optional float minx=2 [default=-6];
+    optional float maxx=3 [default=-0.2];
+    optional float miny=4 [default=-3.5];
+    optional float maxy=5 [default=3.5];
+}

src/wj_lidar/wj_lidar_encapsulation.cpp

@@ -0,0 +1,195 @@
+#include "wj_lidar_encapsulation.h"
+
+/**
+ * 初始化连接
+ * */
+void Wj_lidar_encapsulation::init_tcp_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_)
+{
+
+  io_service iosev;
+
+  ip::tcp::endpoint ep(ip::address_v4::from_string(addr), port);
+  *client_ = new Async_Client(iosev, ep, fundata_, this);
+
+  iosev.run();
+
+}
+
+/**
+ * 创建初始化与读写线程以及连接超时判断
+ * */
+Error_manager Wj_lidar_encapsulation::boost_tcp_init_connection(const char *addr, int port, Async_Client **client_, fundata_t fundata_)
+{
+  int timecnt = 0;
+  *client_ = NULL;
+
+  boost::thread tmp(&init_tcp_connection, addr, port, client_, fundata_);
+  tmp.detach();
+
+  while (timecnt < 50)
+  {
+    timecnt++;
+    usleep(20000); //20 ms
+    if ((*client_)->client_connection_status())
+    {
+      return Error_manager(Error_code::SUCCESS);
+    }
+  }
+  *client_ = NULL;
+  return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
+}
+
+/**
+ * 异步写指令
+ * */
+Error_manager Wj_lidar_encapsulation::boost_tcp_async_send(Async_Client *client_, const char *msg, const int len)
+{
+  if (client_ == NULL || client_->client_connection_status() == 0)
+  {
+    LOG(WARNING) << "not connected , need to connect first \n";
+    return Error_manager(Error_code::WJ_LIDAR_WRITE_FAILED);
+  }
+  else
+  {
+    client_->client_async_write((char *)msg, len);
+    return Error_manager(Error_code::SUCCESS);
+  }
+}
+
+/**
+ * 异步读数据
+ * */
+Error_manager Wj_lidar_encapsulation::boost_tcp_async_read(Async_Client *client_)
+{
+  if (client_ == NULL || client_->client_connection_status() == 0)
+  {
+    LOG(WARNING) << "not connected , need to connect first \n";
+    return Error_manager(Error_code::WJ_LIDAR_READ_FAILED);
+  }
+  else
+  {
+    client_->client_async_read();
+    return Error_manager(Error_code::SUCCESS);;
+  }
+}
+
+/**
+ * 万集读数据回调
+ * 
+ * */
+void Wj_lidar_encapsulation::read_callback(const char *addr, int port, const char *data, const int len, void *handle)
+{
+  if (handle != 0)
+  {
+    Wj_lidar_encapsulation *p_wj_temp = (Wj_lidar_encapsulation *)handle;
+    if (p_wj_temp->mp_protocol != 0)
+    {
+      if (p_wj_temp->mp_protocol->data_process(data, len) != SUCCESS)
+      {
+        LOG(WARNING) << p_wj_temp->m_ip << " data process failed, exceed max length";
+      }
+    }
+    else
+    {
+      LOG(WARNING) << p_wj_temp->m_ip << " protocol null pointer";
+    }
+  }
+}
+
+Error_manager Wj_lidar_encapsulation::get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
+                                                std::chrono::steady_clock::time_point command_time, int timeout_milli)
+{
+  if (!mb_initialized || mp_protocol == 0)
+  {
+    return Error_manager(Error_code::WJ_LIDAR_UNINITIALIZED);
+  }
+  std::chrono::steady_clock::time_point scan_time_temp = mp_protocol->get_scan_time();
+  while (std::chrono::duration_cast<std::chrono::milliseconds>(scan_time_temp - command_time).count() < timeout_milli)
+  {
+
+  }
+
+  if (!mp_protocol->get_scan_points(cloud))
+  {
+    return Error_manager(Error_code::WJ_LIDAR_PROTOCOL_FAILED);
+  }
+  
+
+  cloud->clear();
+  // m_mutex.lock();
+  cloud->operator+=(*m_cloud);
+  //    std::cout<<cloud->size()<<std::endl;
+  //    return true;
+  ros::Duration duration = ros::Time::now() - m_cloud_time;
+  if (print)
+  {
+    // if (m_lidar_param.topic() == "/scan3" || m_lidar_param.topic() == "/scan4" || m_lidar_param.topic() == "/scan5")
+    // {
+    std::cout << m_lidar_param.topic() << " duration: " << duration.toSec() * 1000 << ", cloud size: " << cloud->size() << std::endl;
+    // }
+  }
+  if (duration.toSec() * 1000 > timeout_milli)
+    return false;
+  else
+    return true;
+}
+
+/**
+ * 万集雷达封装类初始化
+ * */
+Error_manager Wj_lidar_encapsulation::initialize(wj::wjLidarParams params)
+{
+  // 1. 判断网络参数合法性并初始化异步读写实例
+  if (params.has_net_config())
+  {
+    m_ip = params.net_config().ip_address();
+    m_port = params.net_config().port();
+    if (boost_tcp_init_connection(m_ip.c_str(), m_port, &mp_client, &read_callback) == SUCCESS)
+    {
+      if (boost_tcp_async_read(mp_client) != SUCCESS)
+      {
+        LOG(WARNING) << m_ip << " null or disconnected.";
+        return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
+      }
+    }
+    else
+    {
+      LOG(WARNING) << m_ip << " 1 sec timeout, connection failed.";
+      return Error_manager(Error_code::WJ_LIDAR_CONNECT_FAILED);
+    }
+  }else{
+    return Error_manager(Error_code::PARAMETER_ERROR);
+  }
+  // 2. 创建协议栈对象
+  mp_protocol = new Wj_716_lidar_protocol();
+  Error_manager code = mp_protocol->initialize(params);
+  if(code != SUCCESS){
+    LOG(ERROR) << code.to_string();
+  }else{
+    mb_initialized = true;
+  }
+  return code;
+}
+
+// 万集雷达封装类构造函数
+Wj_lidar_encapsulation::Wj_lidar_encapsulation() : mb_initialized(false),
+                                                   mb_connected(false),
+                                                   mp_protocol(0),
+                                                   mp_client(0)
+{
+}
+
+// 万集雷达封装类析构函数
+Wj_lidar_encapsulation::~Wj_lidar_encapsulation()
+{
+  if (mp_client != 0)
+  {
+    delete mp_client;
+    mp_client = 0;
+  }
+  if (mp_protocol != 0)
+  {
+    delete mp_protocol;
+    mp_protocol = 0;
+  }
+}

src/wj_lidar/wj_lidar_encapsulation.h

@@ -0,0 +1,45 @@
+#ifndef WJ_LIDAR_ENCAPSULATION_HH
+#define WJ_LIDAR_ENCAPSULATION_HH
+
+#include "async_client.h"
+#include "wj_716_lidar_protocol.h"
+#include "../tool/StdCondition.h"
+#include "../error_code/error_code.h"
+
+using namespace wj_lidar;
+
+class Wj_lidar_encapsulation{
+
+public:
+    // 无参构造函数
+    Wj_lidar_encapsulation();
+    // 析构函数
+    ~Wj_lidar_encapsulation();
+    // 初始化
+    Error_manager initialize(wj::wjLidarParams params);
+    // 万集雷达协议，数据获取回调
+    static void read_callback(const char* addr,int port,const char* data,const int len, void *handle);
+    // 外部调用获取点云
+    Error_manager get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::chrono::steady_clock::time_point command_time, int timeout_milli=1500);
+
+private:
+    // 初始化雷达连接
+    void init_tcp_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_);
+    // 连接与异步读写线程创建
+    Error_manager boost_tcp_init_connection(const char* addr,int port,Async_Client **client_,fundata_t fundata_);
+    // 异步写入
+    Error_manager boost_tcp_async_send(Async_Client *client_ ,const char* msg,const int len);
+    // 异步读取
+    Error_manager boost_tcp_async_read(Async_Client *client_ );
+
+private:
+    Wj_716_lidar_protocol*  mp_protocol;        // 万集雷达协议句柄
+    Async_Client*           mp_client;          // 异步客户端句柄
+    bool                    mb_initialized;     // 初始化Flag
+    bool                    mb_connected;       // 连接状态Flag
+    std::string             m_ip;               // IP地址
+    int                     m_port;             // 端口
+    wj::wjLidarParams       m_wj_params;        // 万集参数
+};
+
+#endif // !WJ_LIDAR_ENCAPSULATION_HH