夹持干雷达

CMakeLists.txt

@@ -1,26 +1,62 @@
 set(SON_PROJECT_NAME clamp_lidar)
 message("========== Load son project ${SON_PROJECT_NAME} ==========" )
-option(OPTION_COMMUNICATION_WITH_PLC "plc通信" ON)
+option(OPTION_COMMUNICATION_WITH_PLC "plc通信" OFF)
 message("<=${SON_PROJECT_NAME}=> OPTION_COMMUNICATION_WITH_PLC: " ${OPTION_COMMUNICATION_WITH_PLC})
 
+if(OPTION_COMMUNICATION_WITH_PLC AND NOT ENABLE_LIBRARY_PLC)
+    message(FATAL_ERROR "OPTION_COMMUNICATION_WITH_PLC ON but ENABLE_LIBRARY_PLC OFF")
+endif ()
+
+if(NOT ENABLE_LIBRARY_PCL)
+    message(FATAL_ERROR "ENABLE_LIBRARY_PCL OFF")
+endif ()
+
+if (NOT ENABLE_LIBRARY_GOOGLE_LOG)
+    message(FATAL_ERROR "ENABLE_LIBRARY_GOOGLE_LOG OFF")
+endif ()
+
+if (NOT ENABLE_LIBRARY_JSON)
+    message(FATAL_ERROR "ENABLE_LIBRARY_JSON OFF")
+endif ()
+
+if (NOT ENABLE_LIBRARY_THREAD)
+    message(FATAL_ERROR "Not enable ENABLE_LIBRARY_THREAD")
+endif ()
+
 if (OPTION_COMMUNICATION_WITH_PLC)
+    set(snap7_include "/usr/local/include/snap7")
+    if(IS_DIRECTORY ${snap7_include})
+        include_directories(${snap7_include})
+    else ()
+        message(FATAL_ERROR "Not found snap7 directory ${snap7_include}")
+    endif ()
+
+    aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc plc)
+    set(SON_PROJECT_SOURCE_LIST ${SON_PROJECT_SOURCE_LIST} ${plc})
+    set(SON_PROJECT_DEPEND_LIST ${SON_PROJECT_DEPEND_LIST} libplc)
     add_definitions(-DOPTION_COMMUNICATION_WITH_PLC=1)
 else ()
     add_definitions(-DOPTION_COMMUNICATION_WITH_PLC=0)
 endif ()
 
 include_directories(
-        /usr/local/include/snap7
+        ${PCL_INCLUDE_DIRS}
 )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc plc)
 
-set(SON_PROJECT_SOURCE_LIST
-        ${plc}
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/lidar lidar)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect detect)
+set(SON_PROJECT_SOURCE_LIST ${SON_PROJECT_SOURCE_LIST}
+        ${lidar}
+        ${detect}
         ${CMAKE_CURRENT_LIST_DIR}/main.cpp
 )
 
-set(SON_PROJECT_DEPEND_LIST
-    libplc
+set(SON_PROJECT_DEPEND_LIST ${SON_PROJECT_DEPEND_LIST}
+        libjson
+        liblog
+        libpcl
+        libthread
+        ${PROTOBUF_LIBRARIES}
 )
 
 add_executable(${SON_PROJECT_NAME} ${SON_PROJECT_SOURCE_LIST})

lidar/async_client.cpp

@@ -5,14 +5,14 @@ Async_Client::Async_Client() : m_socket(m_io_service) {
     m_communication_status = E_UNKNOWN;    //通信状态
     m_is_reconnection_flag = false;        // 断线是否重连的标志位, true:断线自动重连, false:断线不重连, 保持断线状态.
 
-    mp_thread_receive = NULL;        // 接受线程, 内存由本模块管理
-    mp_thread_check_connect = NULL;        // 检查线程, 内存由本模块管理
+    mp_thread_receive = nullptr;        // 接受线程, 内存由本模块管理
+    mp_thread_check_connect = nullptr;        // 检查线程, 内存由本模块管理
     m_check_connect_wait_time_ms = 0;    // 检查连接线程 的等待时间
 
     m_data_updata_time = std::chrono::system_clock::now() - std::chrono::hours(1);
 
     memset(m_data_buf, 0, DATA_LENGTH_MAX);
-    mp_communication_data_queue = NULL;
+    mp_communication_data_queue = nullptr;
 }
 
 /**
@@ -23,11 +23,11 @@ Async_Client::~Async_Client() {
 }
 
 //初始化, 默认重连
-Error_manager Async_Client::init(std::string ip, int port, Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
+Error_manager Async_Client::init(const std::string& ip, int port, Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
                                  bool is_reconnection_flag) {
-    if (p_communication_data_queue == NULL) {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "  POINTER IS NULL ");
+    if (p_communication_data_queue == nullptr) {
+        return {Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL "};
     }
 
     m_is_reconnection_flag = is_reconnection_flag;
@@ -67,19 +67,19 @@ Error_manager Async_Client::uninit() {
     if (mp_thread_receive) {
         mp_thread_receive->join();
         delete mp_thread_receive;
-        mp_thread_receive = NULL;
+        mp_thread_receive = nullptr;
     }
     if (mp_thread_check_connect) {
         mp_thread_check_connect->join();
         delete mp_thread_check_connect;
-        mp_thread_check_connect = NULL;
+        mp_thread_check_connect = nullptr;
     }
 
 
     m_io_service.stop();
 
 
-    mp_communication_data_queue = NULL;
+    mp_communication_data_queue = nullptr;
 
     if (m_communication_status != E_FAULT) {
         m_communication_status = E_UNKNOWN;
@@ -92,14 +92,14 @@ Error_manager Async_Client::check_status() {
     if (m_communication_status == E_READY) {
         return Error_code::SUCCESS;
     } else if (m_communication_status == E_UNKNOWN) {
-        return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_UNINITIALIZED, Error_level::MINOR_ERROR,
-                             " Async_Client::check_status() error ");
+        return {Error_code::WJ_LIDAR_COMMUNICATION_UNINITIALIZED, Error_level::MINOR_ERROR,
+                             " Async_Client::check_status() error "};
     } else if (m_communication_status == E_DISCONNECT) {
-        return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_DISCONNECT, Error_level::MINOR_ERROR,
-                             " Async_Client::check_status() error ");
+        return {Error_code::WJ_LIDAR_COMMUNICATION_DISCONNECT, Error_level::MINOR_ERROR,
+                             " Async_Client::check_status() error "};
     } else {
-        return Error_manager(Error_code::WJ_LIDAR_COMMUNICATION_FAULT, Error_level::MINOR_ERROR,
-                             " Async_Client::check_status() error ");
+        return {Error_code::WJ_LIDAR_COMMUNICATION_FAULT, Error_level::MINOR_ERROR,
+                             " Async_Client::check_status() error "};
     }
 }
 
@@ -130,7 +130,6 @@ void Async_Client::thread_receive() {
         }
     }
     LOG(INFO) << " Async_Client::thread_receive end :" << this;
-    return;
 }
 
 //线程检查连接函数，进行连接, 并检查消息是否按时更新, 如果超时, 那么触发重连
@@ -272,7 +271,7 @@ void Async_Client::handle_read(const boost::system::error_code &error,
                                size_t bytes_transferred) {
     if (!error) {
         Binary_buf *tp_binary_buf = new Binary_buf(m_data_buf, bytes_transferred);
-        if (mp_communication_data_queue != NULL) {
+        if (mp_communication_data_queue != nullptr) {
             //将数据添加到队列中, 然后内存权限转交给队列.
             mp_communication_data_queue->push(tp_binary_buf);
         }

lidar/async_client.h

@@ -46,9 +46,9 @@
 #include "glog/logging.h"
 
 #include "error_code/error_code.hpp"
-#include "tool/thread_condition.h"
-#include "tool/binary_buf.h"
-#include "tool/thread_safe_queue.hpp"
+#include "thread/thread_condition.h"
+#include "thread/binary_buf.h"
+#include "thread/thread_safe_queue.hpp"
 
 
 /**
@@ -83,7 +83,7 @@ public:
     ~Async_Client();
 
     //初始化, 默认重连
-    Error_manager init(std::string ip, int port, Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
+    Error_manager init(const std::string& ip, int port, Thread_safe_queue<Binary_buf *> *p_communication_data_queue,
                        bool is_reconnection_flag = true);
 
     //反初始化

lidar/clamp_lidar_protobuf.proto

@@ -0,0 +1,31 @@
+syntax = "proto2";
+
+message ClampLidarScanBoxLimit {
+    required float dist_limit = 1;
+    required float minx = 2;
+    required float maxx = 3;
+    required float miny = 4;
+    required float maxy = 5;
+}
+
+message ClampLidarDeviceConfig {
+    optional bool enable = 1 [default = false];
+    required string ip_address = 2;
+    required int32 port = 3;
+    required float angle_min = 4;
+    required float angle_max = 5;
+    required float angle_increment = 6;
+    required float time_increment = 7;
+    required float range_min = 8;
+    required float range_max = 9;
+    required ClampLidarScanBoxLimit scan_box_limit = 10;
+}
+
+message PLCCommunicationConfig {
+    required string ip = 1;
+}
+
+message ClampLidarProjectConfig {
+    required PLCCommunicationConfig plc = 1;
+    repeated ClampLidarDeviceConfig devices = 2;
+}

lidar/lidar_manager.cpp

@@ -0,0 +1,65 @@
+#include "lidar_manager.h"
+
+Error_manager ClampLidarManager::Init(const std::string &file) {
+    Error_manager ret = loadProtobufFile(file, m_config);
+    if (ret != SUCCESS) {
+        LOG(WARNING) << ret.to_string();
+        return ret;
+    } else {
+        LOG(INFO) << "\n======================================== "
+                     "Load Clamp Lida Config Success "
+                     "========================================\n";
+    }
+
+    for (auto &device: m_config.devices()) {
+        Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;
+        Point2D_tool::Point2D_transform t_point2D_transform;
+
+        t_polar_coordinates_box.angle_min = device.angle_min();
+        t_polar_coordinates_box.angle_max = device.angle_max();
+        t_polar_coordinates_box.distance_min = device.range_min();
+        t_polar_coordinates_box.distance_max = device.range_max();
+
+        Point2D_tool::Point2D_box t_point2D_box;
+        t_point2D_box.x_min = device.scan_box_limit().minx();
+        t_point2D_box.x_max = device.scan_box_limit().maxx();
+        t_point2D_box.y_min = device.scan_box_limit().miny();
+        t_point2D_box.y_max = device.scan_box_limit().maxy();
+
+        auto *wj = new Wanji_716N_lidar_device();
+        Error_manager code = wj->init(device.ip_address(),
+                                      device.port(),
+                                      t_polar_coordinates_box,
+                                      t_point2D_box,
+                                      t_point2D_transform);
+        if (code != SUCCESS) {
+            LOG(ERROR) << code.get_error_description();
+            return code;
+        } else {
+            mp_wj_device.insert(std::pair<std::string, Wanji_716N_lidar_device*>(device.ip_address(), wj));
+//            clamps[i].safety_statu.set_timeout(0.3);
+        }
+    }
+    LOG(INFO) << "\n======================================== "
+                 "Clamp Lida Config Init End "
+                 "========================================\n";
+
+    for (auto &device: mp_wj_device) {
+        std::thread(&ClampLidarManager::thread_func, this, device.first);
+    }
+    return {};
+}
+
+void ClampLidarManager::thread_func(const std::string &ip_address) {
+
+    while (true) {
+        clamp->mutex.lock();
+        Safety_status safety_t;
+        if (run(&(clamp->lidar), &(clamp->detector), &safety_t)) {
+            clamp->safety_statu = safety_t;
+        }
+        clamp->mutex.unlock();
+
+        usleep(100 * 1000);
+    }
+}

lidar/lidar_manager.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include "log/log.h"
+#include "error_code/error_code.hpp"
+#include "clamp_lidar_protobuf.pb.h"
+#include "protobuf/load_protobuf.hpp"
+#include "wanji_716N_device.h"
+
+class ClampLidarManager {
+public:
+    ClampLidarManager() = default;
+    ~ClampLidarManager() = default;
+
+    Error_manager Init(const std::string &file);
+
+private:
+    void thread_func(const std::string &ip_address);
+
+private:
+    ClampLidarProjectConfig m_config;
+    std::map<std::string, Wanji_716N_lidar_device*> mp_wj_device;
+};
+
+
+

lidar/lidarsJsonConfig.cpp

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

lidar/lidarsJsonConfig.h

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

lidar/wanji_716N_device.cpp

@@ -12,7 +12,7 @@ Wanji_716N_lidar_device::~Wanji_716N_lidar_device() {
 }
 
 // 初始化
-Error_manager Wanji_716N_lidar_device::init(std::string t_ip, int t_port,
+Error_manager Wanji_716N_lidar_device::init(const std::string& t_ip, int t_port,
                                             Point2D_tool::Polar_coordinates_box t_polar_coordinates_box,
                                             Point2D_tool::Point2D_box t_point2D_box,
                                             Point2D_tool::Point2D_transform t_point2D_transform) {
@@ -66,7 +66,7 @@ Error_manager Wanji_716N_lidar_device::uninit() {
     if (mp_execute_thread) {
         mp_execute_thread->join();
         delete mp_execute_thread;
-        mp_execute_thread = NULL;
+        mp_execute_thread = nullptr;
     }
 
     m_async_client.uninit();
@@ -87,11 +87,11 @@ Error_manager Wanji_716N_lidar_device::check_status() {
     if (m_wanji_lidar_device_status == E_READY) {
         return Error_code::SUCCESS;
     } else if (m_wanji_lidar_device_status == E_BUSY) {
-        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
-                             "  Wanji_716N_lidar_device::check_status() error ");
+        return {Error_code::WANJI_LIDAR_DEVICE_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
+                             "  Wanji_716N_lidar_device::check_status() error "};
     } else {
-        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
-                             " Wanji_716N_lidar_device::check_status() error ");
+        return {Error_code::WANJI_LIDAR_DEVICE_STATUS_ERROR, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::check_status() error "};
     }
     return Error_code::SUCCESS;
 }
@@ -106,9 +106,9 @@ bool Wanji_716N_lidar_device::is_ready() {
 Error_manager
 Wanji_716N_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                                 std::chrono::system_clock::time_point command_time, int timeout_ms) {
-    if (p_mutex == NULL || p_cloud.get() == NULL) {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "  POINTER IS NULL ");
+    if (p_mutex == nullptr || p_cloud.get() == nullptr) {
+        return {Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL "};
     }
     //判断是否超时
     while (std::chrono::system_clock::now() - command_time < std::chrono::milliseconds(timeout_ms)) {
@@ -124,17 +124,17 @@ Wanji_716N_lidar_device::get_new_cloud_and_wait(std::mutex *p_mutex, pcl::PointC
         std::this_thread::sleep_for(std::chrono::milliseconds(1));
     }
     //超时退出就报错
-    return Error_manager(Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
-                         " Wanji_716N_lidar_device::get_new_cloud_and_wait error ");
+    return {Error_code::WJ_LIDAR_GET_CLOUD_TIMEOUT, Error_level::MINOR_ERROR,
+                         " Wanji_716N_lidar_device::get_new_cloud_and_wait error "};
 }
 
 //外部调用获取当前点云, 获取指令时间之后的点云, 如果没有就会报错, 不会等待
 Error_manager
 Wanji_716N_lidar_device::get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                            std::chrono::system_clock::time_point command_time) {
-    if (p_mutex == NULL || p_cloud.get() == NULL) {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "  POINTER IS NULL ");
+    if (p_mutex == nullptr || p_cloud.get() == nullptr) {
+        return {Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL "};
     }
     //获取指令时间之后的点云, 如果没有就会报错, 不会等待
     if (m_protocol.get_scan_time() > command_time) {
@@ -142,17 +142,17 @@ Wanji_716N_lidar_device::get_current_cloud(std::mutex *p_mutex, pcl::PointCloud<
         Error_manager code = m_protocol.get_scan_points(p_cloud);
         return code;
     } else {
-        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
-                             " Wanji_716N_lidar_device::get_current_cloud error ");
+        return {Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::get_current_cloud error "};
     }
 }
 
 //外部调用获取最新的点云, 获取指令时间往前一个周期内的点云, 如果没有就会报错, 不会等待
 Error_manager Wanji_716N_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                                       std::chrono::system_clock::time_point command_time) {
-    if (p_mutex == NULL || p_cloud.get() == NULL) {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "  POINTER IS NULL ");
+    if (p_mutex == nullptr || p_cloud.get() == nullptr) {
+        return {Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL "};
     }
     int t_scan_cycle_ms = m_protocol.get_scan_cycle();
     //将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
@@ -162,8 +162,8 @@ Error_manager Wanji_716N_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::
         Error_manager code = m_protocol.get_scan_points(p_cloud);
         return code;
     } else {
-        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
-                             " Wanji_716N_lidar_device::get_last_cloud error ");
+        return {Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::get_last_cloud error "};
     }
 }
 
@@ -171,9 +171,9 @@ Error_manager Wanji_716N_lidar_device::get_last_cloud(std::mutex *p_mutex, pcl::
 //注:函数内部不加锁, 由外部统一加锁.
 Error_manager Wanji_716N_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud,
                                                       std::chrono::system_clock::time_point command_time) {
-    if (p_cloud.get() == NULL) {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "  POINTER IS NULL ");
+    if (p_cloud.get() == nullptr) {
+        return {Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL "};
     }
     int t_scan_cycle_ms = m_protocol.get_scan_cycle();
     //将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
@@ -184,8 +184,8 @@ Error_manager Wanji_716N_lidar_device::get_last_cloud(pcl::PointCloud<pcl::Point
         return code;
     } else {
         //    LOG(WARNING) << "get cloud failed..... "<<m_protocol.get_scan_time().time_since_epoch().count()<<", "<<command_time.time_since_epoch().count();
-        return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
-                             " Wanji_716N_lidar_device::get_last_cloud error ");
+        return {Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
+                             " Wanji_716N_lidar_device::get_last_cloud error "};
     }
 }
 

lidar/wanji_716N_device.h

@@ -34,7 +34,7 @@ public:
     ~Wanji_716N_lidar_device();
 
     // 初始化
-    Error_manager init(std::string ip, int port,
+    Error_manager init(const std::string& ip, int port,
                        Point2D_tool::Polar_coordinates_box t_polar_coordinates_box,
                        Point2D_tool::Point2D_box t_point2D_box,
                        Point2D_tool::Point2D_transform t_point2D_transform);

lidar/wj_716N_lidar_protocol.cpp

@@ -35,8 +35,6 @@ wj_716N_lidar_protocol::wj_716N_lidar_protocol() {
 
     mp_scan_points = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     mp_thread_analysis = nullptr;
-
-    std::cout << "wj_716N_lidar_protocl start success" << std::endl;
 }
 
 bool wj_716N_lidar_protocol::setConfig(wj_716_lidarConfig &new_config, uint32_t level) {
@@ -66,15 +64,6 @@ bool wj_716N_lidar_protocol::setConfig(wj_716_lidarConfig &new_config, uint32_t
     // adjust angle_max to max_ang config param
     index_end = 1081 - ((2.35619449 - config_.max_ang) / scan.angle_increment);
     int samples = index_end - index_start;
-
-    std::cout << "frame_id:" << scan.header.frame_id << std::endl;
-    std::cout << "min_ang:" << scan.angle_min << std::endl;
-    std::cout << "max_ang:" << scan.angle_max << std::endl;
-    std::cout << "angle_increment:" << scan.angle_increment << std::endl;
-    std::cout << "time_increment:" << scan.time_increment << std::endl;
-    std::cout << "range_min:" << scan.range_min << std::endl;
-    std::cout << "range_max:" << scan.range_max << std::endl;
-    std::cout << "samples_per_scan:" << samples << std::endl;
     return true;
 }
 
@@ -83,9 +72,9 @@ Error_manager wj_716N_lidar_protocol::init(Thread_safe_queue<Binary_buf *> *p_co
                                            Point2D_tool::Polar_coordinates_box polar_coordinates_box,
                                            Point2D_tool::Point2D_box point2D_box,
                                            Point2D_tool::Point2D_transform point2D_transform) {
-    if (p_communication_data_queue == NULL) {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "  POINTER IS NULL ");
+    if (p_communication_data_queue == nullptr) {
+        return {Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "  POINTER IS NULL "};
     }
     wj_716_lidarConfig t_config;
     t_config.min_ang = polar_coordinates_box.angle_min;
@@ -97,10 +86,6 @@ Error_manager wj_716N_lidar_protocol::init(Thread_safe_queue<Binary_buf *> *p_co
 
     mp_communication_data_queue = p_communication_data_queue;
     m_point2D_box = point2D_box;
-    std::cout << "box xmin:" << m_point2D_box.x_min << std::endl;
-    std::cout << "box xmax:" << m_point2D_box.x_max << std::endl;
-    std::cout << "box ymin:" << m_point2D_box.y_min << std::endl;
-    std::cout << "box ymax:" << m_point2D_box.y_max << std::endl;
     m_point2D_transform = point2D_transform;
     //接受线程, 默认开启循环, 在内部的wait_and_pop进行等待
     m_condition_analysis.reset(false, true, false);
@@ -142,8 +127,8 @@ Error_manager wj_716N_lidar_protocol::uninit() {
 //获取扫描点云
 Error_manager wj_716N_lidar_protocol::get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out) {
     if (p_cloud_out.get() == nullptr) {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             " Wj_716_lidar_protocol::get_scan_points POINTER IS NULL ");
+        return {Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             " Wj_716_lidar_protocol::get_scan_points POINTER IS NULL "};
     }
     std::unique_lock<std::mutex> lck(m_scan_mutex);
     //将扫描点云追加到p_cloud_out后面, 不要清除p_cloud_out原有的数据

lidar/wj_716N_lidar_protocol.h

@@ -9,10 +9,10 @@
 #include "string.h"
 
 #include "error_code/error_code.hpp"
-#include "tool/binary_buf.h"
-#include "tool/thread_safe_queue.hpp"
-#include "tool/thread_condition.h"
-#include "tool/point2D_tool.h"
+#include "thread/binary_buf.h"
+#include "thread/thread_safe_queue.hpp"
+#include "thread/thread_condition.h"
+#include "pcl/point2D_tool.h"
 
 #include <boost/shared_ptr.hpp>
 #include <boost/asio.hpp>

main.cpp

@@ -3,10 +3,44 @@
 //
 #include <iostream>
 #include "error_code/error_code.hpp"
+#include "log/log.h"
+
+#if OPTION_COMMUNICATION_WITH_PLC
+#include "plc/snap7_clamp.h"
+#endif
+
+#include "protobuf/load_protobuf.hpp"
+#include "lidar/lidar_manager.h"
+
+#define OPTION_WATCH_DIDAR_BY_VTK 0
 
 int main(int argc, char **argv) {
-#ifdef PROJECT_NAME
-    std::cout <<  PROJECT_NAME;
+    const char* clamp_config_file = ETC_PATH PROJECT_NAME "/clamp_lidar_config.json";
+
+    if(ZX::InitGlog(PROJECT_NAME, ETC_PATH PROJECT_NAME "/LogInfo/")) {
+        LOG(INFO) << "Init google log success, log file save to " ETC_PATH PROJECT_NAME "/LogInfo";
+    } else {
+        LOG(ERROR) << "Init google log failed, log file save to " ETC_PATH PROJECT_NAME "/LogInfo";
+        exit(EXIT_FAILURE);
+    }
+
+    ClampLidarManager lidarManager;
+    if (lidarManager.Init(clamp_config_file) != SUCCESS) {
+        return EXIT_FAILURE;
+    }
+
+#if OPTION_COMMUNICATION_WITH_PLC
+    auto snap7_client = &Snap7Clamp::get_instance_references();
+    snap7_client->communication_init();
+    Snap7Clamp::Snap7_communication_statu status = snap7_client->get_status();
+    while (status == Snap7Clamp::SNAP7_COMMUNICATION_READY || status == Snap7Clamp::SNAP7_COMMUNICATION_RECEIVE) {
+        sleep(1);
+        status = snap7_client->get_status();
+    }
+#endif
+
+#if OPTION_WATCH_DIDAR_BY_VTK
+
 #endif
 
     return EXIT_SUCCESS;

proto.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+
+protoc -I="./lidar" clamp_lidar_protobuf.proto --cpp_out="./lidar"