1、夹持杆雷达代码更新；

Modules/ClampSafety/CMakeLists.txt

@@ -2,6 +2,8 @@ set(Target_Name
 		clamp_safety
 		)
 
+option(OPTION_ENABLE_COMMUNICATION_PLC "启用和plc通信" ON)
+
 include_directories(
 	/usr/local/include/snap7
 )
@@ -27,7 +29,7 @@ message(${Target_Name} ":" ${Target_libary})
 
 add_executable(${Target_Name} ${Target_source})
 target_link_libraries(${Target_Name} ${Target_libary})
-
+target_compile_definitions(${Target_Name} PRIVATE __VIEW__PCL=0)
 # 将库文件，可执行文件，头文件安装到指定目录
 install(TARGETS ${Target_Name}
 		LIBRARY DESTINATION lib  # 动态库安装路径

Modules/ClampSafety/lidar/wanji_716N_device.cpp

@@ -171,7 +171,7 @@ 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) {
+    if (p_cloud.get() == nullptr) {
         return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                              "  POINTER IS NULL ");
     }

Modules/ClampSafety/main.cpp

@@ -77,12 +77,13 @@ void init_glog() {
     FLAGS_stop_logging_if_full_disk = true;
 }
 
-typedef struct {
+struct Clamp{
     Wanji_716N_lidar_device lidar;
     clamp_safety_detect detector;
     Safety_status safety_statu;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr last_cloud;
     std::mutex mutex;
-} Clamp;
+};
 
 bool run(Wanji_716N_lidar_device *device, clamp_safety_detect *pdetector, Safety_status *safety) {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -98,10 +99,20 @@ bool run(Wanji_716N_lidar_device *device, clamp_safety_detect *pdetector, Safety
 void thread_func(Clamp *clamp) {
     while (true) {
         clamp->mutex.lock();
-        Safety_status safety_t;
-        if (run(&(clamp->lidar), &(clamp->detector), &safety_t)) {
-            clamp->safety_statu = safety_t;
+
+        clamp->last_cloud->clear();
+        clamp->lidar.get_last_cloud(clamp->last_cloud, std::chrono::system_clock::now());
+
+        if (clamp->last_cloud->size() < 150) {
+            continue;
         }
+
+        clamp->detector.detect(clamp->last_cloud, clamp->safety_statu);
+
+//        Safety_status safety_t;
+//        if (run(&(clamp->lidar), &(clamp->detector), &safety_t)) {
+//            clamp->safety_statu = safety_t;
+//        }
         clamp->mutex.unlock();
 
         usleep(100 * 1000);
@@ -166,13 +177,9 @@ int main() {
 
     //调试状态下，绑定识别器输出窗口
 #if __VIEW__PCL
-    if (parameter.lidars_size()>0)
         clamps[0].detector.set_viewer(&g_viewer,v1);
-    if (parameter.lidars_size()>1)
         clamps[1].detector.set_viewer(&g_viewer,v2);
-    if (parameter.lidars_size()>2)
         clamps[2].detector.set_viewer(&g_viewer,v3);
-    if (parameter.lidars_size()>3)
         clamps[3].detector.set_viewer(&g_viewer,v4);
 
     while(1)
@@ -180,13 +187,9 @@ int main() {
         usleep(100*1000);
         auto t1=std::chrono::steady_clock::now();
         //调试状态下，顺序执行，因为viewer不支持多线程
-        if (parameter.lidars_size()>0)
             run(&(clamps[0].lidar),&(clamps[0].detector),&(clamps[0].safety_statu));
-        if (parameter.lidars_size()>1)
             run(&(clamps[1].lidar),&(clamps[1].detector),&(clamps[1].safety_statu));
-        if (parameter.lidars_size()>2)
             run(&(clamps[2].lidar),&(clamps[2].detector),&(clamps[2].safety_statu));
-        if (parameter.lidars_size()>3)
             run(&(clamps[3].lidar),&(clamps[3].detector),&(clamps[3].safety_statu));
             auto t2=std::chrono::steady_clock::now();
             auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
@@ -199,13 +202,13 @@ int main() {
     }
 #else
     // 和plc建立连接，一直等待连接成功
-//    auto snap7_client = &Snap7Clamp::get_instance_references();
-//    snap7_client->communication_init();
-//    Snap7Clamp::Snap7_communication_statu status = snap7_client->get_status();
-//    while (status == Snap7Clamp::SNAP7_COMMUNICATION_READY || status == Snap7Clamp::SNAP7_COMMUNICATION_RECEIVE) {
-//        sleep(1);
-//        status = snap7_client->get_status();
-//    }
+    auto snap7_client = &Snap7Clamp::get_instance_references();
+    snap7_client->communication_init();
+    Snap7Clamp::Snap7_communication_statu status = snap7_client->get_status();
+    while (status == Snap7Clamp::SNAP7_COMMUNICATION_READY || status == Snap7Clamp::SNAP7_COMMUNICATION_RECEIVE) {
+        sleep(1);
+        status = snap7_client->get_status();
+    }
 
     // 启动四个线程，分别采集四个雷达的数据。
     std::thread threads[CLAMP_SIZE];
@@ -223,18 +226,17 @@ int main() {
             std::unique_lock<std::mutex> t_lock(clamps[i].mutex);
 //            std::unique_lock<std::mutex> t_lock1(snap7_client->m_data_lock);
             if (!clamps[i].safety_statu.is_timeout()) {
-                LOG(WARNING) << "id: " << i << ", we: " << clamps[i].safety_statu.wheel_exist << ", cx: " << clamps[i].safety_statu.cx * 1000;
+//                LOG(WARNING) << "id: " << i << ", we: " << clamps[i].safety_statu.wheel_exist << ", cx: " << clamps[i].safety_statu.cx * 1000;
 //                LOG(INFO) << "orid " << i;
 //                LOG(INFO) << "wheel_exist " << clamps[i].safety_statu.wheel_exist;
 //                LOG(INFO) << "cx " << clamps[i].safety_statu.cx * 1000;
 //                LOG(INFO) << "gap " << clamps[i].safety_statu.gap * 1000;
 //                LOG(INFO) << "clamp_completed " << clamps[i].safety_statu.clamp_completed;
-//                snap7_client->plcData.wheels[i].wheel_exist = clamps[i].safety_statu.wheel_exist;
-////                snap7_client->plcData.wheels[i].offset = heart;
-////                snap7_client->plcData.wheels[i].gap = 255 - heart;
-//                snap7_client->plcData.wheels[i].offset = clamps[i].safety_statu.cx * 1000;
-//                snap7_client->plcData.wheels[i].gap = clamps[i].safety_statu.gap * 1000;
-//                snap7_client->plcData.wheels[i].clamp_completed = clamps[i].safety_statu.clamp_completed;
+                snap7_client->plcData.wheels[i].wheel_exist = clamps[i].safety_statu.wheel_exist;
+                snap7_client->plcData.wheels[i].offset = clamps[i].safety_statu.cx * 1000;
+                snap7_client->plcData.wheels[i].gap = clamps[i].safety_statu.gap * 1000;
+                snap7_client->plcData.wheels[i].clamp_completed = clamps[i].safety_statu.clamp_completed;
+                snap7_client->setCloudData(1 << i, clamps[i].last_cloud);
             } else {
 //                snap7_client->plcData.clear();
             }

Modules/ClampSafety/plc/snap7_clamp.cpp

@@ -61,12 +61,12 @@ Error_manager Snap7Clamp::communication_init()
     t_snap7_buf.init(CLAMP_SAFETY_PLC_DBNUMBER , 0, sizeof(PLCData), t_variable_information_vector, Snap7_buf::LOOP_COMMUNICATION);
 	m_send_buf_map[0] = t_snap7_buf;
 
-//    t_index = 0;
-//    t_variable_information_vector.clear();
-//    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"heart", typeid(unsigned short).name(), t_index,sizeof(unsigned short), 1 });
-//
-//    t_snap7_buf.init(CLAMP_SAFETY_PLC_DBNUMBER , 50, sizeof(unsigned short), t_variable_information_vector, Snap7_buf::LOOP_COMMUNICATION);
-//    m_receive_buf_map[0] = t_snap7_buf;
+    t_index = 0;
+    t_variable_information_vector.clear();
+    t_variable_information_vector.push_back(Snap7_buf::Variable_information{"warning", typeid(unsigned short).name(), t_index,sizeof(unsigned short), 1 });
+
+    t_snap7_buf.init(CLAMP_SAFETY_HEART_DBNUMBER , 0, sizeof(unsigned short), t_variable_information_vector, Snap7_buf::LOOP_COMMUNICATION);
+    m_receive_buf_map[0] = t_snap7_buf;
 
     plcJsonConfig config(ETC_PATH"ClampSafety/plc.json");
     return Snap7_communication_base::communication_init(config.ip());
@@ -79,22 +79,39 @@ Error_manager Snap7Clamp::communication_uninit()
 	return Snap7_communication_base::communication_uninit();
 }
 
+Error_manager Snap7Clamp::setCloudData(const unsigned char &device, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
+    if ((m_save_cloud_flag & device) == 0) {
+        return {};
+    } else {
+        m_save_cloud_flag & (~device);
+    }
+
+    std::string file = "./log/save_cloud/" + std::to_string(device) + "/" +
+                        std::to_string(std::chrono::system_clock::now().time_since_epoch().count()) + ".txt";
+    FILE* pf=fopen(file.c_str(),"w");
+    for (auto &point: cloud->points) {
+        fprintf(pf,"%.3f %.3f %.3f\n",point.x,point.y,point.z);
+    }
+    fclose(pf);
+
+    return {};
+}
+
 //更新数据
 Error_manager Snap7Clamp::updata_receive_buf()
 {
-//	std::unique_lock<std::mutex> t_lock1(m_receive_buf_lock);
-//	std::unique_lock<std::mutex> t_lock2(m_data_lock);
-//    static unsigned short heart;
-//    memcpy(&heart, m_receive_buf_map[0].mp_buf_obverse, m_receive_buf_map[0].m_size);
-
-    Error_manager ret = Error_code::SUCCESS;
-
-//    if (heart == m_heart) {
-//        printf("---Debug %s %d : heart not change %d ---> %d\n", __func__, __LINE__, m_heart, heart);
-//        ret = Error_code::FAILED;
-//    }
-//    m_heart = heart;
-	return ret;
+	std::unique_lock<std::mutex> t_lock1(m_receive_buf_lock);
+	std::unique_lock<std::mutex> t_lock2(m_data_lock);
+    unsigned char warn;
+    memcpy(&warn, m_receive_buf_map[0].mp_buf_obverse, m_receive_buf_map[0].m_size);
+
+    m_save_cloud_flag = (~m_warn_record) & warn;
+    m_warn_record = warn;
+    if (warn > 0) {
+        LOG(WARNING) << "Get warning: " << warn;
+    }
+
+	return {};
 }
 
 Error_manager Snap7Clamp::updata_send_buf()

Modules/ClampSafety/plc/snap7_clamp.h

@@ -6,6 +6,8 @@
 #include "tool/singleton.h"
 #include "snap7_communication_base.h"
 #include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
 
 class Snap7Clamp:public Singleton<Snap7Clamp>, public Snap7_communication_base
 {
@@ -22,8 +24,8 @@ public:
 
 
 #pragma pack(push, 1)	//struct按照1个byte对齐
-#define CLAMP_SAFETY_HEART_DBNUMBER		9070
 #define CLAMP_SAFETY_PLC_DBNUMBER		9070
+#define CLAMP_SAFETY_HEART_DBNUMBER		9075
     struct WheeLData {
         unsigned short wheel_exist;
         float offset;
@@ -89,6 +91,7 @@ public://API functions
 	//反初始化 通信 模块。
 	Error_manager communication_uninit() override;
 
+    Error_manager setCloudData(const unsigned char &device, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
 protected://member functions
 	//更新数据
     Error_manager updata_receive_buf() override;
@@ -101,5 +104,7 @@ public:
 	PLCData         plcData{};
     unsigned short            m_heart;
 private:
+    unsigned char m_warn_record;
+    unsigned char m_save_cloud_flag;
 
 };