Merge branch 'master' of http://116.63.46.168:3000/ADPilot/modularization

include/ivpcl.pri

@@ -8,3 +8,4 @@ unix:INCLUDEPATH += /usr/include/pcl-1.7
 unix:INCLUDEPATH += /usr/include/pcl-1.8
 unix:INCLUDEPATH += /usr/include/pcl-1.12
 unix:INCLUDEPATH += /usr/include/pcl-1.10
+unix:INCLUDEPATH += /usr/include/pcl-1.14

src/detection/detection_lidar_centerpoint/main.cpp

@@ -37,7 +37,7 @@ void init()
     yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.0);
     const std::string densification_world_frame_id = "map";
     const int densification_num_past_frames = 0;//1;
-    const std::string trt_precision = "fp16";
+    const std::string trt_precision = "fp32";
     const std::string encoder_onnx_path = "/home/nvidia/models/pts_voxel_encoder_centerpoint.onnx";//this->declare_parameter<std::string>("encoder_onnx_path");
     const std::string encoder_engine_path ="/home/nvidia/models/pts_voxel_encoder_centerpoint.eng";//this->declare_parameter<std::string>("encoder_engine_path");
     const std::string head_onnx_path = "/home/nvidia/models/pts_backbone_neck_head_centerpoint.onnx";//this->declare_parameter<std::string>("head_onnx_path");
@@ -172,7 +172,6 @@ void ListenPointCloud(const char * strdata,const unsigned int nSize,const unsign
 //        xp.z = p->z;
 //        xp.intensity = p->intensity;
 //        point_cloud->push_back(xp);
-//        p++;
     }
 
 //    std::cout<<"pcl time is "<<xTime.elapsed()<<std::endl;

src/detection/detection_lidar_transfusion/detection_class_remapper.cpp

@@ -0,0 +1,71 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#include <detection_class_remapper.hpp>
+
+namespace autoware::lidar_transfusion
+{
+
+void DetectionClassRemapper::setParameters(
+  const std::vector<int64_t> & allow_remapping_by_area_matrix,
+  const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix)
+{
+  assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size());
+  assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size());
+  assert(
+    std::pow(static_cast<int>(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size());
+
+  num_labels_ = static_cast<int>(std::sqrt(min_area_matrix.size()));
+
+  Eigen::Map<const Eigen::Matrix<int64_t, Eigen::Dynamic, Eigen::Dynamic>>
+    allow_remapping_by_area_matrix_tmp(
+      allow_remapping_by_area_matrix.data(), num_labels_, num_labels_);
+  allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose()
+                                      .cast<bool>();  // Eigen is column major by default
+
+  Eigen::Map<const Eigen::MatrixXd> min_area_matrix_tmp(
+    min_area_matrix.data(), num_labels_, num_labels_);
+  min_area_matrix_ = min_area_matrix_tmp.transpose();  // Eigen is column major by default
+
+  Eigen::Map<const Eigen::MatrixXd> max_area_matrix_tmp(
+    max_area_matrix.data(), num_labels_, num_labels_);
+  max_area_matrix_ = max_area_matrix_tmp.transpose();  // Eigen is column major by default
+
+  min_area_matrix_ = min_area_matrix_.unaryExpr(
+    [](double v) { return std::isfinite(v) ? v : std::numeric_limits<double>::max(); });
+  max_area_matrix_ = max_area_matrix_.unaryExpr(
+    [](double v) { return std::isfinite(v) ? v : std::numeric_limits<double>::max(); });
+}
+
+//void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg)
+//{
+//  for (auto & object : msg.objects) {
+//    const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y;
+
+//    for (auto & classification : object.classification) {
+//      auto & label = classification.label;
+
+//      for (int i = 0; i < num_labels_; ++i) {
+//        if (
+//          allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) &&
+//          bev_area <= max_area_matrix_(label, i)) {
+//          label = i;
+//          break;
+//        }
+//      }
+//    }
+//  }
+//}
+
+}  // namespace autoware::lidar_transfusion

src/detection/detection_lidar_transfusion/detection_class_remapper.hpp

@@ -0,0 +1,47 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_
+#define AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_
+
+#include <Eigen/Core>
+
+//#include <autoware_perception_msgs/msg/detected_object_kinematics.hpp>
+//#include <autoware_perception_msgs/msg/detected_objects.hpp>
+//#include <autoware_perception_msgs/msg/object_classification.hpp>
+//#include <autoware_perception_msgs/msg/shape.hpp>
+
+#include <vector>
+
+namespace autoware::lidar_transfusion
+{
+
+class DetectionClassRemapper
+{
+public:
+  void setParameters(
+    const std::vector<int64_t> & allow_remapping_by_area_matrix,
+    const std::vector<double> & min_area_matrix, const std::vector<double> & max_area_matrix);
+//  void mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg);
+
+protected:
+  Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> allow_remapping_by_area_matrix_;
+  Eigen::MatrixXd min_area_matrix_;
+  Eigen::MatrixXd max_area_matrix_;
+  int num_labels_;
+};
+
+}  // namespace autoware::lidar_transfusion
+
+#endif  // AUTOWARE__LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_

src/detection/detection_lidar_transfusion/detection_lidar_transfusion.pro

@@ -8,6 +8,7 @@ CONFIG -= app_bundle
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
+        detection_class_remapper.cpp \
         main.cpp \
         network/network_trt.cpp \
         postprocess/non_maximum_suppression.cpp \
@@ -15,6 +16,7 @@ SOURCES += \
         preprocess/voxel_generator.cpp \
         simple_profiler.cpp \
         tensorrt_common.cpp \
+        transfusion_call.cpp \
         transfusion_trt.cpp
 
 # Default rules for deployment.
@@ -29,6 +31,7 @@ DISTFILES += \
 
 HEADERS += \
     cuda_utils.hpp \
+    detection_class_remapper.hpp \
     include/network/network_trt.hpp \
     include/postprocess/circle_nms_kernel.hpp \
     include/postprocess/non_maximum_suppression.hpp \
@@ -39,6 +42,7 @@ HEADERS += \
     include/tensorrt_common/logger.hpp \
     include/tensorrt_common/simple_profiler.hpp \
     include/tensorrt_common/tensorrt_common.hpp \
+    transfusion_call.h \
     transfusion_trt.hpp \
     utils.hpp \
     visibility_control.hpp
@@ -143,5 +147,23 @@ else {
 # include paths
 
 
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization
 
 LIBS += -lrt -ldl -lnvinfer -lcudnn  -lcudart -lnvparsers -lnvonnxparser -lnvinfer_plugin -lstdc++fs

src/detection/detection_lidar_transfusion/include/postprocess/non_maximum_suppression.hpp

@@ -46,18 +46,7 @@ struct NMSParams
   // double distance_threshold_{};
 };
 
-std::vector<bool> classNamesToBooleanMask(const std::vector<std::string> & class_names)
-{
-  std::vector<bool> mask;
-  constexpr std::size_t num_object_classification = 8;
-  mask.resize(num_object_classification);
-  for (const auto & class_name : class_names) {
-    const auto semantic_type = getSemanticType(class_name);
-    mask.at(semantic_type) = true;
-  }
-
-  return mask;
-}
+
 
 class NonMaximumSuppression
 {

src/detection/detection_lidar_transfusion/main.cpp

@@ -1,8 +1,35 @@
 #include <QCoreApplication>
 
+#include "transfusion_call.h"
+
+transfusion_call * gptf;
+
+#include <pcl/io/pcd_io.h>
+void testdet(std::string & path)
+{
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    pcl::io::loadPCDFile<pcl::PointXYZI>(path,*point_cloud);
+
+    std::vector<Box3D> det_boxes3d;
+    int i;
+    for(i=0;i<10;i++)
+    {
+    gptf ->detect(point_cloud,det_boxes3d);
+    std::cout<<" box size: "<<det_boxes3d.size()<<std::endl;
+    }
+}
+
+
 int main(int argc, char *argv[])
 {
     QCoreApplication a(argc, argv);
 
+    gptf = new transfusion_call();
+
+    std::string strpath = "/home/nvidia/1.pcd";
+    testdet(strpath);
+
     return a.exec();
 }

src/detection/detection_lidar_transfusion/postprocess/non_maximum_suppression.cpp

@@ -61,9 +61,25 @@ uint8_t getSemanticType(const std::string & class_name)
 }
 
 
+
 namespace autoware::lidar_transfusion
 {
 
+
+static std::vector<bool> classNamesToBooleanMask(const std::vector<std::string> & class_names)
+{
+  std::vector<bool> mask;
+  constexpr std::size_t num_object_classification = 8;
+  mask.resize(num_object_classification);
+  for (const auto & class_name : class_names) {
+    const auto semantic_type = getSemanticType(class_name);
+    mask.at(semantic_type) = true;
+  }
+
+  return mask;
+}
+
+
 void NonMaximumSuppression::setParameters(const NMSParams & params)
 {
   assert(params.search_distance_2d_ >= 0.0);

src/detection/detection_lidar_transfusion/transfusion_call.cpp

@@ -0,0 +1,45 @@
+#include "transfusion_call.h"
+
+transfusion_call::transfusion_call()
+{
+
+    std::string engine_path ="/home/nvidia/models/transfusion.eng";
+    std::string onnx_path = "/home/nvidia/models/transfusion.onnx";
+    std::string trt_precision ="fp16" ;
+
+    std::string densification_world_frame_id = "world";
+    int densification_num_past_frames = 0;
+
+    std::size_t cloud_capacity = 2000000;
+    std::vector<int64_t> voxels_num;
+    std::vector<double>  point_cloud_range;
+    std::vector<double>  voxel_size;
+    std::size_t num_proposals = 500;
+    float circle_nms_dist_threshold = 0.5;
+    std::vector<double>  yaw_norm_thresholds;
+    float score_threshold = 0.2;
+
+    voxels_num.push_back(5000);voxels_num.push_back(30000);voxels_num.push_back(60000);
+    point_cloud_range.push_back(-76.8);point_cloud_range.push_back(-76.8);point_cloud_range.push_back(-3.0);
+    point_cloud_range.push_back(76.8);point_cloud_range.push_back(76.8);point_cloud_range.push_back(5.0);
+    voxel_size.push_back(0.3);voxel_size.push_back(0.3);voxel_size.push_back(8.0);
+    yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.3);
+    yaw_norm_thresholds.push_back(0.3);yaw_norm_thresholds.push_back(0.0);
+
+    NetworkParam network_param(onnx_path, engine_path, trt_precision);
+    DensificationParam densification_param(
+      densification_world_frame_id, densification_num_past_frames);
+    TransfusionConfig config(
+      cloud_capacity, voxels_num, point_cloud_range, voxel_size, num_proposals,
+      circle_nms_dist_threshold, yaw_norm_thresholds, score_threshold);
+
+    detector_ptr_ = std::make_unique<TransfusionTRT>(network_param, densification_param, config);
+}
+
+bool transfusion_call::detect(
+  const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr,
+  std::vector<Box3D> & det_boxes3d)
+{
+    std::unordered_map<std::string, double> proc_timing;
+    return detector_ptr_->detect(pc_ptr,det_boxes3d,proc_timing);
+}

src/detection/detection_lidar_transfusion/transfusion_call.h

@@ -0,0 +1,33 @@
+#ifndef TRANSFUSION_CALL_H
+#define TRANSFUSION_CALL_H
+
+
+#include "detection_class_remapper.hpp"
+#include "postprocess/non_maximum_suppression.hpp"
+#include "preprocess/pointcloud_densification.hpp"
+#include "transfusion_trt.hpp"
+#include "visibility_control.hpp"
+
+
+using namespace autoware::lidar_transfusion;
+
+class transfusion_call
+{
+public:
+    transfusion_call();
+
+    bool detect(
+      const pcl::PointCloud<pcl::PointXYZI>::Ptr & pc_ptr,
+      std::vector<Box3D> & det_boxes3d);
+
+private:
+    DetectionClassRemapper detection_class_remapper_;
+    float score_threshold_{0.0};
+    std::vector<std::string> class_names_;
+
+    NonMaximumSuppression iou_bev_nms_;
+
+    std::unique_ptr<TransfusionTRT> detector_ptr_{nullptr};
+};
+
+#endif // TRANSFUSION_CALL_H

src/driver/driver_lidar_bk_16z/.gitignore

@@ -0,0 +1,74 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+CMakeLists.txt.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_lidar_bk_16z/ICD_LiDAR_API.h

@@ -0,0 +1,949 @@
+/********************************************************************
+ * $I
+ * @Technic Support: <sdk@isurestar.com>
+ * All right reserved, Sure-Star Coop.
+ ********************************************************************/
+#ifndef ICD_LIDAR_API_H_
+#define ICD_LIDAR_API_H_
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <stdint.h>
+#ifndef _WINDOWS
+	#include <unistd.h>
+	#include <sys/time.h>
+#endif
+#pragma pack(2)
+
+const char LIDAR_IPADDR_DFLT[] = "192.168.0.188";  	//!< 默认IP：192.168.0.188
+const char LIDAR_IPADDR_WIFI[] = "192.168.10.1" ;   //!< 无线控制 IP：192.168.10.1
+const char XFANS_IPADDR_DFLT[] = "224.0.0.1" ;  	//!< 默认IP：192.168.0.188
+
+//TCP port
+const int LIDAR_MSGPORT = 2008;  	//消息数据端口
+const int LIDAR_PNTPORT = 2009;  	//雷达数据端口
+const int LIDAR_IMGPORT = 2011;  	//图像数据端口
+const int LIDAR_SLWPORT = 2012;  	//导航数据端口
+const int LIDAR_GPSPORT = 2020;  	//GNSS传输端口
+
+//UDP port
+const int LIDAR_UDPPORT 			= 2013 ;  		//UDP CMD 端口
+const int XFANS_DATA_UDPPORT 		= 2014 ;		//LIDAR_UDP_DATA_UDPPORT
+const int LIDAR_UDP_DATA_UDPPORT 	= 2014 ;		//... obsolete
+const int XFANS_CMD_UDPPORT 		= 2015 ;		//LIDAR_UDP_CMD_UDPPORT
+const int LIDAR_UDP_CMD_UDPPORT		= 2015 ;		//... obsolete
+const int LIDAR_GPSMSG_UDPPORT 		= 2016 ;  		//端口
+const int SCADA_UDP_CPORT 			= 2017 ;		// 本地UDP端口号
+const int IPCFG_UDP_CPORT 			= 2018 ;		// 本地UDP端口号，用于IP配置
+const int GROUND_DATAPORT 			= 2208 ;  		// ATG 消息数据端口
+
+const int LIDAR_UDP_SLW_UDPPORT 	= 2025 ;
+const int LIDAR_UDP_HEART_DBG_UDPPORT = 2030 ;
+
+const int COMMON_BUFFER_SIZE = 10000;
+
+typedef enum {
+    e_msgCnt = 0x01,
+    e_pntCnt = 0x02,
+    e_imgCnt = 0x04,
+    e_slwCnt = 0x08,
+    e_gpsCnt = 0x10,
+    e_allCnt = 0x1F,        // e_msgCnt | e_pntCnt | e_imgCnt | e_slwCnt | e_gpsCnt
+    e_allTcp = 0x1F1F,
+	//
+    e_udpMsg = 0x010000,    //0x01 << 16
+    e_udpGps = 0x020000,
+    e_udpAtg = 0x040000,
+    e_udppnt = 0x080000, // udp快数据
+    e_udpslw = 0x100000, // udp慢数据
+    e_udphet = 0x200000, // upd心跳包
+    e_udpPet = 0x400000,
+    e_udpCmdV40 = 0x800000,
+    e_allUdp = 0x7f0000,
+	//
+	e_allEth = 0x5FFFFF,
+} ICNT_CNTSTATE_E; //before ICNT_STATE_E
+
+/// CMDID字段命令码定义
+typedef enum {
+    eMsgSoftCk,        ///< 0x0
+    eMsgEnvir,         ///< 0x1
+    eMsgCamera,        ///< 0x2
+    eMsgScanner,       ///< 0x3
+    eMsgTurret,        ///< 0x4
+    eMsgLaser,         ///< 0x5
+    eMsgProgam,        ///< 0x6
+    eMsgAll,           ///< 0x7
+    eMsgStat,          ///< 0x8
+    eMsgRegister,      ///< 0x9
+    eMsgRuler,         ///< 0xA
+    eMsgUsbCmr,        ///< 0xB
+    eMsgUmrTrig,       ///< 0xC
+    eMsgHeader,        ///< 0xD
+    eMsgString,        ///< 0xE
+    eMsgStepMto,       ///< 0xF
+    eMsgDataInfo,      ///< 0x10
+    eMsgCameraRprt,    ///< 0x11
+    eMsgSensorRprt,    ///< 0x12
+    eMsgCompassRead,   ///< 0x13
+    eMsgFpgaStat,      ///< 0x14
+    eMsgDataSave,      ///< 0x15
+    eMsgTaskStop,      ///< 0x16
+    eMsgDMI,           ///< 0x17
+    eMsgPCM,           ///< 0x18
+    eMsgPDT,           ///< 0x19
+    eMsgPST,           ///< 0x1A
+    eMsgEnvirAsk,      ///< 0x1B
+    eMsgSyncTime,      ///< 0x1C
+    eMsgGpsTrack,      ///< 0x1D
+    eLidarReset,       ///< 0x1E
+    eMsgStorgeDir,     ///< 0x1F
+    eMsgAppDeviceMeta, ///< 0x20  设备信息
+    eMsgAPPCountPro,   ///< 0x21  统计工程数量
+    eMsgAPPGetPro,     ///< 0x22  获取工程信息
+    eMsgAPPGetProg,    ///< 0x23  获取工程作业信息
+    eMsgAPPGetCommPro, ///< 0x24  获取工程备注信息
+    eMsgAPPUpdatePro,  ///< 0x25  更新工程信息
+    eMsgAPPDeletePro,  ///< 0x26  删除工程
+    eMsgAPPAddCommPro, ///< 0x27  增加工程备注
+    eMsgAPPCreatePro,  ///< 0x28  新建工程
+    eMsgBatchReadRequest,    ///< 0x29
+    eMsgBatchReadRprt,       ///< 0x2A
+    eMsgBatchWriteRequest,   ///< 0x2B
+    eMsgBatchWriteRprt,      ///< 0x2C
+	eMsgMovProgam,           ///< 0x2D
+	eMsgECKGetState,         ///< 0x2E
+	eMsgECKCtrlAlarm,        ///< 0x2F
+	eMsgECKGetBatInfo,       ///< 0x30
+	eMsgECKCtrlPtz,          ///< 0x31
+    eMsgfindDeviceReq,       ///< 0x32 发现设备请求命令
+    eMsgfindDeviceResp,      ///< 0x33 发现设备命令应答
+    eMsgIPCfgReq,            ///< 0x34 IP配置请求命令
+    eMsgIPCfgResp,           ///< 0x35 IP配置命令应答
+    eMsgheartBeatReq,        ///< 0x36 设备心跳信息请求命令
+    eMsgheartBeatResp,       ///< 0x37 设备心跳信心命令应答
+    eMsgDataPath,			///< 0x38 修改文件存储路径指令
+    eMsgAppSvrLocation,		///< 0x39
+    eMsgAppSvrGetcatalog,	///< 0x3a
+    eMsgAppSvrCatalognum,	///< 0x3b
+	eMsgAppSvrUpdatecatalog,	///< 0x3c
+	eMsgAppSvrRemovecatalog,	///< 0x3d
+	eMsgAppSvrAddcomment,		///< 0x3e
+	eMsgAppSvrGetcomment,		///< 0x3f
+	eMsgAppSvrUpdatecomment,	///< 0x40
+    eMsgDACSetReq,			///< 0x41 DAC设备电压设置请求命令
+    eMsgDACSetResp,			///< 0x42 DAC设备电压设置命令应答
+    eMsgTMPGetReq,			///< 0x43 温度读取命令请求
+    eMsgTMPGetResp,			///< 0x44 温度读取命令应答
+    eMsgEnvirExt,         	///< 0x45
+    eMsgMta,         		///< 0x46
+    eMsgCameraRpt,			///< 0x47
+    eMsgADCGetReq,			///< 0x48 ADC数据读取命令
+    eMsgADCGetResp,			///< 0x49 ADC数据读取命令应答
+    eMsgCfgGtrlReq,			///< 0x4A 配置文件控制命令
+    eMsgCfgGtrlResp,		///< 0x4B 配置文件控制命令应答
+
+} LiDAR_MSGID_E, LiDAR_CMDID_E;
+
+const uint16_t OPEN_LIDAR_CMD_NUM = 0x37;
+const uint16_t LIDAR_BODY_MAX = 1028;
+const uint16_t LIDAR_BODY_MAX_OLD = 64;
+
+typedef enum {
+    eLidarProgError,        // 0
+    eLidarProgStandBy,    // 1
+	eLidarProgDateSet,		// 2
+    eLidarProgReady,        // 3
+    eLidarProgEnvir,        // 4
+    eLidarProgCamera,    // 5
+    eLidarProgMotor,    // 6
+    eLidarProgStage,    // 7
+    eLidarProgLaser,    // 8
+    eLidarProgStart,    // 9
+    eLidarProgAction,    // 10 eLidarProgReady <-> eLidarProgAction
+//    eLidarCmrScaning,    // 10 eLidarProgReady <-> eLidarCmrScaning
+//    eLidarCmrWaitImg1,
+//    eLidarCmrWaitImg2,
+//    eLidarCmrTrigOK,
+    eLidarStopAll,
+//    eLidarProgBreak,
+//    eLidarCmrInit,
+//    eLidarCmrInited,
+//    eLidarCmrFinished,
+//    eLidarUCmrInit,
+//    eLidarUCmrScaning,
+//    eLidarUCmrWaiting,
+//    eLidarUCmrFinished,
+//    eLidarCheckStagStop,
+//    eLidarUCmrTrigger,
+//    eLidarCmrTrigger,
+// 	  elidarProgPre,		// obsolete
+} LDRPROG_STAT_E, LiDAR_STATMACH_E;
+
+typedef enum {
+    eDevCmdIdle = 0,
+    eDevCmdWork,
+    eDevCmdSimu,
+//    eDevCmdBreak,
+    eDevCmdReset,
+	eDevCmdAsk, //查询指令
+} SCDEV_CMD_E, LDRDEV_CMD_S;
+
+typedef struct atg_ {
+    unsigned int header;
+    float height;
+    float staDeviation;
+    unsigned int packetIndex;
+    unsigned int checksum_crc32;
+} GROUND_DATA_S;
+
+typedef struct {//ref: struct tm from <time.h>
+    unsigned short tm_sec;    	/* Seconds (0-60) */
+    unsigned short tm_min;    	/* Minutes (0-59) */
+    unsigned short tm_hour;    	/* Hours (0-23) */
+    unsigned short tm_mday;    	/* Day of the month (1-31) */
+    unsigned short tm_mon;      /* Month (0-11) */
+    unsigned short tm_year;     /* Year - 1900 */
+    unsigned int   tm_msec ; 	/* before: wMilliseconds */
+//	unsigned short tm_wday;   /* Day of the week (0-6, Sunday = 0) */
+//	unsigned short tm_yday;   /* Day in the year (0-365, 1 Jan = 0) */
+//	unsigned short tm_isdst;  /* Daylight saving time */
+} CSYSTEMTIME;
+
+typedef enum {//Must consistent with the defination of 0x71 register (FPU)
+	eGNSS_Applanix , 	//	eGpsApplanix	= 0 ,
+  	eGNSS_Trimble , 		//	eGpsTrimble		= 1 ,
+  	eGNSS_Noval , 		//	eGpsNoval		= 2 ,
+  	eGNSS_Javat , 		//	eGpsJavat		= 3 , //not consistent with 0x71
+  	eGNSS_Nmea , 		//	eGpsNmea 		= 4 ,
+  	eGNSS_Gsof ,		//	eGpsGsof 		= 5 ,
+  	eGNSS_Inpps , 		//	eGPSInpps 		= 6 ,
+  	eGNSS_Unicore ,		//
+  	eGNSS_Unic982 ,
+  	eGNSS_Unic482 ,
+  	eGNSS_SinoK823
+} GNSS_TYPE_E , GPS_TYPE_E/* obsolete */ ;
+
+typedef struct envir_cmds_ {
+    CSYSTEMTIME syncTime;    //
+    int tz_secs;            //secs, timezone,
+    int diff_gps_utc;        //secs, the difference between GPS and UTC
+    GNSS_TYPE_E gps_etype;            //obsolete ref : GPS_TYPE_E
+    unsigned int ro_ttsecs;      //Epoch time(time_t), auto calc. from syncTime ; before : utcTime
+//  unsigned int utcSecs ;  	//obsolete
+//  bool pps_bool , utc_bool , gps_bool ; //obsolete
+//obsolete  unsigned char imuAmSign ;       
+//obsolete  unsigned char imuDataState  ;
+//obsolete  float fGPSx , fGPSy , fGPSz ; 
+} SCDCMD_ENVIR_S, SCDCMD_TIMSYN_S;
+
+typedef struct envir_rpts_ {
+    // Environments
+    float temp[2], hemi[2];             //!< unit degree and % ; //obsolete
+    //gnss ....
+    GNSS_TYPE_E gps_etype;              
+	volatile unsigned int gnstmiTime;        	//!< 0x74 0x75 : YYMMDDHH, totally 4 Bytes ; before: gnsUtcTime or UtcTime
+    /// gnsUtcSecs是time_t类型，在32位arm上是4字节，在64位x86_64机器上是8字节，统一改成uint64_t
+	volatile  /*time_t*/uint64_t gnsUtcSecs;            		//!< ... check FPGA to make sure it is utc format ; before: utcSec
+	//  unsigned int ro_ttsecs ;		//auto calc. from gnstmTime, 
+    unsigned short gns_satelites; 		//!< obsolete , ::imu_bsignal, gns_signals
+    unsigned short imu_signals;         //!< ... ? make sure the meaning ; --- IMU
+    //unsigned short ins_bsignal ;    	//!< bit15:12(POS) ; bit11:8(INS) ; bit7:4(IMU); bit3:0(GPS)
+    //coordinate
+    float fAtg , fdiv ;						//
+    float fGPSx, fGPSy, fGPSz;        	//!< GPS
+    float incX, incY;                    //!< inclinometer X and Y
+//    float heading, pitch, roll;
+//zks: which one ?  float gnsLongi, gnsLati , gnsAlti; 		//!< GPS
+//obsolete  short altitude ;          	//!<
+//obsolete  unsigned short airspeed ; 	//!< 
+//obsolete  float compassAng;         	//!< compass angle
+} SCADA_STATE_S, SCDRPT_ENVIR_S;        // before : SCADA_HWINFO_S ,
+
+typedef struct {
+    float heading, pitch, roll;
+	bool pps ;
+	float atgHeight;
+} SCDRPT_ENVIR_EXT_S;
+
+typedef struct {
+	LiDAR_STATMACH_E pntcld_state ;	
+    unsigned int diskSize ;
+    // points statistics
+    unsigned short intens_min, intens_max;      //����ķ���ǿ��(0, 2047)��
+    float ranges_min, ranges_max;        		// unit :  m
+//    float datarecv_rate, datarecv_volumn;    	// kbps ; KB -- updated only in thread lidarAPI->dataRecvHanle()
+    float datahandle_rate, datahandle_volumn;    // kbps ; KB -- updated only in thread IdataHandle->dataHanle() ;
+    bool sdcard_flag;                           //SD卡是否存在：true存在，false不存在
+    //obsolete: LDRPROG_STAT_E lsrState ;
+    //obsolete: float  nRangeScopeMin_;			//has defination in other structure
+    //obsolete: float  nRangeScopeMax_ ;
+    //obsolete: double fDataTotal_ ;           	//�������� KB //obsolete
+    //obsolete: unsigned int   curImpSize_ ;    //��ǰIMP��С KB
+    //obsolete: PRE_DATASTATE_E dataState;  	//����״̬
+    //obsolete: LDRUSB_STATE_E usbDiskStat;     //
+    //obsolete: SL_DFRAME_E frameState;
+} SCDRPT_PNTCLD_S, LiDAR_PNTCLD_S; //// before SCADA_DATAFUN_S
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// *  <1/3> Laser
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+const int SCDLSR_POWR_DFLT = 100;          //!< 100%
+const int SCDLSR_POWR_MIN = 50;            //!< 50%
+const int SCDLSR_POWR_MAX = 100;            //!< 100%
+const int SCDLSR_FREQ_DFLT = 200;            //!< 200 kHz
+const int SCDLSR_FREQ_MIN = 30;
+const int SCDLSR_FREQ_MAX = 600;            //!< 600 kHz
+
+//! laser
+typedef struct {// < 60 Bytes
+    SCDEV_CMD_E cmdID;
+    int freqKHz;
+    int ampPercent;
+	unsigned short laser_freq_enable;
+} SCDCMD_LASER_S;
+
+typedef struct {// < 60 Bytes
+    SCDEV_CMD_E statID;
+    int freqKHz;
+    int ampPercent;
+    int optingTemp;
+} SCDRPT_LASER_S;
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// * <2/3> Scanner
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+const int SCDSCN_SPEED_DFLT = 1800;
+const int SCDSCN_SPEED_MIN = 0;
+const int SCDSCN_SPEED_MAX = 6000;
+
+typedef enum {
+    standReady = 0x0,
+    eScanStand = 0x1,
+    eScanError = 0x2,
+} SCDSCN_STATE_E;
+
+typedef struct {  // must < 60 Byte
+    SCDEV_CMD_E cmdID;
+    int scan_emode;            // ref: SCDSCN_MODE_E
+    float angSpeed;            //!< depends on SCDSCN_MODE_E
+    unsigned short roistart[4], roistop[4];
+} SCDCMD_SCANER_S;
+
+typedef struct {  // must < 60 Byte
+    SCDEV_CMD_E statID;
+    unsigned int speedCount;    //
+    SCDSCN_STATE_E speedState;    //
+    int grade; //int32_t
+} SCDRPT_SCANER_S;
+
+typedef enum {             //!< 转台工作模式
+  eTurretReset     = 0x00, //!< 重置
+  eTurretContinue  = 0x01, //!< 连续模式
+  eTurretArea      = 0x02, //!< 区域模式
+  eTurretPosition  = 0x04, //!< 位置模式
+  eTurretNextTrige = 0x08, //!< 下一个曝光点
+  eTurretStop      = 0x10, //!< 停止
+  eTurretSearch    = 0x20, //!< 查询
+} SCDTURRET_MODE_E ;
+
+typedef struct turret_s_ {
+  SCDEV_CMD_E cmdID ;
+  SCDTURRET_MODE_E stag_emode ;		//ref: SCDTURRET_MODE_E
+  float angSpeed ;
+  float strAngle , stpAngle;          	//!<
+  unsigned short roistr[4], roistp[4] ;
+} SCDCMD_TURRET_S ;
+
+typedef struct {
+  SCDEV_CMD_E statID ;
+  float currAng ;                     //!< 当前角度
+} SCDRPT_TURRET_S ;
+
+typedef enum {               //!< 相机曝光模式
+  eCmrTimerNero,             //!< 时间曝光, 无零位触发
+  eCmrTimerZero,             //!< 时间曝光, 有零位触发
+  eCmrAngleNero,             //!< 角度曝光, 无零位触发
+  eCmrAngleZero,             //!< 角度曝光, 有零位触发
+  eCmrPosnNero,              //!< 位置曝光, 无零位触发
+  eCmrPosnZero,              //!< 位置曝光, 有零位触发
+  eCmrRangeNero,             //!< 距离曝光, 无零位触发
+  eCmrRangeZero,             //!< 距离曝光, 有零位触发
+} SCDCMR_MODE_E;
+
+typedef struct _cmr_cmds_ { // must < 60 bytes
+  SCDEV_CMD_E cmdID ;        	//!< 
+  SCDCMR_MODE_E trigger_emode ; //!< ref: SCDCMR_MODE_E
+  int interval ;               	//!< 
+  int angOffset ;              	//!< 
+  unsigned int msgCount;       	//!< 
+  float strAngle , stpAngle;   	//!< 
+} SCDCMD_CAMERA_S ;
+
+#define CMR_NUMBER 6 // 1 trige + 6 flash
+typedef struct {
+  SCDEV_CMD_E statID ;        	//!<
+  unsigned int flashCount[CMR_NUMBER] ;
+  unsigned int trigCount ;
+} SCDRPT_CAMERA_S ;
+
+typedef struct{
+    SCDEV_CMD_E cmdID;
+	char dataDir[60];
+}SCDCMD_DATA_DIR_S;
+
+typedef struct {//lidarsvr-657
+	unsigned int freq12;//freq1[15:0] freq2[31:16]
+	unsigned int freq34;//freq3[15:0] freq4[31:16]
+	unsigned int freqRef;//base freq
+	unsigned int freqRefSPeriod;//freq start Period
+	unsigned int scanFreq;
+	unsigned int onoff;
+} SCDCMD_MTA_S;
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// * <3/3> Programming
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+/*
+typedef struct _prog_cmds_ {
+    SCDEV_CMD_E cmdID;
+    //basic ...
+    int echo_etype;                //0x04 by default ; ref: ECHO_TYPE_E
+    int laserFreqKHz; //
+    int laserFirePwr; // %
+    float scanAngleSpeed;        // RPM
+    float stagAngleSpeed;        // RPM, obsolete
+    //advanced  ...
+    int range_from, range_to;    //the ranging scope
+    int angle_start, angle_stop; //the anguar scope
+    int prog_emode;                //0x0 by default ; obsolete : ref : SCDSCAN_TYPE_E ;
+} SCDCMD_PROGRM_S;
+*/
+//sdk2.0 to here
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// *                           数据采集
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+#define MPMODE_REVISE_RANGE (120)  //!< MP模式过滤杂点数据 单位m
+typedef enum {
+	eProgrmModeNon = 0,
+	eProgrmModeMP = 1,
+}SCDMODE_PROGRAM_E;
+
+#define ROI_NUMBER (2)
+
+typedef enum {
+	eNetFlow = 0x01,
+	eDeviceSave = 0x02,
+	ePCSave = 0x04, //?
+	eUSBSave = 0x08,
+	eMemorySave = 0x10,
+}SCDMODE_DATAPLACE_S;
+
+
+typedef enum {
+	eFileStay, // no action to file handler
+	eFileNew,
+	eFileRenew,
+	eFileClose,
+	eFileCloseAll,
+	eFileImpClose,
+	eFileIssClose,
+} LiDARImp_Action_E;
+
+/************************************************************************************
+ *
+ *************************************************************************************/
+typedef enum { 
+  ePoints_NULL ,
+  ePoints_NetFlow , 	//eNetFlow = 0x01,
+  ePoints_DevSave , 	//eDeviceSave = 0x02 ,
+  ePoints_PcSave  , 	//ePCSave = 0x04 , 
+  ePoints_UsbSave , 	//eUSBSave = 0x08,
+  ePoints_MemSave , 	//eMemorySave = 0x10,
+} SCDMODE_DATAPLACE_E , Point_DataFlow_E;
+
+typedef enum {
+  eDataNULL   ,
+  eIMPDATA	  , //= 0x1 ,
+  eXYZIDATA   , //= 0x2 ,
+  eARIDATA	  , //= 0x4 ,
+  eCRSDATA	  , //= 0x8 ,
+  eWUDDATA	  , //= 0x10,
+  eNARIDATA   , //= 0x12,
+  eGEOXYZDAT  , //= 0x14,
+  eCPTDATA	  , //= 0x16,
+  eSTDATA	  , //= 0x18,
+  ePLRDATA	  , //= 0x20,
+  eISSDATA	  , //= 0x21,
+  ePMISSDATA  , //= 0x22,
+} DATA_TYPE_E , Point_DataType_E ;
+
+//
+typedef struct {
+	// data collecting Setups
+	LDRDEV_CMD_S cmdID ;
+	Point_DataFlow_E data_flow ;				// storePlace ;
+	Point_DataType_E data_type ; 		 		// saving format
+} SCDCMD_DCTRL_S , SCDCMD_DATACTRL_S ;
+
+
+typedef struct _prog_cmds_ {
+    SCDEV_CMD_E cmdID;
+    //basic ...
+    int echo_etype;                //0x04 by default ; ref: ECHO_TYPE_E
+    int laserFreqKHz; //
+    int laserFirePwr; // %
+    float scanAngleSpeed;        // RPM
+    float stagAngleSpeed;        // RPM, obsolete
+    //advanced  ...
+    int range_from, range_to;    //the ranging scope
+    int angle_start, angle_stop; //the anguar scope
+    int prog_emode;                //0x0 by default ; obsolete : ref : SCDSCAN_TYPE_E ;
+} SCDCMD_PROGRM_S;
+
+typedef struct{
+	float sHoriAngle;// 0到360
+	float sVertAngle;// -25到15
+	float ptzTurnSpd;// 0.1到0.5度每秒, 0为不转
+	unsigned char scanDataFrame; //frame number
+	unsigned char state;
+	unsigned short recvTmOutMs;//超时时间ms,0表示不设置
+	SCDCMD_PROGRM_S prog;
+	unsigned char ptzPosDelayTime;//转到指定位置后延时扫描时间，单位s
+} SCDCMD_MOV_PROGRM_S;//后面考虑移出
+
+typedef struct{
+	int state;//1表示运行中，0表示空闲
+} SCDCMD_GET_PROSTATE_S;
+
+typedef struct{
+	int onoff;//1打开0关闭
+	unsigned char vol;//level 1-30
+} SCDCMD_CTRL_ALARM_S;
+
+typedef struct{
+	unsigned int power;
+	float voltage;
+	float current;
+} SCDCMD_GET_BATINFO_S;
+
+typedef struct{
+	unsigned char ctrl;//1 reset, 2 move to pos, 3 get pos
+	float HoriAngle;
+	float VertAngle;
+} SCDCMD_CTL_PTZ_S;
+
+
+typedef struct {
+    int request;  // 
+} SCDCMD_APPDeviceMeta_S;
+
+
+typedef struct {
+    float tempDc ; //温度，单位°
+    float batPer; //电池电量百分比
+    float tfCapPer; //容量百分比 如：60.1，表示容量已用60.1%
+    int deviceID ;  //序列号
+    char softVer[16];//固件版本
+    char hwVer[8];   //硬件版本
+    char proctorData[8];//生产日期,如：20220801
+} SCDRPT_APPDeviceMeta_S;
+
+
+typedef struct {
+    int request;  // 
+} SCDCMD_APPCountPro_S;
+
+typedef struct {
+  unsigned short count;  //如5个工程
+} SCDRPT_APPCountPro_S;
+
+
+
+typedef struct {
+    unsigned short num;  // 如：0-4
+} SCDCMD_APPGetPro_S;
+
+typedef struct {
+  char progId[13]; //220810181012
+  char proName[22]; //工程名称
+} SCDRPT_APPGetPro_S;
+
+
+typedef struct {
+    char progId[13];  //工程id
+} SCDCMD_APPGetProg_S;
+
+
+typedef struct {
+   char progStartTime[16];  //开始时间: 20220803181000
+   unsigned int proRunTime; // 作业时长，单位秒
+   unsigned short  proRoom; //工程作业所占空间大小,单位MB
+   char pos[40];
+} SCDRPT_APPGetProg_S;
+
+typedef struct {
+    char progId[13];  //工程id
+} SCDCMD_APPGetCommPro_S;
+
+typedef struct {
+  char proComment[53]; //工程备注
+} SCDRPT_APPGetCommPro_S;
+
+
+typedef struct {
+    char progId[13];   //工程id
+    char newProName[22];   //新工程名称
+} SCDCMD_APPUpdatePro_S;
+
+
+typedef struct {
+    int updateResult;  //0 表示成功，其他值失败
+} SCDRPT_APPUpdatePro_S;
+
+
+typedef struct {
+    char progId[13];   //工程id
+} SCDCMD_APPDeletePro_S;
+
+
+typedef struct {
+    int deleteResult;  //0 表示成功，其他值失败
+} SCDRPT_APPDeletePro_S;
+
+typedef struct {
+    char progId[13];   //工程id
+    char proComment[53];
+} SCDCMD_APPAddCommPro_S;
+
+
+typedef struct {
+    int addResult;  //0 表示成功，其他值失败
+} SCDRPT_APPAddCommPro_S;
+
+
+typedef struct {
+  char proName[22]; //工程名称
+  char pos[40];//位置信息
+} SCDCMD_APPCreatePro_S;
+
+typedef struct {
+   int newResult;
+} SCDRPT_APPCreatePro_S;
+
+
+typedef struct {
+    SCDEV_CMD_E cmdID;
+    int startResult;  //0 表示成功，其他值失败
+} SCDRPT_PROGRAMStart_S;
+
+typedef struct {
+    SCDEV_CMD_E cmdID;
+    int stopresult;    //0 表示成功，其他值失败
+    char progId[13];  //当前工程id
+} SCDRPT_PROGRAMStop_S;
+
+typedef struct{
+	unsigned char flag;//0 表示读，1表示写
+	unsigned int reg;
+	unsigned int value;
+}SCDCMD_REGISTER_S;
+
+//批量读请求
+typedef struct{
+    //批量地址数组，最大可读取16组地址数据，当只需查询3组地址数据时，后13组数据用0填充
+ 	unsigned int request[16];
+}SCDCMD_BATCH_READ_S;
+
+//批量读反馈
+typedef struct{
+    //批量地址数组对应的value
+	unsigned int response[16];
+}SCDRPT_BATCH_READ_S;
+
+//批量写请求
+typedef struct{
+    //批量地址、数值数组，地址、数值交替出现，最多可写8组数据，当只需写3组数据时，后5组数据用0填充
+	unsigned int request[16];
+}SCDCMD_BATCH_WRITE_S;
+
+//批量写反馈
+typedef struct{
+    //批量地址、数值数组，地址、数值交替出现
+	unsigned int response[16];
+}SCDRPT_BATCH_WRITE_S;
+
+/// 发现设备命令内容结构
+typedef struct SCDCMD_FINDDEV_S
+{
+    unsigned char request[64]; ///< 这里的body只是为了占位里面的命令内容将会被忽略
+}SCDCMD_FINDDEV_S;
+
+/// 发现设备命令回应结构
+typedef struct SCDRPT_FINDDEV_S
+{
+    uint8_t arrui8Vender[16];   ///< 设备厂商名称
+    uint8_t arrui8devName[16];  ///< 设备名称
+    uint32_t ui32devVersion;    ///< 设备程序版本
+    uint32_t ui32devStatus;     ///< 设备状态
+    uint32_t ui32devMAC[2];     ///< 设备MAC
+    uint32_t ui32devIPMode;     ///< 设备当前IP配置模式
+    uint32_t ui32devIP;         ///< 设备当前IP
+    uint32_t ui32devMask;       ///< 设备当前子网掩码
+    uint32_t ui32devGateway;    ///< 设备当前网关
+}SCDRPT_FINDDEV_S;
+
+/// IP模式配置设置命令
+typedef struct  SCDCMD_IPCFG_S 
+{
+    uint32_t ui32mode;   ///< IP配置模式
+    uint32_t ui32IP;     ///< 当前配置的IP
+    uint32_t ui32mask;   ///< 当前要配置的子网掩码
+    uint32_t ui32gateway;///< 当前要配置的网关
+    uint32_t ui32MAC[2]; ///< MAC地址，根据这个去选择是否接受配置
+}SCDCMD_IPCFG_S;
+/// IP模式配置命令返回
+typedef struct  SCDRPT_IPCFG_S 
+{
+    uint32_t ui32status; ///< 返回状态可能的定义见LiDARIPCfg.cpp中ipcfg函数返回值
+}SCDRPT_IPCFG_S;
+
+/// 心跳包命令
+typedef struct  SCDCMD_HEARTBEAT_S 
+{
+   uint32_t devstatus;   ///< 写的话要写的状态值。0 - 空闲  1 - 只读 2 - 控制
+    uint8_t request[60];
+}SCDCMD_HEARTBEAT_S ;
+/// 心跳包命令返回
+typedef struct  SCDRPT_HEARTBEAT_S 
+{
+    uint32_t cmdstatus;  ///< 读的话返回设备状态，写的话返回写入后的状态
+    uint8_t response[60]; ///< 命令占位
+}SCDRPT_HEARTBEAT_S;
+
+/// DAC的设备类型
+typedef enum DAC_DEV_TYPE
+{
+    DAC101 = 0,
+    DAC104 = 1,
+}DAC_DEV_TYPE;
+/// DAC设备通道表示，目前最多4通道
+typedef enum DAC_DEV_CHANNEL
+{
+    CHANNEL_0 = 0,
+    CHANNEL_1 = 1,
+    CHANNEL_2 = 2,
+    CHANNEL_3 = 3,
+    CHANNEL_ALL = 4,
+}DAC_DEV_CHANNEL;
+/// DAC电压输出设置
+typedef struct SCDCMD_DAC_S
+{
+    DAC_DEV_TYPE    dactype;
+    DAC_DEV_CHANNEL dacchannel;
+    float  Voltage;
+}SCDCMD_DAC_S;
+/// DAC电压设置反馈
+typedef struct  SCDRPT_DAC_S 
+{
+    uint32_t status;  ///< 设置状态
+}SCDRPT_DAC_S;
+/// 温度芯片的设备类型
+typedef enum TMP_DEV_TYPE
+{
+    TMP422 = 0,
+}TMP_DEV_TYPE;
+/// 温度芯片设备通道表示，目前最多5通道
+typedef enum TMP_DEV_CHANNEL
+{
+    TMP_LOCAL = 0,
+    TMP_REMOTE_1 = 1,
+    TMP_REMOTE_2 = 2,
+    TMP_REMOTE_3 = 3,
+    TMP_REMOTE_4 = 4,
+}TMP_DEV_CHANNEL;
+/// 温度读取请求
+typedef struct SCDCMD_TMP_S
+{
+    TMP_DEV_TYPE    tmptype;
+    TMP_DEV_CHANNEL tmpchannel;
+}SCDCMD_TMP_S;
+/// 温度读取反馈
+typedef struct  SCDRPT_TMP_S 
+{
+    TMP_DEV_TYPE    tmptype;
+    TMP_DEV_CHANNEL tmpchannel;
+    uint32_t status;    ///< 返回读取状态
+    float    tmp_value; ///< 返回读取到的值
+}SCDRPT_TMP_S;
+
+/// ADC转换的设备类型
+typedef enum ADC_DEV_TYPE
+{
+    XADC = 0,
+}ADC_DEV_TYPE;
+/// ADC读取请求
+typedef struct SCDCMD_ADC_S
+{
+    ADC_DEV_TYPE    adctype;
+    uint32_t adcchannel;
+}SCDCMD_ADC_S;
+/// ADC读取反馈
+typedef struct  SCDRPT_ADC_S
+{
+    ADC_DEV_TYPE    adctype;
+    uint32_t  adcchannel;
+    uint32_t status;    ///< 返回读取状态
+    float    adc_value; ///< 返回读取到的值
+}SCDRPT_ADC_S;
+
+/// 配置文件控制请求
+typedef struct SCDCMD_CFGCTRL
+{
+    uint32_t flag;           ///< 0为读，1为写
+    uint32_t total_lens;     ///< 如果标志位读，则是已读取到的数量，如果是写，则是要写入的总数
+    uint32_t data_offset;    ///< 如果为读，此位无效，如果位写，此为是正在写入数据在整个数据中的偏移
+    uint8_t  data_buf[1016]; ///< 文件数据
+}SCDCMD_CFGCTRL_S;
+/// 配置文件控制反馈
+typedef struct  SCDRPT_CFGCTRL
+{
+    uint32_t  total_lens;     ///< 如果为读命令的回应，要读取文件的整体大小，如果是写命令的应答，则是写入了多少文件
+    uint32_t  data_offset;    ///< 如果为读命令的回应，就是当前包在整体的偏移位置，如果写写命令的应答，此位无效
+    uint8_t  data_buf[1020];  ///< 文件数据
+}SCDRPT_CFGCTRL_S;
+/***********************************************************************
+ ***********************************************************************
+ **** Communication Interface
+ ***********************************************************************
+ ***********************************************************************/
+typedef struct cmd_lidar_L1 {
+    LiDAR_MSGID_E cmdId ;               //!<
+    int isAsk ;
+    union {
+        SCDCMD_ENVIR_S envCtrl;            //! SCDCMD_TIMSYN_S  synData ;
+        SCDRPT_ENVIR_S envRprt;
+		SCDRPT_ENVIR_EXT_S envExtRprt;
+        SCDRPT_PNTCLD_S pntRprt;        //! real time data collection, before : infoData
+        SCDCMD_LASER_S lsrCtrl;        //!
+        SCDRPT_LASER_S lsrRprt;
+        SCDCMD_SCANER_S scnCtrl;        //!
+        SCDRPT_SCANER_S scnRprt;
+        SCDCMD_PROGRM_S prgCtrl;        //!
+        SCDCMD_REGISTER_S regCtrl;        //!
+        SCDCMD_DATACTRL_S dataCtrl;
+		//	  USBCMR_CTRL_S    umrData ;		//!
+		//	  USBCMR_TRIG_S    umrTrig ;		//!
+		SCDCMD_TURRET_S  stgData ;
+		SCDRPT_TURRET_S  stgRprt;
+		//	  SCDCMD_STEPMOTOR_S stepMtoData ;	//!
+		//	  SCADA_FPGA_STATE fpgaData ;		//! FPGA状态	 --merged with regCtrl ;
+		//	  SCADA_DATAFUN_S  storageData ;	//!
+		//	  SCDCMD_DMI_S	   dmiData ;		//!
+		SCDCMD_CAMERA_S  pmCmrData ;		//!
+		SCDRPT_CAMERA_S  cmrRprt;
+		//	  SCDCMD_PALM_S    pmDate ; 		//!
+		//	  SCDCMD_PALMSTG_S pmStgData ;		//!
+		//	  SCDCFG_LIMIT_S   limitData ;
+		SCDCMD_MTA_S 	mtaData;
+		SCDCMD_APPDeviceMeta_S appDevMeatCtrl;
+        SCDRPT_APPDeviceMeta_S appDevMeatRprt;
+        SCDCMD_APPCountPro_S   appCountCtrl;
+        SCDRPT_APPCountPro_S   appCountRprt;
+        SCDCMD_APPGetPro_S     appGetProCtrl;
+        SCDRPT_APPGetPro_S     appGetProRprt;
+        SCDCMD_APPGetProg_S    appGetProgCtrl;
+        SCDRPT_APPGetProg_S    appGetProgRprt;
+        SCDCMD_APPGetCommPro_S appGetCommProCtrl;
+        SCDRPT_APPGetCommPro_S appGetCommProRprt;
+        SCDCMD_APPUpdatePro_S  appUpdateProCtrl;
+        SCDRPT_APPUpdatePro_S  appUpdateProRprt;
+        SCDCMD_APPDeletePro_S  appDeleteProCtrl;
+        SCDRPT_APPDeletePro_S  appDeleteProRprt;
+        //SCDCMD_APPAddCommPro_S appAddCommProCtrl;
+        SCDRPT_APPAddCommPro_S appAddCommProRprt;
+        SCDCMD_APPCreatePro_S  appCreateProCtrl;
+        SCDRPT_APPCreatePro_S  appCreateProRprt;
+        SCDRPT_PROGRAMStart_S  appProgramStartRprt;
+        SCDRPT_PROGRAMStop_S   appProgramStopRprt;
+
+        SCDCMD_BATCH_READ_S    batchReadCtrl;
+        SCDRPT_BATCH_READ_S    batchReadRprt;
+        SCDCMD_BATCH_WRITE_S   batchWriteCtrl;
+        SCDRPT_BATCH_WRITE_S   batchWriteRprt;
+
+		SCDCMD_DATA_DIR_S	dataDir;
+
+        SCDCMD_MOV_PROGRM_S movPrgCtrl;
+		SCDCMD_GET_PROSTATE_S getPrgState;
+		SCDCMD_CTRL_ALARM_S ctrlAlarm;
+		SCDCMD_GET_BATINFO_S getBatInfo;
+		SCDCMD_CTL_PTZ_S ctrlPtz;
+
+        SCDCMD_DAC_S setdacinfo;
+        SCDRPT_DAC_S setdacrprt;
+        SCDCMD_TMP_S gettempinfo;
+        SCDRPT_TMP_S gettemprprt;
+        SCDCMD_ADC_S getadcinfo;
+        SCDRPT_ADC_S getadcrprt;
+
+        char msgstream[64];
+    };
+	
+	cmd_lidar_L1() {
+		cmdId = eLidarReset ; // eMsgSoftCk ;
+	  isAsk = false ;
+	}
+} LIDAR_CMDRPT_U ;
+
+const char LIDAR_COMMAND_MARK[8] = "A7C7A5C";
+typedef struct comu_lidar_L1 {
+    const char command_mark[8] = "A7C7A5C";
+    LIDAR_CMDRPT_U commander;
+    unsigned int check_sum;
+} LIDAR_COMMAND_S ; //before: SCADA_CMDRPTS_S
+
+/// 后续再添加的内容必须使用后边定义的结构体，因为协议已经定义变长，但是实际上旧的结构仍不支持
+typedef struct LIDAR_CMDRPT_U1 {
+    union BODY{
+         /// 发现设备命令与返回
+        SCDCMD_FINDDEV_S findDevicereq;
+        SCDRPT_FINDDEV_S findDeviceresp;
+        /// IP模式配置设备命令与返回
+        SCDCMD_IPCFG_S IPCfgreq;
+        SCDRPT_IPCFG_S IPCfgresp;
+        /// 心跳包命令与返回
+        SCDCMD_HEARTBEAT_S heartBeatreq;
+        SCDRPT_HEARTBEAT_S heartBeatresp;
+        /// 配置文件控制命令
+        SCDCMD_CFGCTRL_S cfgctrlreq;
+        SCDRPT_CFGCTRL_S cfgctrlresp;
+
+        char msgstream[LIDAR_BODY_MAX];
+    }unibody;
+} LIDAR_CMDRPT_U1 ;
+/// OpenLiDAR协议命令头
+typedef struct OPEN_LIDAR_HEAD 
+{
+    char     strsyncword[8]; ///< 同步字，固定为"A7C7A5C"
+    uint16_t ui16cmdid;      ///< 命令id，具体可接受定义见枚举类型LiDAR_MSGID_E定义
+    uint16_t ui16lens;       ///< body段的实际长度，可以设置为0，如果设置为0的话则body段为64字节
+    uint32_t reqid;          ///< reqid命令包的请求ID，回复包的回复ID也使用此位并且回复包需要和命令包一致
+}OPEN_LIDAR_HEAD;
+/// OpenLiDAR协议命令尾
+typedef struct OPEN_LIDAR_TAIL
+{
+   uint32_t check_sum;      ///< 协议包除去同步字后整体的crc32校验值
+}OPEN_LIDAR_TAIL;
+
+/// 新的OpenLiDAR协议结构
+typedef struct LIDAR_COMMAND_S_1
+{
+    OPEN_LIDAR_HEAD sthead;    ///< 协议命令头
+    LIDAR_CMDRPT_U1 commander; ///< 请求包或应答包的具体数据
+    OPEN_LIDAR_TAIL sttril;    ///< 尾包，crc校验值
+} LIDAR_COMMAND_S_1;
+
+#pragma pack()
+#endif

src/driver/driver_lidar_bk_16z/common/ioapi.cpp

@@ -0,0 +1,156 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+//#include "ros/ros.h"
+#include <iostream>
+#include "ioapi.h"
+
+namespace rfans_driver
+{
+
+  static const int POLL_TIMEOUT = 1000; // one second (in msec)
+  static size_t packet_size_pcap = 1206;
+
+  ////////////////////////////////////////////////////////////////////////
+  // base class implementation
+  ////////////////////////////////////////////////////////////////////////
+  IOAPI::IOAPI()
+  {
+  }
+
+  IOAPI::~IOAPI()
+  {
+  }
+
+  /** @brief Write the Regidit. */
+  int IOAPI::HW_WRREG(int flag, int regAddress, unsigned int regData)
+  {
+    int rtn = 0;
+    lidarAPi::DEB_FRAME_S wdFrame_;
+    long tmp_len = sizeof(lidarAPi::DEB_FRAME_S);
+    // ROS_INFO("write the regidit length=%d",tmp_len);
+    wdFrame_ = packDEBV3Frame(lidarAPi::eCmdWrite, regAddress, regData);
+    write((unsigned char *)&wdFrame_, tmp_len);
+    return rtn;
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // InputSocket class implementation
+  ////////////////////////////////////////////////////////////////////////
+  /** @brief constructor
+   *
+   *  @param private_nh ROS handle for calling node.
+   *  @param port UDP port number
+   */
+  IOSocketAPI::IOSocketAPI(std::string ipstr, uint16_t devport, int16_t pcport)
+  {
+    m_sockfd = -1;
+    m_sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+    // ROS_INFO("m_devaddr6=%d",m_sockfd);
+    if (m_sockfd == -1)
+    {
+      perror(" create socket error");
+      return;
+    }
+
+    sockaddr_in my_addr;
+    memset(&my_addr, 0, sizeof(my_addr));
+    my_addr.sin_family = AF_INET;
+    my_addr.sin_port = htons(pcport);
+    my_addr.sin_addr.s_addr = INADDR_ANY;
+
+    memset(&m_devaddr, 0, sizeof(m_devaddr));
+    m_devaddr.sin_family = AF_INET;
+    m_devaddr.sin_port = htons(devport);
+    m_devaddr.sin_addr.s_addr = inet_addr(ipstr.c_str());
+
+    if (bind(m_sockfd, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
+    {
+      perror("bind message");
+      return;
+    }
+    int buff_size = 2000000;
+    setsockopt(m_sockfd, SOL_SOCKET, SO_RCVBUF, (const char *)(&buff_size), sizeof(int));
+    if (fcntl(m_sockfd, F_SETFL, O_NONBLOCK | FASYNC) < 0)
+    {
+      perror("non-block message");
+      return;
+    }
+
+    // ROS_INFO_STREAM("rfans socket fd is " << m_sockfd);
+    reset();
+  }
+
+  /** @brief destructor */
+  IOSocketAPI::~IOSocketAPI(void)
+  {
+    (void)close(m_sockfd);
+    // ROS_INFO("m_devaddr7=%d",m_sockfd);
+  }
+
+  /** @brief Write */
+  int IOSocketAPI::write(unsigned char *data, int size)
+  {
+    int rtn = 0;
+    unsigned char tmpCmd[UDP_FRAME_MIN];
+    unsigned char *tmpBuf = data;
+    socklen_t sender_address_len = sizeof(m_devaddr);
+    // ROS_INFO("m_devaddr1=%d",m_sockfd);
+    if (m_sockfd <= 0)
+      return rtn;
+
+    if (size < UDP_FRAME_MIN && size > 0)
+    {
+      memcpy(tmpCmd, data, size);
+      tmpBuf = tmpCmd;
+      size = UDP_FRAME_MIN;
+    }
+    // ROS_INFO("write size=%d",size);
+    rtn = sendto(m_sockfd, data, size, 0, (sockaddr *)&m_devaddr, sender_address_len);
+    if (rtn < 0)
+    {
+      // perror("IOSocketAPI:write") ;
+    }
+    return rtn;
+  }
+
+  int IOSocketAPI::reset()
+  {
+    //  m_bufferPro->reset();
+    return 0;
+  }
+
+  /** @brief read from RFans UDP Data  */
+  int IOSocketAPI::read(unsigned char *data, int size)
+  {
+    struct pollfd fds[1];
+    fds[0].fd = m_sockfd;
+    fds[0].events = POLLIN;
+    int nbytes = 0;
+    sockaddr_in sender_address;
+    socklen_t sender_address_len = sizeof(sender_address);
+
+    int retval = 0;
+    retval = poll(fds, 1, POLL_TIMEOUT);
+    if (fds[0].revents & POLLIN)
+    {
+      nbytes = recvfrom(m_sockfd, data, size, 0,
+                        (sockaddr *)&sender_address, &sender_address_len);
+      if (m_devaddr.sin_addr.s_addr != sender_address.sin_addr.s_addr)
+      {
+        nbytes = 0;
+      }
+    }
+    if (retval == 0)
+    {
+        std::cout<<"IOSocketAPI::read  poll() timeout"<<std::endl;
+//      ROS_WARN("IOSocketAPI::read  poll() timeout");
+      nbytes = 0;
+    }
+    return nbytes;
+  }
+
+} // end namespace

src/driver/driver_lidar_bk_16z/common/ioapi.h

@@ -0,0 +1,92 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#ifndef _RFANS_IOAPI_H_
+#define _RFANS_IOAPI_H_
+
+#include <unistd.h>
+#include <stdio.h>
+
+#include <netinet/in.h>
+//#include <starros/Packet.h>
+//#include <pcap.h>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+
+#include <string>
+#include <sstream>
+
+//#include <sensor_msgs/PointCloud2.h>
+
+#include "ssFrameLib.h"
+
+class SSBufferDec;
+namespace rfans_driver
+{
+  /** @brief Rfans IOAPI base class */
+  static uint16_t DATA_PORT_NUMBER = 2014;
+  class IOAPI
+  {
+  public:
+    IOAPI();
+    ~IOAPI();
+
+    /** @brief Read one Rfans packet.
+     *
+     * @param packet from Rfans
+     *
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          > 0 if incomplete packet (is this possible?)
+     */
+    /** @brief Write . */
+    virtual int write(unsigned char *data, int size) = 0;
+
+    /** @brief read . */
+    virtual int read(unsigned char *data, int size) = 0;
+
+    /** @brief reset . */
+    virtual int reset() = 0;
+
+    virtual int HW_WRREG(int flag, int regAddress, unsigned int regData);
+
+  protected:
+    SSBufferDec *m_bufferPro;
+  };
+
+  ////////////////////////////////////////////////////////////////////////
+  // InputSocket class implementation
+  ////////////////////////////////////////////////////////////////////////
+  /** @brief Rfans IOAPI from socket. */
+  class IOSocketAPI : public IOAPI
+  {
+  public:
+    IOSocketAPI(std::string ipstr = DEVICE_IP_STRING,
+                uint16_t devport = DEVICE_PORT_NUMBER,
+                int16_t pcport = PC_PORT_NUMBER);
+    virtual ~IOSocketAPI();
+    virtual int write(unsigned char *data, int size);
+    virtual int read(unsigned char *data, int size);
+    virtual int reset();
+
+  private:
+    int m_sockfd;
+    in_addr devip_;
+    sockaddr_in m_devaddr;
+
+//    ros::NodeHandle private_nh_;
+    uint16_t m_devport_;
+    uint16_t m_pcport_;
+    std::string devip_str_;
+  };
+
+}
+#endif

src/driver/driver_lidar_bk_16z/common/ssFrameLib.cpp

@@ -0,0 +1,50 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#include "ssFrameLib.h"
+
+int checkSum(unsigned char *_dataBuf, int count_)
+{
+  int rtn = 0;
+  for (int i = 0; i < count_; i++)
+  {
+    rtn += _dataBuf[i];
+  }
+  rtn = rtn & 0xFF;
+  return rtn;
+}
+
+lidarAPi::DEB_FRAME_S packDEBV3Frame(lidarAPi::SCD_FRAME_TYPE_E flag, int regAddress, int regData)
+{
+  lidarAPi::DEB_FRAME_S tmpFrame_;
+  memset(&tmpFrame_, 0, sizeof(lidarAPi::DEB_FRAME_S));
+
+  switch (flag)
+  {
+  case lidarAPi::eCmdWrite:
+    tmpFrame_.msgHead = DEB_FRAME_WRITE;
+    break;
+  case lidarAPi::eCmdRead:
+    tmpFrame_.msgHead = DEB_FRAME_READ;
+    break;
+  case lidarAPi::eCmdQuery:
+    tmpFrame_.msgHead = DEB_FRAME_READ;
+    break;
+  default:
+    tmpFrame_.msgHead = DEB_FRAME_WRITE;
+    break;
+  }
+  unsigned short tmpSData = 0;
+  tmpSData = regAddress;
+
+  tmpFrame_.regAddress = tmpSData;
+
+  tmpFrame_.regData = regData;
+
+  unsigned char *tmpBase = (unsigned char *)&tmpFrame_;
+  tmpFrame_.msgCheckSum = checkSum(tmpBase + 2, sizeof(lidarAPi::DEB_FRAME_S) - 2);
+  return tmpFrame_;
+}

src/driver/driver_lidar_bk_16z/common/ssFrameLib.h

@@ -0,0 +1,170 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#ifndef _FRAME_LIB_H_
+#define _FRAME_LIB_H_
+
+#include <iostream>
+#include <string>
+#include <string.h>
+
+using namespace std;
+
+const static int REG_DEVICE_CTRL = (0x2040);
+const static int REG_DEVICE_CTRL_OLD = (0x0040);
+const static int REG_DATA_LEVEL = 0x2008;
+const static int REG_DATA_LEVEL_OLD = 0x0008;
+
+const static int CMD_SCAN_SPEED_5HZ = 0x20;
+const static int CMD_SCAN_SPEED_10HZ = 0x60; // 转速10hz
+const static int CMD_SCAN_SPEED_20HZ = 0xF0;
+
+const static int CMD_SCAN_SPEED_10HZ_C = 0x00;
+const static int CMD_SCAN_SPEED_20HZ_C = 0x20;
+const static int CMD_SCAN_SPEED_40HZ_C = 0x60;
+const static int CMD_SCAN_SPEED_60HZ_C = 0xA0;
+const static int CMD_SCAN_SPEED_80HZ_C = 0xE0;
+
+const static int CFANS_ECHO = 0x36; // cfans 0x36
+const static int CFANS_DUAL = 0x39; // cfans 0x36
+
+const static int CMD_SCAN_DISABLE = 0x00;
+const static int CMD_SCAN_ENABLE = 0x3;
+const static int CMD_LASER_ENABLE = 0x2;
+const static int CMD_CALC_DATA = 0x2;
+const static int CMD_DEBUG_DATA = 0x5;
+const static int CMD_RCV_CLOSE = 0x0;
+const static int CMD_LEVEL0_ECHO = 0x00000000;
+const static int CMD_LEVLE0_DUAL_ECHO = 0x00000002;
+const static int CMD_LEVEL1_ECHO = 0x01000200;
+const static int CMD_LEVEL1_DUAL_ECHO = 0x01000202;
+const static int CMD_LEVEL2_ECHO = 0x0201BF00;
+const static int CMD_LEVEL2_DUAL_ECHO = 0x0201BF02;
+const static int CMD_LEVEL3_ECHO = 0x03000100;
+const static int CMD_LEVEL3_DUAL_ECHO = 0x03000102;
+
+const static int ANGLE_SPEED_0HZ = 0;
+const static int ANGLE_SPEED_5HZ = 5;
+const static int ANGLE_SPEED_10HZ = 10;
+const static int ANGLE_SPEED_20HZ = 20;
+
+const static int ANGLE_SPEED_0HZ_cfan = 0;
+const static int ANGLE_SPEED_10HZ_cfan = 10;
+const static int ANGLE_SPEED_20HZ_cfans = 20;
+const static int ANGLE_SPEED_40HZ_cfans = 40;
+const static int ANGLE_SPEED_60HZ_cfans = 60;
+const static int ANGLE_SPEED_80HZ_cfans = 80;
+
+static unsigned short DEVICE_PORT_NUMBER = 2015;
+static unsigned short PC_PORT_NUMBER = 2015;
+static std::string DEVICE_IP_STRING = "192.168.0.3";
+
+static unsigned short UDP_FRAME_MIN = (18);
+
+#define DEB_FRAME_WRITE (0xA5) // write head sync
+#define DEB_FRAME_READ (0x5a)  // read head sync
+#define DEB_FRAME_ERROR (0xE7) // err  haad sync
+
+#define FRAME_MSG_LENGTH (1024)
+
+const int UDPREG_MAX_COUNT = 256;
+const int ROMREG_MAX_COUNT = 0x7FF;
+
+#pragma pack(1)
+typedef struct // 256 Byte
+{
+  unsigned int pkgflag;
+  unsigned int pkgnumber;
+  unsigned char gps_time[6];
+  unsigned int maca;
+  unsigned short macb;
+  unsigned short dataport;
+  unsigned short ctrlport;
+  unsigned char motorspd;
+  unsigned int deviceType;
+  unsigned short phaseAngle;
+  unsigned char pack_format;
+  unsigned char device_id;
+  short temperature;
+  unsigned int err_chksum;
+  unsigned int point_freq;
+  unsigned int dev_status;
+  unsigned int low_sn;
+  unsigned int high_sn;
+  unsigned long long fpga_5m_cnt;
+  unsigned char time_server;
+  unsigned char pointcloud_packet_protocol;
+  unsigned long long unix_time_stamp;
+  unsigned char padding[183];
+} HEARTBEAT_S;
+#pragma pack()
+
+namespace lidarAPi
+{
+  typedef enum
+  {
+    eCmdWrite, // write command
+    eCmdRead,  // read command
+    eCmdQuery, // Query command
+  } SCD_FRAME_TYPE_E;
+
+#pragma pack(1)
+  typedef struct
+  {                            //! rfans udp command package
+    unsigned char msgHead;     //!< head sync
+    unsigned char msgCheckSum; //!< check sum
+    unsigned short regAddress; //!< register add
+    unsigned int regData;      //!< register data
+  } DEB_FRAME_S;
+
+  typedef enum
+  {
+    eDevCmdIdle = 0,
+    eDevCmdWork, // 1
+    eDevCmdSimu,
+    eDevCmdBreak,
+    eDevCmdReset,
+    eDevCmdAsk,
+  } DEB_CMD_E;
+
+  typedef enum
+  {
+    eFormatCalcData = 0x2,
+    eFormatDebugData = 0x5,
+  } DEB_DFORMAT_E;
+
+  static int DEVICE_MOTOR_HZ = 5;
+  typedef struct
+  {
+    DEB_CMD_E cmdstat;
+    DEB_DFORMAT_E dataFormat;
+    int scnSpeed;
+    int dataLevel;
+    int lsrFreq;
+    float rangeMin, rangeMax;
+  } DEB_PROGRM_S;
+
+  typedef enum
+  {
+    LEVEL0_ECHO = 0,
+    LEVEL0_DUAL_ECHO,
+    LEVEL1_ECHO,
+    LEVEL1_DUAL_ECHO,
+    LEVEL2_ECHO,
+    LEVEL2_DUAL_ECHO,
+    LEVEL3_ECHO,
+    LEVEL3_DUAL_ECHO,
+
+  } MULTI_LEVEL_DATA;
+
+#pragma pack()
+}
+
+int checkSum(unsigned char *_dataBuf, int count_);
+
+lidarAPi::DEB_FRAME_S packDEBV3Frame(lidarAPi::SCD_FRAME_TYPE_E flag, int regAddress_, int regData_);
+
+#endif

src/driver/driver_lidar_bk_16z/common/utility.h

@@ -0,0 +1,161 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#ifndef _UTILITY_H_
+#define _UTILITY_H_
+
+#include <string>
+#include <string.h>
+#include <fstream>
+#include <sstream>
+#include <iostream>
+
+#include <vector>
+#include <queue>
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/file.h>
+
+#include <condition_variable>
+#include <thread>
+
+#include <cmath>
+#include <stdlib.h>
+#include <cstdlib>
+#include <sys/time.h>
+
+#include <Eigen/Eigen>
+
+using namespace std;
+
+struct InputPara_S
+{
+  string device_ip;
+  string device_name;
+  string frame_id;
+  string simu_filepath;
+  string export_path;
+  string display_mode;
+  string filter_path;
+  string isf_path;
+  int dataport;
+  int ctrlport;
+  int heart_port;
+  int scnSpeed;
+  bool dual_echo;
+  string cfg_path;
+  bool read_once;
+  int device_start;
+  bool read_fast;
+  bool save_xyz;
+  bool save_isf;
+  int data_format;
+  int leveltype;
+  int decrangetype;
+  string levelvec;
+  string chn_corr_table_path;
+  string window_pulsewidth_table_path;
+  bool isFaceToPS;
+  int lsrFreq;
+  bool is_trans;
+  bool is_ntp;
+
+  float anlign_angle_x, anlign_angle_y, anlign_angle_z;
+  int road_type;
+  float groud_distance;
+
+  bool use_algorithm;
+  int algorithmType;
+
+  bool Level0C;
+  bool Level0F;
+  bool Level0G;
+
+  bool Level1A;
+  bool Level1B;
+  bool Level1F;
+  bool Level1G;
+  bool Level1L;
+  bool Level1U;
+  bool Level1Q;
+
+  bool Level2A;
+  bool Level2D;
+
+  bool LevelAB;
+  bool LevelAC;
+  bool LevelAG;
+  bool LevelAH;
+  bool LevelAI;
+  bool LevelAJ;
+  bool LevelAM;
+
+  InputPara_S()
+  {
+  device_ip = "";
+  device_name = "";
+  frame_id = "";
+  simu_filepath = "";
+  export_path = "";
+  display_mode = "";
+  filter_path = "";
+  isf_path = "";
+  dataport = 0;
+  ctrlport = 0;
+  heart_port = 0;
+  scnSpeed = 0;
+  dual_echo = false;
+  cfg_path = "";
+  read_once = true;
+  device_start = 0;
+  read_fast = false;
+  save_xyz = false;
+  save_isf = false;
+  data_format = 0;
+  leveltype = 0;
+  decrangetype = 0;
+  levelvec = "";
+  chn_corr_table_path = "";
+  window_pulsewidth_table_path = "";
+  isFaceToPS = false;
+  lsrFreq = 0;
+  is_trans = false;
+  is_ntp = false;
+
+  anlign_angle_x = 0., anlign_angle_y = 0., anlign_angle_z = 0.;
+  road_type = 0;
+  groud_distance = 0.;
+
+  use_algorithm = false;
+  algorithmType = 0;
+
+  Level0C = false;
+  Level0F = false;
+  Level0G = false;
+
+  Level1A = false;
+  Level1B = false;
+  Level1F = false;
+  Level1G = false;
+  Level1L = false;
+  Level1U = false;
+  Level1Q = false;
+
+  Level2A = false;
+  Level2D = false;
+
+  LevelAB = false;
+  LevelAC = false;
+  LevelAG = false;
+  LevelAH = false;
+  LevelAI = false;
+  LevelAJ = false;
+  LevelAM = false;
+  }
+};
+
+#endif

src/driver/driver_lidar_bk_16z/driver_lidar_bk_16z.pro

@@ -0,0 +1,44 @@
+QT = core
+
+CONFIG += c++17 cmdline
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        common/ioapi.cpp \
+        common/ssFrameLib.cpp \
+        lidar_16z.cpp \
+        lidar_bkdriver.cpp \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    ICD_LiDAR_API.h \
+    common/ioapi.h \
+    common/ssFrameLib.h \
+    common/utility.h \
+    lidar_16z.h \
+    lidar_bkdriver.h
+
+
+INCLUDEPATH += $$PWD/common
+
+INCLUDEPATH += /usr/include/eigen3
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+!include(../../../include/ivyaml-cpp.pri ) {
+    error( "Couldn't find the ivyaml-cpp.pri file!" )
+}

src/driver/driver_lidar_bk_16z/lidar_16z.cpp

@@ -0,0 +1,128 @@
+#include "lidar_16z.h"
+
+
+static float V_theta[16] = {-15,-13,-11,-9,-7,-5,-3,-1,1,3,5,7,9,11,13,15};
+static float H_Beta[16] = {-2.5,-2.5 ,-2.5 ,-2.5,-2.5,-2.5 ,-2.5 ,-2.5,
+                    2.5,-2.5 ,-2.5 ,-2.5, -2.5,-2.5 ,-2.5 ,-2.5};
+
+lidar_16z::lidar_16z() {
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+        new pcl::PointCloud<pcl::PointXYZI>());
+
+    point_cloud->header.frame_id = "velodyne";
+    point_cloud->height = 1;
+    point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+    point_cloud->width = 0;
+    point_cloud->header.seq =0;
+    mpoint_cloud_temp = point_cloud;
+
+    mpdriver = new lidar_bkdriver();
+    mpTheadDecode = new  std::thread(&lidar_16z::ThreadDecode,this);
+}
+
+lidar_16z::~lidar_16z(){
+    mbRun = false;
+    mpTheadDecode->join();
+    delete mpdriver;
+}
+
+void lidar_16z::ThreadDecode()
+{
+    static double azutotal = 0.0;
+    static int g_seq = 0;
+    while(mbRun)
+    {
+        std::shared_ptr<unsigned char> pdata_ptr;
+        int64_t nrecvtime;
+        int ndatasize = 0;
+        int nread = mpdriver->GetRecvData(pdata_ptr,nrecvtime,ndatasize);
+        if(nread == 1)
+        {
+            std::cout<<"read "<<ndatasize<<std::endl;
+            if(ndatasize % 1184 != 0)
+            {
+                std::cout<<" data not complete. datasize: "<<ndatasize<<std::endl;
+            }
+            else
+            {
+                unsigned char * p = pdata_ptr.get();
+                unsigned short * pang_start = (unsigned short * )(p+12);
+                unsigned short * pang_end = (unsigned short *)(p+14);
+                (void)pang_end;
+                double ang_space = 0.2;
+                int i;
+                int nheadlen = 32;
+                double fAzuAng = static_cast<double>(*pang_start) * 0.01;
+                for(i=0;i<12;i++)
+                {
+                    int j;
+                    for(j=0;j<16;j++)
+                    {
+                        unsigned char * pecho = (unsigned char *)(p +nheadlen +  i*96 + j*6);
+                        unsigned short * prange;
+                        unsigned char * pchch;
+                        double frange;
+                        unsigned char  nch;
+                        unsigned char intensity;
+                        pchch = pecho;
+                        prange = (unsigned short *)(pecho +3);
+                        intensity = *(pecho + 5);
+                        nch = * pchch;
+                        frange = static_cast<double>(*prange);
+                        frange = frange * 0.004;
+                        if(nch<=16){
+                            double fAng = (-fAzuAng - H_Beta[nch] - mfRollAng) * M_PI/180.0;
+                            double vtheta = V_theta[nch] * M_PI/180.0;
+                            double x = frange * cos(fAng)* cos(vtheta);
+                            double y = frange * sin(fAng)* cos(vtheta);
+                            double z = frange * sin(vtheta);
+
+                            pcl::PointXYZI point;
+                            point.x = x;
+                            point.y = y;
+                            point.z = z;
+                            point.intensity = intensity;
+
+                            mpoint_cloud_temp->points.push_back(point);
+                            ++mpoint_cloud_temp->width;
+                        }
+                        else
+                        {
+                            std::cout<<" channel error."<<std::endl;
+                        }
+                    }
+                    fAzuAng = fAzuAng + ang_space;
+
+                    azutotal = azutotal + ang_space;
+                    if(azutotal>=360.0)
+                    {
+                        //share pointcloud
+                        mmutexpc.lock();
+                        mpoint_cloud = mpoint_cloud_temp;
+                        mbpcupdate = true;
+                        mmutexpc.unlock();
+                        mcv.notify_all();
+                        pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                            new pcl::PointCloud<pcl::PointXYZI>());
+
+                        point_cloud->header.frame_id = "velodyne";
+                        point_cloud->height = 1;
+                        point_cloud->header.stamp = static_cast<uint64_t>(std::chrono::system_clock::now().time_since_epoch().count()/1000);
+                        point_cloud->width = 0;
+                        point_cloud->header.seq =g_seq;
+                        g_seq++;
+                        mpoint_cloud_temp = point_cloud;
+                        azutotal = 0;
+                    }
+                }
+            }
+        }
+    }
+}
+
+int lidar_16z::GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud)
+{
+    if(mbpcupdate == false)return 0;
+    return 1;
+}

src/driver/driver_lidar_bk_16z/lidar_16z.h

@@ -0,0 +1,42 @@
+#ifndef LIDAR_16Z_H
+#define LIDAR_16Z_H
+
+#include <mutex>
+#include <thread>
+#include <condition_variable>
+
+#include "lidar_bkdriver.h"
+
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+class lidar_16z
+{
+public:
+    lidar_16z();
+    ~lidar_16z();
+
+    int GetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr & point_cloud);
+
+private:
+    lidar_bkdriver * mpdriver;
+
+    void ThreadDecode();
+
+private:
+    bool mbRun = true;
+    std::thread * mpTheadDecode;
+
+    double mfRollAng = 0.0;
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud;
+    pcl::PointCloud<pcl::PointXYZI>::Ptr mpoint_cloud_temp;
+
+    std::mutex mmutexpc;
+    std::mutex mmutexcv;
+    std::condition_variable mcv;
+    bool mbpcupdate = false;
+};
+
+#endif // LIDAR_16Z_H

src/driver/driver_lidar_bk_16z/lidar_bkdriver.cpp

@@ -0,0 +1,419 @@
+#include "lidar_bkdriver.h"
+
+static uint32_t s_CrcTab32[] = {
+    0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
+    0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD,
+    0x4C11DB70, 0x48D0C6C7, 0x4593E01E, 0x4152FDA9, 0x5F15ADAC, 0x5BD4B01B, 0x569796C2, 0x52568B75,
+    0x6A1936C8, 0x6ED82B7F, 0x639B0DA6, 0x675A1011, 0x791D4014, 0x7DDC5DA3, 0x709F7B7A, 0x745E66CD,
+    0x9823B6E0, 0x9CE2AB57, 0x91A18D8E, 0x95609039, 0x8B27C03C, 0x8FE6DD8B, 0x82A5FB52, 0x8664E6E5,
+    0xBE2B5B58, 0xBAEA46EF, 0xB7A96036, 0xB3687D81, 0xAD2F2D84, 0xA9EE3033, 0xA4AD16EA, 0xA06C0B5D,
+    0xD4326D90, 0xD0F37027, 0xDDB056FE, 0xD9714B49, 0xC7361B4C, 0xC3F706FB, 0xCEB42022, 0xCA753D95,
+    0xF23A8028, 0xF6FB9D9F, 0xFBB8BB46, 0xFF79A6F1, 0xE13EF6F4, 0xE5FFEB43, 0xE8BCCD9A, 0xEC7DD02D,
+    0x34867077, 0x30476DC0, 0x3D044B19, 0x39C556AE, 0x278206AB, 0x23431B1C, 0x2E003DC5, 0x2AC12072,
+    0x128E9DCF, 0x164F8078, 0x1B0CA6A1, 0x1FCDBB16, 0x018AEB13, 0x054BF6A4, 0x0808D07D, 0x0CC9CDCA,
+    0x7897AB07, 0x7C56B6B0, 0x71159069, 0x75D48DDE, 0x6B93DDDB, 0x6F52C06C, 0x6211E6B5, 0x66D0FB02,
+    0x5E9F46BF, 0x5A5E5B08, 0x571D7DD1, 0x53DC6066, 0x4D9B3063, 0x495A2DD4, 0x44190B0D, 0x40D816BA,
+    0xACA5C697, 0xA864DB20, 0xA527FDF9, 0xA1E6E04E, 0xBFA1B04B, 0xBB60ADFC, 0xB6238B25, 0xB2E29692,
+    0x8AAD2B2F, 0x8E6C3698, 0x832F1041, 0x87EE0DF6, 0x99A95DF3, 0x9D684044, 0x902B669D, 0x94EA7B2A,
+    0xE0B41DE7, 0xE4750050, 0xE9362689, 0xEDF73B3E, 0xF3B06B3B, 0xF771768C, 0xFA325055, 0xFEF34DE2,
+    0xC6BCF05F, 0xC27DEDE8, 0xCF3ECB31, 0xCBFFD686, 0xD5B88683, 0xD1799B34, 0xDC3ABDED, 0xD8FBA05A,
+    0x690CE0EE, 0x6DCDFD59, 0x608EDB80, 0x644FC637, 0x7A089632, 0x7EC98B85, 0x738AAD5C, 0x774BB0EB,
+    0x4F040D56, 0x4BC510E1, 0x46863638, 0x42472B8F, 0x5C007B8A, 0x58C1663D, 0x558240E4, 0x51435D53,
+    0x251D3B9E, 0x21DC2629, 0x2C9F00F0, 0x285E1D47, 0x36194D42, 0x32D850F5, 0x3F9B762C, 0x3B5A6B9B,
+    0x0315D626, 0x07D4CB91, 0x0A97ED48, 0x0E56F0FF, 0x1011A0FA, 0x14D0BD4D, 0x19939B94, 0x1D528623,
+    0xF12F560E, 0xF5EE4BB9, 0xF8AD6D60, 0xFC6C70D7, 0xE22B20D2, 0xE6EA3D65, 0xEBA91BBC, 0xEF68060B,
+    0xD727BBB6, 0xD3E6A601, 0xDEA580D8, 0xDA649D6F, 0xC423CD6A, 0xC0E2D0DD, 0xCDA1F604, 0xC960EBB3,
+    0xBD3E8D7E, 0xB9FF90C9, 0xB4BCB610, 0xB07DABA7, 0xAE3AFBA2, 0xAAFBE615, 0xA7B8C0CC, 0xA379DD7B,
+    0x9B3660C6, 0x9FF77D71, 0x92B45BA8, 0x9675461F, 0x8832161A, 0x8CF30BAD, 0x81B02D74, 0x857130C3,
+    0x5D8A9099, 0x594B8D2E, 0x5408ABF7, 0x50C9B640, 0x4E8EE645, 0x4A4FFBF2, 0x470CDD2B, 0x43CDC09C,
+    0x7B827D21, 0x7F436096, 0x7200464F, 0x76C15BF8, 0x68860BFD, 0x6C47164A, 0x61043093, 0x65C52D24,
+    0x119B4BE9, 0x155A565E, 0x18197087, 0x1CD86D30, 0x029F3D35, 0x065E2082, 0x0B1D065B, 0x0FDC1BEC,
+    0x3793A651, 0x3352BBE6, 0x3E119D3F, 0x3AD08088, 0x2497D08D, 0x2056CD3A, 0x2D15EBE3, 0x29D4F654,
+    0xC5A92679, 0xC1683BCE, 0xCC2B1D17, 0xC8EA00A0, 0xD6AD50A5, 0xD26C4D12, 0xDF2F6BCB, 0xDBEE767C,
+    0xE3A1CBC1, 0xE760D676, 0xEA23F0AF, 0xEEE2ED18, 0xF0A5BD1D, 0xF464A0AA, 0xF9278673, 0xFDE69BC4,
+    0x89B8FD09, 0x8D79E0BE, 0x803AC667, 0x84FBDBD0, 0x9ABC8BD5, 0x9E7D9662, 0x933EB0BB, 0x97FFAD0C,
+    0xAFB010B1, 0xAB710D06, 0xA6322BDF, 0xA2F33668, 0xBCB4666D, 0xB8757BDA, 0xB5365D03, 0xB1F740B4};
+#define IS_CRC_START_32 0xFFFFFFFF
+#define PACKAGELEN 60000
+
+lidar_bkdriver::lidar_bkdriver() {
+    setparam();
+    socketInit();
+    configDeviceParams();
+
+    mpThreadRaw = new std::thread(&lidar_bkdriver::RecvRawThread,this);
+    mpThreadHeartbeat = new std::thread(&lidar_bkdriver::HeartBeatThread,this);
+}
+
+lidar_bkdriver::~lidar_bkdriver()
+{
+    mbRun = false;
+    mpThreadHeartbeat->join();
+    mpThreadRaw->join();
+}
+
+void lidar_bkdriver::socketInit()
+{
+    m_ctl_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.ctrlport, m_input_para.ctrlport);
+    m_data_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.dataport, m_input_para.dataport);
+    m_heart_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.heart_port, m_input_para.heart_port);
+}
+
+void lidar_bkdriver::setparam()
+{
+    m_input_para.device_name ="R-Fans-16";
+    m_input_para.display_mode = "overlay";
+
+    m_input_para.device_ip = "192.168.0.10";
+    m_input_para.dataport = 2014;
+    m_input_para.ctrlport = 2013;
+    m_input_para.heart_port = 2030;
+
+    m_input_para.frame_id =  "world";
+
+    m_input_para.save_xyz = false;
+    m_input_para.scnSpeed = 10;
+    m_input_para.simu_filepath = "";
+    m_input_para.cfg_path = "";
+    m_input_para.dual_echo = false;
+    m_input_para.export_path = "";
+    m_input_para.read_once = false;
+    m_input_para.save_isf = false;
+    m_input_para.isf_path = "";
+    m_input_para.data_format = 0;
+    m_input_para.decrangetype = 1;
+    m_input_para.leveltype = 0;
+    m_input_para.levelvec = "1,0,0,0,0,0,0,0";
+    m_input_para.chn_corr_table_path = "";
+    m_input_para.window_pulsewidth_table_path = "";
+    m_input_para.isFaceToPS = false;
+    m_input_para.lsrFreq = 1000;
+    m_input_para.is_trans = false;
+    m_input_para.is_ntp = false;
+
+    m_input_para.anlign_angle_x = 0;
+    m_input_para.anlign_angle_y = 0;
+    m_input_para.anlign_angle_z = 0;
+    m_input_para.groud_distance = -1.9;
+    m_input_para.road_type = 0 ;
+
+    m_input_para.use_algorithm = false;
+    m_input_para.algorithmType = 0;
+
+    m_input_para.Level0C = false;
+    m_input_para.Level0G = false;
+    m_input_para.Level0F = false;
+
+    m_input_para.Level1A = false;
+    m_input_para.Level1B = false;
+    m_input_para.Level1F = false;
+    m_input_para.Level1G = false;
+    m_input_para.Level1L = false;
+    m_input_para.Level1U = false;
+    m_input_para.Level1Q = false;
+
+    m_input_para.Level2A = false;
+    m_input_para.Level2D = false;
+
+    m_input_para.LevelAB = false;
+    m_input_para.LevelAC = false;
+    m_input_para.LevelAG = false;
+    m_input_para.LevelAH = false;
+    m_input_para.LevelAI = false;
+    m_input_para.LevelAJ = false;
+    m_input_para.LevelAM = false;
+}
+
+void lidar_bkdriver::configDeviceParams()
+{
+
+    lidarAPi::DEB_PROGRM_S params;
+    params.cmdstat = lidarAPi::eDevCmdWork;
+    params.lsrFreq = m_input_para.lsrFreq;
+    params.scnSpeed = m_input_para.scnSpeed;
+    setState(params);
+
+    return ;
+
+
+    bool dual_echo = m_input_para.dual_echo;
+
+    params.cmdstat = lidarAPi::eDevCmdWork;
+    params.dataFormat = lidarAPi::eFormatCalcData;
+
+    // set start rps
+    params.scnSpeed = m_input_para.scnSpeed;
+    progSet(params);
+
+    unsigned int regData = 0;
+    regData = 0;
+    if (dual_echo)
+    {
+        regData = CFANS_DUAL;
+    }
+    else
+    {
+        regData = CFANS_ECHO;
+    }
+
+    m_ctl_socket->HW_WRREG(0, REG_DATA_LEVEL, regData);
+    m_ctl_socket->HW_WRREG(0, REG_DATA_LEVEL_OLD, regData);
+}
+
+
+int lidar_bkdriver::progSet(lidarAPi::DEB_PROGRM_S &program)
+{
+    unsigned int tmpData = 0;
+    if ((m_input_para.device_name == "R-Fans-32") || (m_input_para.device_name == "R-Fans-16"))
+    {
+        switch (program.scnSpeed)
+        {
+        case ANGLE_SPEED_0HZ:
+            tmpData |= CMD_SCAN_DISABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ;
+            break;
+        case ANGLE_SPEED_10HZ:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ;
+            break;
+        case ANGLE_SPEED_20HZ:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_20HZ;
+            break;
+        case ANGLE_SPEED_5HZ:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_5HZ;
+            break;
+        default:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ;
+            break;
+        }
+    }
+    else if ((m_input_para.device_name == "C-Fans-128") || (m_input_para.device_name == "C-Fans-32") || (m_input_para.device_name == "C-Fans-256"))
+    {
+        switch (program.scnSpeed)
+        {
+        case ANGLE_SPEED_0HZ_cfan:
+            tmpData |= CMD_SCAN_DISABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ_C;
+            break;
+        case ANGLE_SPEED_10HZ_cfan:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ_C;
+            break;
+        case ANGLE_SPEED_20HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_20HZ_C;
+            break;
+        case ANGLE_SPEED_40HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_40HZ_C;
+            break;
+        case ANGLE_SPEED_60HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_60HZ_C;
+            break;
+        case ANGLE_SPEED_80HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_80HZ_C;
+            break;
+        default:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_40HZ_C;
+            break;
+        }
+    }
+
+    switch (program.cmdstat)
+    {
+    case lidarAPi::eDevCmdWork: // 默认为工作状态，此值在QT中设为常量
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL_OLD, tmpData);
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL, tmpData);
+        break;
+    case lidarAPi::eDevCmdIdle: // 待机状态
+        tmpData = CMD_RCV_CLOSE;
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL_OLD, tmpData);
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL, tmpData);
+        break;
+    case lidarAPi::eDevCmdAsk:
+        break;
+    default:
+        break;
+    }
+
+    return 0;
+}
+
+uint32_t isCrc32(uint8_t const *pIn, uint16_t len)
+{
+    if (pIn == NULL)
+    {
+        printf("%s:%d pIn is NULL\n", __FILE__, __LINE__);
+        return 0;
+    }
+
+    uint32_t crc;
+    uint32_t tmp;
+    uint32_t longC;
+    uint8_t const *ptr;
+    uint16_t a;
+
+    crc = IS_CRC_START_32;
+    ptr = pIn;
+
+    if (ptr != NULL)
+        for (a = 0; a < len; a++)
+        {
+
+            longC = 0x000000FFL & (uint32_t)*ptr;
+            tmp = crc ^ longC;
+            crc = (crc >> 8) ^ s_CrcTab32[tmp & 0xff];
+
+            ptr++;
+        }
+
+    crc ^= 0xffffffffL;
+
+    return crc & 0xffffffffL;
+}
+
+int lidar_bkdriver::lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfreq, int mt_lsrpwr)
+{
+    LIDAR_COMMAND_S cmd;
+    memset((void *)&cmd.commander, 0, sizeof(LIDAR_CMDRPT_U));
+    cmd.commander.cmdId = eMsgProgam;
+    cmd.commander.isAsk = 0;
+    cmd.commander.prgCtrl.cmdID = me_cmd;
+    cmd.commander.prgCtrl.echo_etype = 8;
+    cmd.commander.prgCtrl.laserFreqKHz = mt_lsrfreq;
+    cmd.commander.prgCtrl.laserFirePwr = mt_lsrpwr;
+    cmd.commander.prgCtrl.scanAngleSpeed = mt_scnspd;
+    cmd.check_sum = isCrc32((uint8_t *)&cmd.commander, sizeof(LIDAR_CMDRPT_U));
+    return m_ctl_socket->write((unsigned char *)&cmd, sizeof(LIDAR_COMMAND_S));
+}
+
+int lidar_bkdriver::setState(lidarAPi::DEB_PROGRM_S &program)
+{
+    switch (program.cmdstat)
+    {
+    case lidarAPi::eDevCmdWork:
+        lidar_run(SCDEV_CMD_E::eDevCmdWork, program.scnSpeed, program.lsrFreq);
+        break;
+    case lidarAPi::eDevCmdIdle:
+        lidar_run(SCDEV_CMD_E::eDevCmdIdle, program.scnSpeed, program.lsrFreq);
+        break;
+    case lidarAPi::eDevCmdAsk:
+        break;
+    default:
+        break;
+    }
+    return 0;
+}
+
+
+int lidar_bkdriver::RecvRawThread()
+{
+    const int MAXPACLEN = 60000;
+    while(mbRun)
+    {
+        std::shared_ptr<unsigned char> pread = std::shared_ptr<unsigned char>(new unsigned  char[MAXPACLEN]);
+        int ret = m_data_socket->read(pread.get(), MAXPACLEN);
+        // cout << ret << " " << ros::Time::now() << endl;
+        if (ret <= 0)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" configure. "<<std::endl;
+            usleep(500);
+            configDeviceParams();
+            continue;
+        }
+
+        mmutexrawdata.lock();
+        if(mvecterraw.size()>100)
+        {
+            std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" raw vector full. skip one."<<std::endl;
+        }
+        else
+        {
+            iv::lidarraw xraw;
+            xraw.mnrecvtime = std::chrono::system_clock::now().time_since_epoch().count();
+            xraw.mpraw_ptr = pread;
+            xraw.ndatasize = ret;
+            mvecterraw.push_back(xraw);
+        }
+        mmutexrawdata.unlock();
+        mcv.notify_all();
+
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" recv "<<ret<<std::endl;
+        std::cout<<" mod: "<<ret%1248<<std::endl;
+
+    }
+
+    return 1;
+}
+
+int lidar_bkdriver::HeartBeatThread()
+{
+    float temper;
+    while(mbRun)
+    {
+        if (m_heart_socket->read((unsigned char *)&m_heart, sizeof(m_heart)) == 256)
+        {
+            temper = m_heart.temperature / 100.0;
+            (void)temper;
+            // if (m_input_para.save_isf && !m_input_para.isf_path.empty())
+            // {
+            //     m_heartbeat_vec.push_back(m_heart);
+            // }
+        }
+
+        static int num = 0;
+        if (0 == num++)
+        {
+            std::cout<<"temperature = "<<temper<<"motoer speed: "<<m_heart.motorspd<<std::endl;
+            std::cout<<"DEVICE_id = "<<m_heart.device_id<<std::endl;
+            // ROS_WARN("temperature=%f", temper);
+            // ROS_INFO("DEVICE_id=%d", m_heart.device_id);
+        }
+        num %= 10;
+
+        std::this_thread::sleep_for(chrono::seconds(1));
+    }
+
+    return 1;
+
+}
+
+int lidar_bkdriver::GetRecvData(std::shared_ptr<unsigned char> &pdata_ptr, int64_t &nRecvTime, int & ndatasize)
+{
+    int nread = 0;
+    nread = ExecGetRecvData(pdata_ptr,nRecvTime,ndatasize);
+    if(nread > 0)return nread;
+
+    std::unique_lock<std::mutex> lk(mmutexcv);
+    if(mcv.wait_for(lk,std::chrono::milliseconds(100)) == std::cv_status::timeout){
+        lk.unlock();
+    }
+    else {
+        lk.unlock();
+    }
+
+    return ExecGetRecvData(pdata_ptr,nRecvTime,ndatasize);
+}
+
+int lidar_bkdriver::ExecGetRecvData(std::shared_ptr<unsigned char> & pdata_ptr, int64_t & nRecvTime, int & ndatasize)
+{
+    int nread = 0;
+    mmutexrawdata.lock();
+    if(mvecterraw.size() > 0)
+    {
+        pdata_ptr = mvecterraw[0].mpraw_ptr;
+        nRecvTime = mvecterraw[0].mnrecvtime;
+        ndatasize = mvecterraw[0].ndatasize;
+        mvecterraw.erase(mvecterraw.begin());
+        nread = 1;
+    }
+    mmutexrawdata.unlock();
+    return nread;
+}
+

src/driver/driver_lidar_bk_16z/lidar_bkdriver.h

@@ -0,0 +1,72 @@
+#ifndef LIDAR_BKDRIVER_H
+#define LIDAR_BKDRIVER_H
+
+#include "ssFrameLib.h"
+#include "ioapi.h"
+#include "utility.h"
+
+#include <thread>
+#include <memory>
+#include <condition_variable>
+#include <mutex>
+#include <vector>
+
+#include "ICD_LiDAR_API.h"
+
+namespace  iv {
+
+struct lidarraw
+{
+    std::shared_ptr<unsigned char> mpraw_ptr;
+    int ndatasize;
+    int64_t  mnrecvtime;
+};
+}
+
+class lidar_bkdriver
+{
+public:
+    lidar_bkdriver();
+    ~lidar_bkdriver();
+
+public:
+    int GetRecvData(std::shared_ptr<unsigned char> & pdata_ptr, int64_t & nRecvTime,int & ndatasize);
+
+private:
+    rfans_driver::IOAPI *m_ctl_socket;
+    rfans_driver::IOAPI *m_data_socket;
+    rfans_driver::IOAPI *m_heart_socket;
+
+private:
+    void socketInit();
+    void setparam();
+    void configDeviceParams();
+    int progSet(lidarAPi::DEB_PROGRM_S &program);
+
+    int lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfreq, int mt_lsrpwr = 100);
+    int setState(lidarAPi::DEB_PROGRM_S &program);
+
+    int RecvRawThread();
+    int HeartBeatThread();
+
+    int ExecGetRecvData(std::shared_ptr<unsigned char> & pdata_ptr, int64_t & nRecvTime, int & ndatasize);
+
+private:
+    bool mbRun = true;
+    std::thread * mpThreadRaw;
+    std::thread * mpThreadHeartbeat;
+
+    std::mutex mmutexrawdata;
+    std::vector<iv::lidarraw> mvecterraw;
+
+    std::mutex mmutexcv;
+    std::condition_variable mcv;
+
+
+    InputPara_S m_input_para;
+
+    HEARTBEAT_S m_heart;
+    vector<HEARTBEAT_S> m_heartbeat_vec;
+};
+
+#endif // LIDAR_16ZDRIVER_H

src/driver/driver_lidar_bk_16z/main.cpp

@@ -0,0 +1,13 @@
+#include <QCoreApplication>
+
+
+#include "lidar_bkdriver.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    lidar_bkdriver * pdriver = new lidar_bkdriver();
+
+    return a.exec();
+}

src/tool/pcd2bin/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/tool/pcd2bin/main.cpp

@@ -0,0 +1,154 @@
+#include <QCoreApplication>
+
+#include <iostream>
+//#include <pcl/io/io.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/io.h>
+
+#include <QFile>
+
+#include <getopt.h>
+
+
+static char gstrpcdpath[256];
+static char gstrbinpath[256];
+
+void print_useage()
+{
+    std::cout<<" -i --input $pcdpath : map path. eq.  -i /home/nvidia/input.pcd"<<std::endl;
+    std::cout<<" -o --output $binpath : bin path. eq.  -o /home/nvidia/output.bin"<<std::endl;
+    std::cout<<" -h --help print help"<<std::endl;
+}
+
+int  GetOptLong(int argc, char *argv[]) {
+    int nRtn = 0;
+    int opt; // getopt_long() 的返回值
+    int digit_optind = 0; // 设置短参数类型及是否需要参数
+
+
+    // 如果option_index非空，它指向的变量将记录当前找到参数符合long_opts里的
+    // 第几个元素的描述，即是long_opts的下标值
+    int option_index = 0;
+    // 设置短参数类型及是否需要参数
+    const char *optstring = "i:o:h";
+
+    // 设置长参数类型及其简写，比如 --reqarg <==>-r
+    /*
+    struct option {
+             const char * name;  // 参数的名称
+             int has_arg; // 是否带参数值，有三种：no_argument， required_argument，optional_argument
+             int * flag; // 为空时，函数直接将 val 的数值从getopt_long的返回值返回出去，
+                     // 当非空时，val的值会被赋到 flag 指向的整型数中，而函数返回值为0
+             int val; // 用于指定函数找到该选项时的返回值，或者当flag非空时指定flag指向的数据的值
+        };
+    其中：
+        no_argument(即0)，表明这个长参数不带参数（即不带数值，如：--name）
+            required_argument(即1)，表明这个长参数必须带参数（即必须带数值，如：--name Bob）
+            optional_argument(即2)，表明这个长参数后面带的参数是可选的，（即--name和--name Bob均可）
+     */
+    static struct option long_options[] = {
+        {"input", required_argument, NULL, 'i'},
+        {"output", required_argument, NULL, 'o'},
+        {"help",  no_argument,       NULL, 'h'},
+ //       {"optarg", optional_argument, NULL, 'o'},
+        {0, 0, 0, 0}  // 添加 {0, 0, 0, 0} 是为了防止输入空值
+    };
+
+    while ( (opt = getopt_long(argc,
+                               argv,
+                               optstring,
+                               long_options,
+                               &option_index)) != -1) {
+//        printf("opt = %c\n", opt); // 命令参数，亦即 -a -b -n -r
+//        printf("optarg = %s\n", optarg); // 参数内容
+//        printf("optind = %d\n", optind); // 下一个被处理的下标值
+//        printf("argv[optind - 1] = %s\n",  argv[optind - 1]); // 参数内容
+//        printf("option_index = %d\n", option_index);  // 当前打印参数的下标值
+//        printf("\n");
+        switch(opt)
+        {
+        case 'i':
+            strncpy(gstrpcdpath,optarg,255);
+            break;
+        case 'o':
+            strncpy(gstrbinpath,optarg,255);
+            break;
+        case 'h':
+            print_useage();
+            nRtn = 1; //because use -h
+            break;
+        default:
+            break;
+        }
+
+    }
+
+    return nRtn;
+}
+
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    snprintf(gstrpcdpath,255,"");
+    snprintf(gstrbinpath,255,"");
+
+    int nRtn = GetOptLong(argc,argv);
+    if(nRtn == 1)  //show help,so exit.
+    {
+        return 0;
+    }
+
+    if(strnlen(gstrpcdpath,256) < 1)
+    {
+        std::cout<<" no input."<<std::endl;
+        print_useage();
+        return -1;
+    }
+
+    if(strnlen(gstrbinpath,256) < 1)
+    {
+        std::cout<<" no output."<<std::endl;
+        print_useage();
+        return -2;
+    }
+
+    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(
+                new pcl::PointCloud<pcl::PointXYZI>());
+
+    pcl::io::loadPCDFile<pcl::PointXYZI>(gstrpcdpath,*point_cloud);
+
+
+    QFile xFile;
+    xFile.setFileName(gstrbinpath);
+    if(!xFile.open(QIODevice::ReadWrite))
+    {
+        std::cout<<" Open Save bin File Fail."<<std::endl;
+        return -3;
+    }
+
+    int i;
+    int nPCount = static_cast<int>(point_cloud->width);
+    for(i=0;i<nPCount;i++)
+    {
+        pcl::PointXYZI xp = point_cloud->points[i];
+        double x,y,z,intesi;
+        x = xp.x;
+        y = xp.y;
+        z = xp.z;
+        intesi = xp.intensity;
+        xFile.write((char *)&x,sizeof(double));
+        xFile.write((char *)&y,sizeof(double));
+        xFile.write((char *)&z,sizeof(double));
+        xFile.write((char *)&intesi,sizeof(double));
+
+    }
+    xFile.close();
+    return 0;
+
+
+
+//    return a.exec();
+}

src/tool/pcd2bin/pcd2bin.pro

@@ -0,0 +1,42 @@
+QT -= gui
+
+CONFIG += c++1z console
+CONFIG -= app_bundle
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}
+
+
+LIBS += -lboost_system -lboost_program_options
+
+unix:LIBS +=  -lpcl_common\
+        -lpcl_features\
+        -lpcl_filters\
+        -lpcl_io\
+        -lpcl_io_ply\
+        -lpcl_kdtree\
+        -lpcl_keypoints\
+        -lpcl_octree\
+        -lpcl_outofcore\
+        -lpcl_people\
+        -lpcl_recognition\
+        -lpcl_registration\
+        -lpcl_sample_consensus\
+        -lpcl_search\
+        -lpcl_segmentation\
+        -lpcl_surface\
+        -lpcl_tracking\
+        -lpcl_visualization