1、测试mqtt；

CMakeLists.txt

@@ -3,13 +3,7 @@ set(SON_PROJECT_NAME tof3d)
 message("========== Load son project ${SON_PROJECT_NAME} ==========" )
 
 unset(OPTION_ENABLE_TENSORRT_DETECT_CODE CACHE)
-
-#find_package(CUDA REQUIRED)
-#if (${CUDA_FOUND})
-#    option(OPTION_ENABLE_TENSORRT_DETECT_CODE "" ON)
-#else ()
-    option(OPTION_ENABLE_TENSORRT_DETECT_CODE "" OFF)
-#endif ()
+option(OPTION_ENABLE_TENSORRT_DETECT_CODE "" OFF)
 message("<=${SON_PROJECT_NAME}=> OPTION_ENABLE_TENSORRT_DETECT_CODE: " ${OPTION_ENABLE_TENSORRT_DETECT_CODE})
 
 if (OPTION_ENABLE_TENSORRT_DETECT_CODE)
@@ -94,7 +88,7 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/vzense vzense)
 
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication communication)
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication/grpc communication_grpc)
-#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication/mqtt communication_mqtt)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication/mqtt communication_mqtt)
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication/rabbitmq communication_rabbitmq)
 
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect detect)
@@ -114,16 +108,15 @@ set(SON_PROJECT_SOURCE_LIST ${SON_PROJECT_SOURCE_LIST}
 )
 
 set(SON_PROJECT_DEPEND_LIST ${SON_PROJECT_DEPEND_LIST}
-        liblog
-        libthread
-        librabbitmq
-        libmessage
+        zxthread
+        zxrabbitmq
+        zxpahoc
         ${NebulaSDK_PATH}/lib/libNebula_api.so
         ${absl_LIBRARIES}
         protobuf::libprotobuf
         gRPC::grpc++_reflection
         ${OpenCV_LIBRARIES}
-        libpcl
+        ${PCL_LIBRARIES}
         ${CERES_LIBRARIES}
         ${ALL_LIBS}
 )

communication/communication_manager.cpp

@@ -15,5 +15,10 @@ Error_manager CommunicationManager::Init(const CommunicationManagerConfig &confi
         std::string file_path = ETC_PATH PROJECT_NAME + config.rabbitmq_config_file();
         RabbitmqCommunicationTof3D::get_instance_pointer()->rabbitmq_init_from_protobuf(file_path);
     }
-    return Error_manager();
+
+    if (config.mqtt_enable()) {
+        std::string file_path = ETC_PATH PROJECT_NAME + config.mqtt_config_file();
+        Tof3DMqttAsyncClient::iter()->init(file_path);
+    }
+    return {};
 }

communication/communication_manager.h

@@ -1,6 +1,7 @@
-#include "error_code/error_code.hpp"
+#include "tool/error_code.hpp"
 #include "rabbitmq/rabbitmq_communication.h"
 #include "grpc/streamServer.h"
+#include "mqtt/mqtt_communication.h"
 #include "proto/communication.pb.h"
 
 class CommunicationManager {

communication/mqtt/mqtt_communication.cpp

@@ -0,0 +1,103 @@
+#include "mqtt_communication.h"
+
+void Tof3DMqttAsyncClient::init(const std::string &file) {
+    m_message_arrived_callback_ = CloudDataArrived;
+
+    MqttAsyncClient::init_from_proto(file);
+
+    condit = new Thread_condition();
+
+    t = new std::thread(&Tof3DMqttAsyncClient::run, this);
+
+    condit->notify_all(true);
+}
+
+// 识别结果数据包含识别结果、时间序号、具体识别信息
+int Tof3DMqttAsyncClient::CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen,
+                                             MQTTAsync_message *message) {
+    // 1、查看识别结果
+
+    // 2、队列取数据
+//    cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
+//    memcpy(merge_mat.data, message->payload, message->payloadlen);
+//    cv::imshow("receive", merge_mat);
+    LOG(INFO) << topicName;
+
+    // 3、分离出四份点云
+
+    // 4、发送对应队列
+
+    return 1;
+}
+
+void Tof3DMqttAsyncClient::run() {
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(3000));
+    auto t_start_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
+
+    while (condit->is_alive()) {
+        condit->wait();
+        cost = std::chrono::steady_clock::now() - t_start_time;
+        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(99, cost.count() * 1000)));
+        t_start_time = std::chrono::steady_clock::now();
+
+        if (condit->is_alive()) {
+//            // 1、获取图片并合并
+//            cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
+//            DeviceTof3D::DeviceTof3DSaveInfo devices_info[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
+//            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+//                devices_info[device_index] = DeviceTof3D::iter()->getDeviceSaveInfo((DeviceAzimuth) device_index);
+//                devices_info[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
+//                                                                           ((device_index & 0x2) >> 1) * 480, 640, 480)));
+//            }
+//            cv::imshow("merge_mat", merge_mat);
+//            cv::waitKey(1);
+
+//            // 2、信息放入队列
+//            m_cloud_mutex.lock();
+//            for (auto iter = m_lf_info.begin(); iter != m_lf_info.end();) {
+//                long save_time = std::chrono::steady_clock::now().time_since_epoch().count() - iter->first;
+//                if (save_time > 1000000000) {
+//                    m_rf_info.erase(iter->first);
+//                    m_lr_info.erase(iter->first);
+//                    m_rr_info.erase(iter->first);
+//                    iter = m_lf_info.erase(m_lf_info.find(iter->first));
+//                } else {
+//                    iter++;
+//                }
+//            }
+//            long time_point = std::chrono::steady_clock::now().time_since_epoch().count();
+//            m_lf_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
+//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_LF]));
+//            m_rf_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
+//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_RF]));
+//            m_lr_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
+//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_LR]));
+//            m_rr_info.insert(std::pair<long, DeviceTof3D::DeviceTof3DSaveInfo>(
+//                    time_point, devices_info[DeviceAzimuth::DEVICE_AZIMUTH_RR]));
+//            m_cloud_mutex.unlock();
+
+            // 3、将ir图打包发送mqtt
+//            std::vector<unsigned char> message;
+////            message.push_back(time_point);
+//
+////            for (int rows = 0; rows < merge_mat.rows - 0; rows++) {
+////                for (int cols = 0; cols < merge_mat.cols - 0; cols++) {
+////                    message.push_back(merge_mat.at<uchar>(rows, cols));
+////                }
+////            }
+//            for (auto iter = merge_mat.datastart; iter != merge_mat.dataend; iter++) {
+//                message.push_back(*iter);
+//            }
+//            cv::Mat merge_mat_2 = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
+//            memcpy(merge_mat_2.data, message.data(), message.size());
+            std::vector<unsigned char > test;
+            for (int i = 0; i < 640*480*2*1; i++) {
+                test.push_back('a');
+            }
+            LOG(INFO) << "message size " << test.size();
+            SendMessage("tof/ir", test.data(), test.size());
+        }
+    }
+}

communication/mqtt/mqtt_communication.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#include "pahoc/mqtt_async.h"
+#include "tool/log.hpp"
+#include "vzense/device_tof3d.h"
+
+class Tof3DMqttAsyncClient : public MqttAsyncClient {
+public:
+    static Tof3DMqttAsyncClient *iter() {
+        static Tof3DMqttAsyncClient *instance = nullptr;
+        if (instance == nullptr) {
+            instance = new Tof3DMqttAsyncClient();
+        }
+        return instance;
+    }
+
+    void init(const std::string &file);
+
+protected:
+    Tof3DMqttAsyncClient() = default;
+
+private:
+    void run();
+
+    static int CloudDataArrived(MqttAsyncClient *client, char *topicName, int topicLen, MQTTAsync_message *message);
+
+public:
+
+private:
+    std::mutex m_cloud_mutex;
+    std::map<long, DeviceTof3D::DeviceTof3DSaveInfo> m_lf_info;
+    std::map<long, DeviceTof3D::DeviceTof3DSaveInfo> m_rf_info;
+    std::map<long, DeviceTof3D::DeviceTof3DSaveInfo> m_lr_info;
+    std::map<long, DeviceTof3D::DeviceTof3DSaveInfo> m_rr_info;
+
+    std::thread *t = nullptr;
+    Thread_condition *condit = nullptr;
+};

communication/rabbitmq/rabbitmq_communication.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "thread/singleton.h"
+#include "tool/singleton.hpp"
 #include "rabbitmq/rabbitmq_base.h"
 #include "detect/detect_manager.h"
 #include "Boundary.h"

communication/transitData.h

@@ -9,7 +9,7 @@
 #include <pcl/point_types.h>
 #include <pcl/point_cloud.h>
 #include "tool/timedlockdata.hpp"
-#include "thread/singleton.h"
+#include "tool/singleton.hpp"
 #include <mutex>
 
 #define NUM_ 4

detect/ceres_detect/car_pose_detector.h

@@ -20,7 +20,7 @@
 
 #include <opencv2/opencv.hpp>
 
-#include "thread/singleton.h"
+#include "tool/singleton.hpp"
 
 class Car_pose_detector : public Singleton<Car_pose_detector> {
     friend class Singleton<Car_pose_detector>;

detect/ceres_detect/detect_wheel_ceres.cpp

@@ -446,6 +446,44 @@ bool detect_wheel_ceres::detect_dynP(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl
   return true;
 }
 
+bool detect_wheel_ceres::detect_fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                                     float& cx,float& cy,float& theta,float& wheel_base,
+                                     float& width,float& front_theta,float& loss){
+    pcl::PointXYZ tp1,tp2,tp3,tp4;
+    float yaw1=0.0;
+    float yaw2=0.0;
+    float yaw3=0.0;
+    float yaw4=0.0;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr out1,out2,out3,out4;
+    FallModel::fall(cl1,out1,tp1,yaw1,eNagtive);
+    FallModel::fall(cl2,out2,tp2,yaw2,ePositive);
+    FallModel::fall(cl3,out3,tp3,yaw3,eNagtive);
+    FallModel::fall(cl4,out4,tp4,yaw4,ePositive);
+
+    printf("yaw: %f %f %f %f\n",yaw1*180.0/M_PI,yaw2*180.0/M_PI,yaw3*180.0/M_PI,yaw4*180.0/M_PI);
+
+    float angle_max = 10 * M_PI / 180.0;
+    if (fabs(yaw1) > angle_max || fabs(yaw1) > angle_max || fabs(yaw1) > angle_max || fabs(yaw1) > angle_max) {
+        static int error_index = 0;
+        LOG(ERROR) << "left front cloud result: tp " << tp1 << " yaw " << yaw1;
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl1_" + std::to_string(error_index) + ".txt", cl1);
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out1_" + std::to_string(error_index) + ".txt", out1);
+        LOG(ERROR) << "left front cloud result: tp " << tp2 << " yaw " << yaw2;
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl2_" + std::to_string(error_index) + ".txt", cl2);
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out2_" + std::to_string(error_index) + ".txt", out2);
+        LOG(ERROR) << "left front cloud result: tp " << tp3 << " yaw " << yaw3;
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl3_" + std::to_string(error_index) + ".txt", cl3);
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out3_" + std::to_string(error_index) + ".txt", out3);
+        LOG(ERROR) << "left front cloud result: tp " << tp4 << " yaw " << yaw4;
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/cl4_" + std::to_string(error_index) + ".txt", cl4);
+        Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/error/out4_" + std::to_string(error_index) + ".txt", out4);
+        error_index++;
+    }
+
+    return true;
+
+}
 
 bool detect_wheel_ceres::detect_2Step(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,

detect/ceres_detect/detect_wheel_ceres.h

@@ -11,6 +11,8 @@
 #include <pcl/point_cloud.h>
 #include <pcl/common/centroid.h>
 #include <opencv2/opencv.hpp>
+#include "fallModel.h"
+#include "tool/point3D_tool.hpp"
 
 class detect_wheel_ceres {
 public:
@@ -23,6 +25,9 @@ public:
     }
 
     ~detect_wheel_ceres();
+    bool detect_fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
+                     pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,
+                     float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
 
     bool detect_2Step(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::PointCloud<pcl::PointXYZ>::Ptr cl2,
                       pcl::PointCloud<pcl::PointXYZ>::Ptr cl3,pcl::PointCloud<pcl::PointXYZ>::Ptr cl4,

detect/ceres_detect/fallModel.cpp

@@ -0,0 +1,119 @@
+//
+// Created by zx on 23-12-7.
+//
+
+#include "fallModel.h"
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/common/common.h>
+
+FallModel::FallModel(){
+
+}
+bool FallModel::fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& out,
+                                                    pcl::PointXYZ& tp,float& theta,FallDirect direct){
+
+  auto t1=std::chrono::steady_clock::now();
+  pcl::PointXYZ minp,maxp;
+  pcl::getMinMax3D(*cloud,minp,maxp);
+  float plate=minp.x;
+  if(direct==ePositive)
+    plate=maxp.x;
+
+  Eigen::Vector4f centroid;  //质心
+  pcl::compute3DCentroid(*cloud,centroid); // 计算质心
+  pcl::PointXYZ src_center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
+
+  pcl::PointXYZ new_center, force_center;
+  float dtheta=M_PI,dx=1.0,forceLen=1.0;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr t_cl=cloud;
+  int iter=0;
+  while(  fabs(dx)>1e-5 || fabs(forceLen)>1e-4) {
+    if(iter==1000){
+      printf(" iter 1000 Failed\n");
+      return false;
+    }
+
+    float gravy, force;
+    computeForce(t_cl, new_center, force_center, gravy, force, plate,direct);
+
+    float dis = plate - new_center.x;
+    float r = 1 - exp(-100 * dis * dis);
+    if(direct==ePositive)
+      dx = r * (gravy-force) / float(t_cl->size());
+    else
+      dx = r * (force - gravy) / float(t_cl->size());
+
+    dx = 0.02 * (1 / (1. + exp(-81. * dx)) - 0.5);
+
+    if(direct==ePositive)
+      forceLen = force_center.y - new_center.y;
+    else
+      forceLen = -force_center.y + new_center.y;
+
+    dtheta = force * forceLen * M_PI / 180.0 * 0.01;
+    float dyaw = 0.05 * (1 / (1. + exp(-81. * dtheta)) - 0.5);
+    t_cl = transformCloud(t_cl, new_center, dx, dyaw);
+
+    theta += dyaw;
+    /*printf(" gravy:%f  force:%f force_len:%f  dx :%f  dtheta:%f Yaw:%f\n", gravy, force,
+           forceLen, dx, dtheta * 180.0 / M_PI, theta * 180.0 / M_PI);*/
+    iter++;
+  }
+  auto t2=std::chrono::steady_clock::now();
+  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);
+  double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
+
+  printf("iter:%d  time:%f\n",iter,time);
+  tp=pcl::PointXYZ(new_center.x-src_center.x,new_center.y-src_center.y,new_center.z-src_center.z);
+  out= t_cl;
+  return true;
+}
+
+void FallModel::computeForce(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointXYZ& center,
+                  pcl::PointXYZ& force_center,float& gravy,float& force,float plate ,FallDirect direct){
+  Eigen::Vector4f centroid;  //质心
+  pcl::compute3DCentroid(*cloud,centroid); // 计算质心
+  center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
+  gravy=cloud->size()*1.0;
+  force=0;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr gravy_cl(new pcl::PointCloud<pcl::PointXYZ>);
+  for(int i=0;i<cloud->size();++i){
+    float deep=cloud->points[i].x-plate;
+    if(direct==eNagtive) {
+      if (deep < 0) {
+        float t = pow(36 * deep, 4) * 10.0;
+        if (t > 2) {
+          t = 2;
+        }
+        force += t;
+        gravy_cl->push_back(cloud->points[i]);
+      }
+    }else{
+      if (deep > 0) {
+        float t = pow(36 * deep, 4) * 10.0;
+        if (t > 2) {
+          t = 2;
+        }
+        force += t;
+        gravy_cl->push_back(cloud->points[i]);
+      }
+    }
+  }
+  pcl::compute3DCentroid(*gravy_cl,centroid); // 计算质心
+  force_center=pcl::PointXYZ(centroid[0],centroid[1],centroid[2]);
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr FallModel::transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                             pcl::PointXYZ center,float dx,float theta) {
+  Eigen::Rotation2D<float> rotation(theta);
+  Eigen::Matrix<float, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+  pcl::PointCloud<pcl::PointXYZ>::Ptr r_cl(new pcl::PointCloud<pcl::PointXYZ>);
+  for (int i = 0; i < cloud->size(); ++i) {
+    const Eigen::Matrix<float, 2, 1> point(cloud->points[i].x, cloud->points[i].y);
+    const Eigen::Matrix<float, 2, 1> tp(center.x, center.y);
+    //减去经过车辆旋转计算的左前中心
+    const Eigen::Matrix<float, 2, 1> tanslate_point = rotation_matrix * (point - tp) +tp;
+    r_cl->push_back(pcl::PointXYZ(tanslate_point[0]+dx, tanslate_point[1], 0));
+  }
+  return r_cl;
+}

detect/ceres_detect/fallModel.h

@@ -0,0 +1,36 @@
+//
+// Created by zx on 23-12-7.
+//
+
+#ifndef CERES_TEST_FALLMODEL_H
+#define CERES_TEST_FALLMODEL_H
+
+#include <ceres/ceres.h>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/centroid.h>
+#include <opencv2/opencv.hpp>
+#include "wheel_detector.h"
+
+enum FallDirect{
+    ePositive=0,    //正方向落体
+    eNagtive=1,     //负方向落体
+
+};
+class FallModel {
+public:
+    FallModel();
+    static  bool fall(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr& out,
+                     pcl::PointXYZ& tp,float& theta,FallDirect direct);
+
+    static pcl::PointCloud<pcl::PointXYZ>::Ptr transformCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                                              pcl::PointXYZ center,float dx,float theta);
+protected:
+    static void computeForce(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointXYZ& center,
+                        pcl::PointXYZ& force_center,float& gravy,float& force,float minx,FallDirect direct);
+
+
+};
+
+#endif //CERES_TEST_FALLMODEL_H

detect/ceres_detect/wheel_detector.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "thread/singleton.h"
+#include "tool/singleton.hpp"
 #include <algorithm>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -9,7 +9,7 @@
 #include <pcl/common/transforms.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
-#include "pcl/point3D_tool.h"
+#include "tool/point3D_tool.hpp"
 
 
 class WheelCeresDetector : public Singleton<WheelCeresDetector> {

detect/detect_manager.cpp

@@ -61,7 +61,7 @@ void DetectManager::run() {
             // 模型识别
             std::vector<Object> objs;
             cv::cvtColor(merge_mat, merge_mat, cv::COLOR_GRAY2RGB);
-            detector->detect(merge_mat, objs, merge_mat);
+//            detector->detect(merge_mat, objs, merge_mat);
             cv::imshow("merge_mat", merge_mat);
             cv::waitKey(1);
             if (objs.empty()) {
@@ -129,6 +129,12 @@ void DetectManager::run() {
                     detect_result.car.wheel_base = 2.7;
                 }
 
+                // TODO：测试功能
+                wheel_detect->detect_fall(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
+                                          vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
+                                          detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
+                                          detect_result.car.theta, detect_result.car.wheel_base,
+                                          detect_result.car.wheel_width,detect_result.car.front_wheels_theta, detect_result.car.loss);
                 // 根据车轮数采用不同的优化方式
                 if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
                     objs.size() > 2) {

detect/detect_manager.h

@@ -2,7 +2,7 @@
 
 #include <thread>
 
-#include "error_code/error_code.hpp"
+#include "tool/error_code.hpp"
 #include "ceres_detect/detect_wheel_ceres.h"
 #include "proto/enum_type.pb.h"
 #include "proto/detect_message.pb.h"

proto/communication.proto

@@ -24,6 +24,8 @@ message CommunicationManagerConfig {
     optional bool grpc_enable = 3 [default = true];
     optional string grpc_server_ip = 4 [default = "127.0.0.1"];
     optional int32 grpc_server_port = 5 [default = 9876];
+    optional bool mqtt_enable = 6 [default = true];
+    optional string mqtt_config_file = 7 [default = "/mqtt.json"];   // 安装目录下的路径，并非绝对路径或者bin文件开始的相对路径
 }
 
 message measure_buffer{

tof3DMain.cpp

@@ -2,11 +2,11 @@
 // Created by zx on 2023/10/7.
 //
 #include <iostream>
-#include "log/log.h"
+#include "tool/log.hpp"
 #include "vzense/device_tof3d.h"
 #include "detect/detect_manager.h"
 #include "proto/measure_manager.pb.h"
-#include "protobuf/load_protobuf.hpp"
+#include "tool/load_protobuf.hpp"
 #include "communication/communication_manager.h"
 
 int main(int argc, char * argv[]) {
@@ -16,7 +16,8 @@ int main(int argc, char * argv[]) {
     // 初始化相机处理程序
     tof3dManagerParams tof3d_etc;
     if (loadProtobufFile(ETC_PATH PROJECT_NAME "/tof3d_manager.json", tof3d_etc) == SUCCESS) {
-        auto ret = DeviceTof3D::iter()->Init(tof3d_etc.vzense_tof3d_devices());
+        // auto ret = DeviceTof3D::iter()->Init(tof3d_etc.vzense_tof3d_devices());
+        Error_manager ret;
         if (ret != SUCCESS) {
             LOG(ERROR) << ret.to_string();
             return EXIT_FAILURE;
@@ -27,10 +28,10 @@ int main(int argc, char * argv[]) {
     }
 
     // 初始化测量程序
-    DetectManager::iter()->Init();
+    // DetectManager::iter()->Init();
 
     // 通信
-    CommunicationManager::iter()->Init(tof3d_etc.communication());
+     CommunicationManager::iter()->Init(tof3d_etc.communication());
 
     // 加载看门狗 TODO：添加看门狗，监视线程
     LOG(INFO) << "---------- all init complate ----------";

vzense/back/device_tof3d_b.cpp

@@ -472,7 +472,7 @@ Error_manager DeviceTof3D::IrFrame2Mat(VzFrame &frame, cv::Mat &mat) {
 }
 
 #include <random>
-#include "file/pathcreator.h"
+#include "file/pathcreator.hpp"
 
 void DeviceTof3D::receiveFrameThread(const std::string &ip) {
     LOG(INFO) << ip << " in thread " << std::this_thread::get_id();

vzense/back/device_tof3d_b.h

@@ -15,13 +15,13 @@
 
 #include "error_code/error_code.hpp"
 #include "VzenseNebula_api.h"
-#include "log/log.h"
+#include "log/log.hpp"
 #include "thread/thread_condition.h"
 #include "communication/grpc/streamServer.h"
 #include "tool/timedlockdata.hpp"
 #include "tool/ip.hpp"
 #include "pcl/trans_coor.hpp"
-#include "pcl/point3D_tool.h"
+#include "pcl/point3D_tool.hpp"
 #include "proto/vzense.pb.h"
 
 #ifdef ENABLE_TENSORRT_DETECT

vzense/device_tof3d.cpp

@@ -1,5 +1,4 @@
 #include "device_tof3d.h"
-
 Error_manager DeviceTof3D::Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
     LOG(INFO) << "---------- Init DeviceTof3D ----------";
     if (tof_devices.size() < DeviceAzimuth::DEVICE_AZIMUTH_MAX) {
@@ -39,7 +38,7 @@ Error_manager DeviceTof3D::Init(const google::protobuf::RepeatedPtrField<tof3dVz
         if (device.etc->enable_device() && device.handle == nullptr) {
             ConnectDevice(device.etc->azimuth());
             m_devices_status[device.etc->azimuth()] = 0;
-            setTof3dParams(device.handle, device.etc->bip());
+//            setTof3dParams(device.handle, device.etc->bip());
             device.handle_mutex = &all_device_mutex;
             device.condit = new Thread_condition();
             device.t = new std::thread(&DeviceTof3D::run, this, device.etc->azimuth());
@@ -533,33 +532,31 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
                 // 灰度图转换
                 Frame2Mat(irFrame, depthInfo.irMat);
 
-                // 点云转换
-                for (int cols = 0; cols < depthInfo.pointMat.cols - 0; cols++) {
-                    for (int rows = 0; rows < depthInfo.pointMat.rows - 0; rows++) {
-                        int i = rows * depthInfo.pointMat.cols + cols;
-                        if (sqrt(worldV[i].x * worldV[i].x + worldV[i].y * worldV[i].y + worldV[i].z * worldV[i].z) *
-                            0.001f > 5) {
-                            continue;
-                        }
-                        Eigen::Vector3f trans;
-                        trans << worldV[i].x * 0.001f, worldV[i].y * 0.001f, worldV[i].z * 0.001f;
-                        trans = rotation_matrix3[azimuth] * trans + move[azimuth];
-                        if (trans(2) < 0.03 || fabs(trans(0)) > 1.5 || fabs(trans(1)) > 2.7 || trans(2) > 1) {
-                            continue;
-                        }
-                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = trans(0);
-                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = trans(1);
-                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = trans(2);
-                    }
-                }
-
-                // 将图片和点云存储起来
-                t_ir_img = depthInfo.irMat.clone();
-                t_point_img = depthInfo.pointMat.clone();
+//                // 点云转换
+//                for (int cols = 0; cols < depthInfo.pointMat.cols - 0; cols++) {
+//                    for (int rows = 0; rows < depthInfo.pointMat.rows - 0; rows++) {
+//                        int i = rows * depthInfo.pointMat.cols + cols;
+////                        if (sqrt(worldV[i].x * worldV[i].x + worldV[i].y * worldV[i].y + worldV[i].z * worldV[i].z) *
+////                            0.001f > 5) {
+////                            continue;
+////                        }
+//                        Eigen::Vector3f trans;
+//                        trans << worldV[i].x * 0.001f, worldV[i].y * 0.001f, worldV[i].z * 0.001f;
+////                        trans = rotation_matrix3[azimuth] * trans + move[azimuth];
+////                        if (trans(2) < 0.03 || fabs(trans(0)) > 1.5 || fabs(trans(1)) > 2.7 || trans(2) > 1) {
+////                            continue;
+////                        }
+//                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = trans(0);
+//                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = trans(1);
+//                        depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = trans(2);
+//                    }
+//                }
+
+                memcpy(depthInfo.pointMat.data, worldV, 480 * 640 * sizeof(VzVector3f));
 
-                TransitData::get_instance_pointer()->setImage(t_ir_img, azimuth);
-                TransitData::get_instance_pointer()->setImageCL(t_point_img, azimuth);
                 m_device_mat[azimuth] = depthInfo;
+//                cv::imshow(std::to_string(azimuth), depthInfo.pointMat);
+//                cv::waitKey(1);
             }
         } else {
             LOG_EVERY_N(WARNING, 1000) << device.etc->ip() << " device handle is null.";

vzense/device_tof3d.h

@@ -12,14 +12,14 @@
 #include <pcl/filters/voxel_grid.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
-#include "log/log.h"
-#include "error_code/error_code.hpp"
 #include "VzenseNebula_api.h"
 #include "thread/thread_condition.h"
-#include "tool/timedlockdata.hpp"
-#include "pcl/point3D_tool.h"
 #include "proto/vzense.pb.h"
 #include "communication/transitData.h"
+#include "tool/log.hpp"
+#include "tool/error_code.hpp"
+#include "tool/timedlockdata.hpp"
+#include "tool/point3D_tool.hpp"
 
 class DeviceTof3D {
 public:
@@ -29,7 +29,7 @@ public:
 
         DeviceTof3DSaveInfo() {
             irMat = cv::Mat::zeros(480, 640, CV_8UC1);
-            pointMat = cv::Mat::zeros(480, 640, CV_32FC4);
+            pointMat = cv::Mat::zeros(480, 640, CV_32FC3);
         }
 
         DeviceTof3DSaveInfo& operator=(const DeviceTof3DSaveInfo &p) {
@@ -114,11 +114,6 @@ private:
             delete condit;
             condit = nullptr;
 
-            if (handle != nullptr) {
-                delete handle;
-                handle = nullptr;
-            }
-
             delete etc;
             etc = nullptr;
         }