1、一系列小修改；

communication/mqtt/mqtt_communication.cpp

@@ -1,4 +1,5 @@
 #include "mqtt_communication.h"
+#include "tool/time.hpp"
 
 void Tof3DMqttAsyncClient::init(const std::string &file) {
     m_message_arrived_callback_ = CloudDataArrived;
@@ -20,6 +21,8 @@ int Tof3DMqttAsyncClient::CloudDataArrived(void *client, char *topicName, int to
         JetStream::LabelYolo seg_results;
         seg_results.ParseFromArray(message->payload, message->payloadlen);
         LabelYoloDataBuffer::iter()->lock_exe(&LabelYoloDataBuffer::set, seg_results);
+        int lable = TimeTool::timeDropoutLL(TimeTool::TemporalPrecision::TemporalPrecisionMs, TimeTool::TemporalPrecision::TemporalPrecisionHour);
+        LOG(INFO) << "after " << (lable - seg_results.label()) << "ms";
     }
     return 1;
 }
@@ -37,10 +40,7 @@ void Tof3DMqttAsyncClient::run() {
         t_start_time = std::chrono::steady_clock::now();
 
         if (condit->is_alive()) {
-            static int lable = 0;
-            if (lable++ > 1000) {
-                lable = 0;
-            }
+            int lable = TimeTool::timeDropoutLL(TimeTool::TemporalPrecision::TemporalPrecisionMs, TimeTool::TemporalPrecision::TemporalPrecisionHour);
             // 1、获取图片并合并
             cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
             DeviceTof3D::DeviceTof3DSaveInfo devices_info[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
@@ -49,13 +49,21 @@ void Tof3DMqttAsyncClient::run() {
                 MatDataBuffer::iter()->lock_exe(&MatDataBuffer::set, device_index, lable, devices_info[device_index].pointMat);
                 devices_info[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
                                                                            ((device_index & 0x2) >> 1) * 480, 640, 480)));
+                MatDataBuffer::iter()->lock_exe(&MatDataBuffer::setIr, lable, merge_mat);
             }
+            // cv::imwrite(ETC_PATH PROJECT_NAME "/failed/merge_mat_" + std::to_string(lable) + ".jpg", merge_mat);
             JetStream::LabelImage testimage;
-            NetMessageTrans::lableMat2Proto(lable, merge_mat, testimage);
+            NetMessageTrans::lableMat2Proto(lable, merge_mat, testimage, false);
             int size_1 = testimage.ByteSizeLong();
             unsigned char data[size_1];
             testimage.SerializeToArray((void *)data, size_1);
             SendMessage("tof/ir", data, size_1);
+
+            int test_lable = 0;
+            cv::Mat pic;
+            NetMessageTrans::Proto2lableMat(testimage, test_lable, pic);
+            cv::imshow("pic", pic);
+            cv::waitKey(1);
         }
     }
 }

communication/mqtt/net_message_trans.h

@@ -6,18 +6,33 @@
 
 class NetMessageTrans {
 public:
-    static void lableMat2Proto(int lable, cv::Mat &mat, JetStream::LabelImage &image) {
+    static void lableMat2Proto(int lable, cv::Mat &mat, JetStream::LabelImage &image, bool encode = false) {
         image.set_label(lable);
         image.mutable_ir()->set_width(mat.cols);
         image.mutable_ir()->set_height(mat.rows);
         image.mutable_ir()->set_channel(mat.type());
-        image.mutable_ir()->set_data(mat.data, mat.dataend - mat.datastart);
+        image.mutable_ir()->set_encode(encode);
+        if (encode) {
+            std::vector<uchar> merge_encode_data;
+//            cv::cvtColor(mat, mat, cv::COLOR_GRAY2RGB);
+            cv::imencode(".jpg", mat, merge_encode_data, {cv::IMWRITE_JPEG_QUALITY, 80});
+            image.mutable_ir()->set_data(merge_encode_data.data(), merge_encode_data.size());
+        } else {
+            image.mutable_ir()->set_data(mat.data, mat.dataend - mat.datastart);
+        }
         // LOG(INFO) << "mat.dataend - mat.datastart = " << mat.dataend - mat.datastart;
     }
 
     static void Proto2lableMat(JetStream::LabelImage &image, int &lable, cv::Mat &mat) {
         lable = image.label();
-        mat = cv::Mat(image.ir().height(), image.ir().width(), image.ir().channel(), (void*)image.ir().data().data());
+        if (image.ir().encode()) {
+            std::vector<uchar> data;
+            data.resize(image.ir().data().size());
+            memcpy((void *)data.data(), (void *)image.ir().data().data(), data.size());
+            mat = cv::imdecode(data, 0);
+        } else {
+            mat = cv::Mat(image.ir().height(), image.ir().width(), image.ir().channel(), (void*)image.ir().data().data());
+        }
     }
 
     static void getSegBoxData() {

communication/rabbitmq/Boundary.cpp

@@ -39,8 +39,8 @@ Boundary::~Boundary() {}
 
 BoundRet Boundary::limit_boundary(JetStream::MeasureInfo info, JetStream::Boundary bound) {
     //判断角度
-    if (info.theta() <= bound.minangle() / 180.0 * M_PI) return eRightAngle;
-    if (info.theta() >= bound.maxangle() / 180.0 * M_PI) return eLeftAngle;
+    if (info.theta() <= bound.minangle() / 180.0 * M_PI) return BoundRetRightAngle;
+    if (info.theta() >= bound.maxangle() / 180.0 * M_PI) return BoundRetLeftAngle;
     //计算四轮位置
     pcl::PointXYZ pt[4];
     pt[0] = pcl::PointXYZ(-info.width() / 2.0, info.wheelbase() / 2.0, 0);
@@ -55,23 +55,23 @@ BoundRet Boundary::limit_boundary(JetStream::MeasureInfo info, JetStream::Bounda
         // printf(" %f  %f\n", x * x + y * y, bound.radius() * bound.radius());
         if (x * x + y * y > bound.radius() * bound.radius()) {
             if (fabs(y) > fabs(x)) {
-                if (y > 0) return eFront;
-                else return eBack;
+                if (y > 0) return BoundRetFront;
+                else return BoundRetBack;
             } else {
-                if (x > 0) return eRight;
-                else return eLeft;
+                if (x > 0) return BoundRetRight;
+                else return BoundRetLeft;
             }
         }
         //判断左右
-        if (x < bound.left()) return eLeft;
-        if (x > bound.right()) return eRight;
+        if (x < bound.left()) return BoundRetLeft;
+        if (x > bound.right()) return BoundRetRight;
     }
     //判断摆正后，后轮是否能够的着
-    if (info.trans_y() - 0.5 * info.wheelbase() <= bound.buttom()) return eBack;
+    if (info.trans_y() - 0.5 * info.wheelbase() <= bound.buttom()) return BoundRetBack;
 
-    if (fabs(info.ftheta()) > bound.fwheelangle()) return eWheelAngle;
+    if (fabs(info.ftheta()) > bound.fwheelangle()) return BoundRetWheelAngle;
 
-    return eNormal;
+    return BoundRetNormal;
 
 }
 
@@ -83,13 +83,13 @@ void Boundary::limit(JetStream::MeasureInfo &info) {
     info.set_trans_y(trans_y);
     //判断轴距和宽度是否超
     if (info.width() >= limit_.maxwidth()) {
-        info.set_border_plc(int(eWidth));
-        info.set_border_display(int(eWidth));
+        info.set_border_plc(int(BoundRetWidth));
+        info.set_border_display(int(BoundRetWidth));
         return;
     }
     if (info.wheelbase() >= limit_.maxwheelbase()) {
-        info.set_border_plc(int(eWheelBase));
-        info.set_border_display(int(eWheelBase));
+        info.set_border_plc(int(BoundRetWheelBase));
+        info.set_border_display(int(BoundRetWheelBase));
         return;
     }
 

communication/rabbitmq/Boundary.h

@@ -11,16 +11,16 @@
 #include "detect/detect_manager.h"
 
 enum BoundRet {
-    eNormal = Range_status::Range_correct,  //正常
-    eWidth = Range_status::Range_car_width,
-    eWheelBase = Range_status::Range_car_wheelbase,
-    eFront = Range_status::Range_front,   //前超界
-    eBack = Range_status::Range_back,    //后超界
-    eLeft = Range_status::Range_left,    //左超界
-    eRight = Range_status::Range_right,   //右超界
-    eLeftAngle = Range_status::Range_angle_anti_clock, //左倾角过大
-    eRightAngle = Range_status::Range_angle_clock,  //右倾角过大
-    eWheelAngle = Range_status::Range_steering_wheel_nozero   //前轮角未回正
+    BoundRetNormal = Range_status::Range_correct,  //正常
+    BoundRetWidth = Range_status::Range_car_width,
+    BoundRetWheelBase = Range_status::Range_car_wheelbase,
+    BoundRetFront = Range_status::Range_front,   //前超界
+    BoundRetBack = Range_status::Range_back,    //后超界
+    BoundRetLeft = Range_status::Range_left,    //左超界
+    BoundRetRight = Range_status::Range_right,   //右超界
+    BoundRetLeftAngle = Range_status::Range_angle_anti_clock, //左倾角过大
+    BoundRetRightAngle = Range_status::Range_angle_clock,  //右倾角过大
+    BoundRetWheelAngle = Range_status::Range_steering_wheel_nozero   //前轮角未回正
 };
 
 class Boundary {

communication/rabbitmq/rabbitmq_communication.cpp

@@ -23,7 +23,31 @@ Error_manager RabbitmqCommunicationTof3D::execute_msg(Rabbitmq_message *p_msg) {
 
 //处理消息, 需要子类重载
 Error_manager RabbitmqCommunicationTof3D::execute_time_consume_msg(Rabbitmq_message *p_msg) {
-//    LOG(INFO) << "get message from " << p_msg->m_routing_key;
+    if (p_msg->m_routing_key.find("dispatch_") != std::string::npos &&
+        p_msg->m_routing_key.find("_statu_port") != std::string::npos) {
+        dispatch_node_statu t_dispatch_node_statu;
+        if(google::protobuf::TextFormat::ParseFromString(p_msg->m_message_buf, &t_dispatch_node_statu))
+        {
+            // TODO:开关门控制算法流程
+//            usleep(5 * 1000 * 1000);
+//            DetectManager::iter()->Stop();
+//            DeviceTof3D::iter()->Stop();
+//
+//            usleep(5 * 1000 * 1000);
+//            DeviceTof3D::iter()->Continue();
+//            DetectManager::iter()->Continue();
+        }
+    }
+
+    //终端点击存车时, 发送存车表单给检查节点, 这里顺便发送给感测节点.
+    if (p_msg->m_routing_key == "user_park_command_request_port") {
+        park_table t_park_table_msg; //存车表单
+        if(google::protobuf::TextFormat::ParseFromString(p_msg->m_message_buf, &t_park_table_msg))
+        {
+            // TODO:从算法取数据保存
+        }
+    }
+
     return {};
 }
 

detect/ceres_detect/wheel_detector.h

@@ -20,6 +20,7 @@ public:
         bool haveCloud = false;
         bool segmentOk = false;
         bool detectOk = false;
+        bool detectOkOnTime = false;
         pcl::PointXYZ center;
         float confidence = 0;
         float theta = 0;
@@ -29,6 +30,7 @@ public:
             haveCloud = false;
             segmentOk = false;
             detectOk = false;
+            detectOkOnTime = false;
             center.x = 0;
             center.y = 0;
             center.z = 0;

detect/data_buf_lable.hpp

@@ -45,7 +45,7 @@ public:
         if (azimuth >= DeviceAzimuth::DEVICE_AZIMUTH_MAX) {
             return false;
         }
-        if (data[azimuth].size() > 100) {
+        if (data[azimuth].size() > 10) {
             data[azimuth].erase(data[azimuth].begin());
         }
         data[azimuth].emplace_back(lable, mat);
@@ -59,20 +59,48 @@ public:
         }
          for (auto & iter : data[azimuth]) {
              if (iter.lable != lable) {
-                 LOG(INFO) << lable << " != " << iter.lable;
+                 //LOG(INFO) << lable << " != " << iter.lable;
                  continue;
              } else {
                  std::chrono::duration<double> cost = std::chrono::steady_clock::now() - iter.t;
                  if (cost.count() > timeout) {
-                     LOG(INFO) << "data is too old : " << cost.count() << "s";
+                     //LOG(INFO) << "data is too old : " << cost.count() << "s";
                      return false;
                  } else {
-                     mat = iter.pointsMat.clone();
+                     mat = iter.mat.clone();
                      return true;
                  }
              }
          }
-        LOG(INFO) << "Not found data: " << lable << " in device " << azimuth;
+        //(INFO) << "Not found data: " << lable << " in device " << azimuth;
+        return false;
+    }
+
+    bool setIr(int lable, cv::Mat &mat) {
+        if (ir.size() > 10) {
+            ir.erase(ir.begin());
+        }
+        ir.emplace_back(lable, mat);
+        return true;
+    }
+
+    bool getIr(int lable, cv::Mat &mat, double timeout = 10) {
+        for (auto & iter : ir) {
+            if (iter.lable != lable) {
+                //LOG(INFO) << lable << " != " << iter.lable;
+                continue;
+            } else {
+                std::chrono::duration<double> cost = std::chrono::steady_clock::now() - iter.t;
+                if (cost.count() > timeout) {
+                    //LOG(INFO) << "data is too old : " << cost.count() << "s";
+                    return false;
+                } else {
+                    mat = iter.mat.clone();
+                    return true;
+                }
+            }
+        }
+        //(INFO) << "Not found data: " << lable << " in device " << azimuth;
         return false;
     }
 
@@ -80,14 +108,15 @@ private:
     struct DataBufferInfo {
         std::chrono::steady_clock::time_point t;
         int lable;
-        cv::Mat pointsMat;
+        cv::Mat mat;
 
-        DataBufferInfo(int lable_, cv::Mat &mat_) : lable(lable_), pointsMat(mat_.clone()) {
+        DataBufferInfo(int lable_, cv::Mat &mat_) : lable(lable_), mat(mat_.clone()) {
             t = std::chrono::steady_clock::now();
         }
     };
 private:
     std::vector<DataBufferInfo> data[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
+    std::vector<DataBufferInfo> ir;
 };
 
 class LabelYoloDataBuffer : public lockFunc<LabelYoloDataBuffer> {

detect/detect_manager.cpp

@@ -4,7 +4,11 @@
 
 #include "detect_manager.h"
 
-Error_manager DetectManager::Init() {
+Error_manager DetectManager::Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices) {
+    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+        updateTrans((DeviceAzimuth)device_index, tof_devices[device_index].trans());
+    }
+
     m_thread = new std::thread(&DetectManager::run, this);
     m_condit.notify_all(true);
 
@@ -16,6 +20,24 @@ Error_manager DetectManager::Release() {
     return {};
 }
 
+Error_manager DetectManager::updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans) {
+    Eigen::Matrix4f rotaition = Eigen::Matrix4f::Identity();
+    Eigen::Matrix3f rotation_matrix3 = Eigen::Matrix3f::Identity();
+    rotation_matrix3 =
+            Eigen::AngleAxisf(trans.yaw() * M_PI / 180.f, Eigen::Vector3f::UnitZ()) *
+            Eigen::AngleAxisf(trans.pitch() * M_PI / 180.f, Eigen::Vector3f::UnitY()) *
+            Eigen::AngleAxisf(trans.roll() * M_PI / 180.f, Eigen::Vector3f::UnitX());
+    Eigen::Vector3f move;
+    move << trans.x(), trans.y(), trans.z();
+    rotaition.block<3, 3>(0, 0) = rotation_matrix3;
+    rotaition.col(3) << move;
+    LOG(INFO) << "device " << azimuth << " update rotation info: \n" << rotaition;
+    rotation_matrix4[azimuth] = rotaition;
+    return {};
+}
+
+#include "tool/time.hpp"
+
 void DetectManager::run() {
     LOG(INFO) << "detect thread running in " << std::this_thread::get_id();
 
@@ -27,45 +49,89 @@ void DetectManager::run() {
 
     auto t_start_time = std::chrono::steady_clock::now();
     std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
+    RecordImages record_images;
 
     while (m_condit.is_alive()) {
         m_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)));
+        std::this_thread::sleep_for(std::chrono::milliseconds(50 - std::min<int>(49, cost.count() * 1000)));
         t_start_time = std::chrono::steady_clock::now();
 
         if (m_condit.is_alive()) {
             std::vector<int> have_cloud;
             DetectResult detect_result;
             JetStream::LabelYolo seg_results;
+            for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
+                vct_wheel_cloud[i]->clear();
+            }
             if (!LabelYoloDataBuffer::iter()->lock_exe(&LabelYoloDataBuffer::get, seg_results, 10)) {
                 continue;
             }
-            LOG(INFO) << seg_results.label() << " " << seg_results.boxes(0).lines_size();
+            int lable = TimeTool::timeDropoutLL(TimeTool::TemporalPrecision::TemporalPrecisionMs, TimeTool::TemporalPrecision::TemporalPrecisionHour);
+            LOG(INFO) << "after " << (lable - seg_results.label()) << "ms";
+            // 判断识别结果数量以及是否存图
+            int seg_num = 0;
+            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+                if (seg_results.boxes(device_index).confidence() > 0.7) {
+                    seg_num++;
+                }
+            }
+            if (seg_num == 1 && record_images.name_1.empty()) {
+                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_1, 10)) {
+                    record_images.name_1 = TimeTool::timeDropoutStr();
+                } else {
+                    record_images.name_1.clear();
+                }
+
+            }
+            if (seg_num == 2 && record_images.name_2.empty()) {
+                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_2, 10)) {
+                    record_images.name_2 = TimeTool::timeDropoutStr();
+                } else {
+                    record_images.name_2.clear();
+                }
+
+            }
+            if (seg_num == 3 && record_images.name_3.empty()) {
+                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_3, 10)) {
+                    record_images.name_3 = TimeTool::timeDropoutStr();
+                } else {
+                    record_images.name_3.clear();
+                }
+
+            }
+            if (seg_num == 4 && record_images.name_4.empty()) {
+                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::getIr, seg_results.label(), record_images.img_4, 10)) {
+                    record_images.name_4 = TimeTool::timeDropoutStr();
+                } else {
+                    record_images.name_4.clear();
+                }
+
+            }
+            // 取高置信度识别结果
             for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
-                if (seg_results.boxes(device_index).lines().empty()) {
-                    LOG(INFO) << device_index << " empty";
+                if (seg_results.boxes(device_index).confidence() < 0.9) {
+                    DLOG(INFO) << device_index << " empty";
                     continue;
                 }
                 cv::Mat pointMat;
-                LOG(INFO) << "b get data" << " empty";
+                //LOG(INFO) << "b get data" << " empty";
                 if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::get, device_index, seg_results.label(), pointMat, 10)) {
                     cv::imshow(std::to_string(device_index), pointMat);
                     cv::waitKey(1);
-                }
-                LOG(INFO) << "e get data" << " empty";
-                continue;
-                have_cloud.push_back(device_index);
-                for (int row = 0; row < 480; row++) {
-                    for (int col = seg_results.boxes(device_index).lines(row).begin(); col < seg_results.boxes(device_index).lines(row).end(); col++) {
-                        vct_wheel_cloud[device_index]->emplace_back(pointMat.at<cv::Vec3f>(row, col)[0],
-                                                                    pointMat.at<cv::Vec3f>(row, col)[1],
-                                                                    pointMat.at<cv::Vec3f>(row, col)[2]);
+                    have_cloud.push_back(device_index);
+                    for (int row = 0; row < 480; row++) {
+                        for (int col = seg_results.boxes(device_index).lines(row).begin(); col < seg_results.boxes(device_index).lines(row).end(); col++) {
+                            vct_wheel_cloud[device_index]->emplace_back(pointMat.at<cv::Vec3f>(row, col)[0] * 0.001,
+                                                                        pointMat.at<cv::Vec3f>(row, col)[1] * 0.001,
+                                                                        pointMat.at<cv::Vec3f>(row, col)[2] * 0.001);
+                        }
                     }
+                    pcl::transformPointCloud(*vct_wheel_cloud[device_index], *vct_wheel_cloud[device_index], rotation_matrix4[device_index]());
+                    // Point3D_tool::WriteTxtCloud(ETC_PATH PROJECT_NAME "/data/cloud_" + std::to_string(device_index) + ".txt", vct_wheel_cloud[device_index]);
                 }
             }
-            continue;
 
             // 点云处理
             if (!have_cloud.empty()) {
@@ -84,12 +150,6 @@ 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 ||
                         have_cloud.size() > 2) {
@@ -193,3 +253,18 @@ Error_manager DetectManager::carMoveStatus() {
     }
     return {};
 }
+
+Error_manager DetectManager::Stop() {
+    m_condit.kill_all();
+    m_thread->join();
+    delete m_thread;
+    m_thread = nullptr;
+    return {};
+}
+
+Error_manager DetectManager::Continue() {
+    m_condit.reset();
+    m_thread = new std::thread(&DetectManager::run, this);
+    m_condit.notify_all(true);
+    return {};
+}

detect/detect_manager.h

@@ -1,6 +1,8 @@
 #pragma once
 
 #include <thread>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
 
 #include "tool/error_code.hpp"
 #include "ceres_detect/detect_wheel_ceres.h"
@@ -38,6 +40,17 @@ public:
         }
     };
 
+    struct RecordImages {
+        std::string name_1;         // 图片名, 表示图片被保存时的时间;
+        std::string name_2;
+        std::string name_3;
+        std::string name_4;
+        cv::Mat img_1;              // 四和一图片, 第一张是第一次识别到一个识别结果情况下的图片
+        cv::Mat img_2;              // 四和一图片, 第二张是第一次识别到二个识别结果情况下的图片
+        cv::Mat img_3;              // 四和一图片, 第三张是第一次识别到三个识别结果情况下的图片
+        cv::Mat img_4;              // 四和一图片, 第四张是第一次识别到四个识别结果情况下的图片
+    };
+
 public:
     static DetectManager *iter() {
         static DetectManager *instance = nullptr;
@@ -49,14 +62,20 @@ public:
 
     ~DetectManager() = default;
 
-    Error_manager Init();
+    Error_manager Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices);
 
     Error_manager Release();
 
+    Error_manager Stop();
+
+    Error_manager Continue();
+
     MeasureStatu getMeasureResult(DetectResult &result);
 
     Error_manager carMoveStatus();
 protected:
+    Error_manager updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans);
+
     void run();
 
     void stop();
@@ -70,6 +89,7 @@ private:
 
     TimedLockData<bool> move_statu;
     TimedLockData<MeasureStatu> detect_statu;
+    TimedLockData<Eigen::Matrix4f> rotation_matrix4[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
     TimedLockData<DetectResult> m_detect_result_time_lock_data;
 };
 

proto/communication.proto

@@ -28,8 +28,199 @@ message CommunicationManagerConfig {
     optional string mqtt_config_file = 7 [default = "/mqtt.json"];   // 安装目录下的路径，并非绝对路径或者bin文件开始的相对路径
 }
 
+/*分配的车位信息*/
+message parkspace_info{
+    required int32 id=1;
+    required int32 serial_id=2;    //排序id
+    required int32 table_id=3;     //标签id
+    required int32 unit_id=4;      //单元号
+    required int32 floor=5;        //楼层号
+    required int32 room_id=6;      //同层编号
+    required float height=7;       //车高档位
+}
+
+//任务表单状态
+enum STATU{
+    eNormal=0;        // 正常
+    eWarning=1;       // 警告
+    eError=2;         // 错误
+    eCritical=3;      // 严重错误
+}
+
+//表单流程模式
+enum Table_process_mode
+{
+    PROCESS_NORMAL = 0;             // 0：正常模式， 检查节点会向收费系统发送请求，收费系统的答复 通过后，再向调度发送请求
+    PROCESS_ONLY_TO_DISPATCH = 1;   // 1：强制存取车，检查节点会向收费系统发送请求，忽略收费系统的答复，直接向调度发送请求。
+    PROCESS_ONLY_TO_PAY = 2;        // 2：虚拟存取车， 检查节点会向收费系统发送请求，忽略收费系统的答复。
+}
+
+/*
+表单执行状态
+ */
+message table_statu{
+    required STATU execute_statu=1;                    // 执行状态
+    required string statu_description=2;               // 状态描述
+    required Table_process_mode table_process_mod=3;   // 表单流程模式
+}
+/*
+号牌信息
+ */
+message plate_number_info
+{
+    required string plate_number = 1;        // 号牌
+    required string plate_color = 2;         // 号牌颜色
+    required string plate_type = 3;          // 号牌类型, 车辆类型
+    required int32  plate_confidence = 4;    // 号牌可信度, 1-100 值越高可信度越高
+    required string recognition_time = 5;    // 识别时间点, yyyyMMddHHmmss
+    required string plate_full_image = 6;    // 号牌全景图, base64编码
+    required string plate_clip_image = 7;    // 号牌特写图, base64编码
+}
+/*
+停车表单
+ */
+message park_table{
+    required table_statu statu=1;                      //表单状态
+    required int32 queue_id=2;                         //指令排队编号
+
+    required string car_number=3;
+    required int32 unit_id=4;
+    required int32 terminal_id=5;
+    required string primary_key=6;
+
+    required measure_buffer entrance_measure_info=7;     // 入口测量信息
+    required parkspace_info allocated_space_info=8;    // 分配的车位信息
+    required measure_buffer actually_measure_info=9;     // 实际测量信息或者叫二次测量信息
+    required parkspace_info actually_space_info=10;    // 实际停放的车位
+    required int32 import_id =11;                      // 入口id, 1~2
+
+    required plate_number_info car_number_info = 12;   // 车牌号信息
+}
+
+/*
+取车表单
+ */
+message pick_table{
+    required table_statu statu=1;      // 表单状态
+    required int32 queue_id=2;         // 指令排队编号
+
+    required string car_number=3;
+    required int32 unit_id=4;
+    required int32 terminal_id=5;
+    required string primary_key=6;
+
+    required parkspace_info actually_space_info=7;     // 实际停放的车位信息
+
+    required measure_buffer actually_measure_info=8;     // 存车时的实际测量信息（轴距）
+
+    required int32 export_id=9;                        // 出口id, 3~4
+    required bool  is_leaved=10;                       // 是否离开
+    required plate_number_info car_number_info = 11;   // 车牌号信息
+}
+
+/*
+以下是状态消息
+ */
+
+/*
+单片机节点状态
+ */
+message out_mcpu_statu{     //数值+1后
+    required int32 door_statu=1;       //外门状态       0无效, 1无效, 2开到位, 3 关到位, 4开关中, 5 故障
+    required int32 outside_safety=2;    //是否有车      0无效, 1无车, 2有车
+}
+
+message in_mcpu_statu{      //数值+1后
+    required int32 door_statu=1;       //外门状态       0无效, 1无效, 2开到位, 3 关到位, 4开关中, 5 故障
+    required int32 back_io=2;          //后超界       0无效, 1后超界, 2正常
+    required int32 is_occupy=3;        //是否有车      0无效, 1无车, 2有车
+    required int32 heighth=4;          //车高状态      0无效, 1无效, 2小车, 3中车, 4大车, 5故障, 6故障
+}
+/*
+测量节点状态
+ */
 message measure_buffer{
     required measure_info measure_info_to_plc_forward=1;     //雷达数据给plc，正向
     required measure_info measure_info_to_plc_reverse=2;     //雷达数据给plc，反向
     required measure_info measure_info_to_terminal=3;        //雷达数据给终端，边界较小
 }
+
+//搬运器状态枚举
+enum CarrierStatu{
+    eIdle=0;
+    eBusy=1;
+    eFault=2;
+}
+
+//plc出入口状态结构体
+message dispatch_plc_passway_status
+{
+    required int32			car_height = 1;			            //车高 0=无车 1=小车 2=中车 3=大车 4=超高车
+    required int32			outside_door_status = 2;	        //外门状态 0=无状态 1=开到位 2=关到位
+    required int32			inside_door_status = 3;	            //内门状态 0=无状态 1=开到位 2=关到位
+    required int32			comb_body_status = 4;	            //梳体状态 0=无状态 1=上到位 2=下到位, AB特有
+    required float			turnplate_angle_min = 5;	        //转盘角度最小值, C特有, 负值, 例如 -5度
+    required float			turnplate_angle_max = 6;	        //转盘角度最大值, C特有, 正值, 例如 +5度
+    required int32			sensor_1 = 7;				        //传感器状态的集合1
+                                                        //0 bit, 地感 0=无车 1=有车
+                                                        //1 bit, 移动检测 0=运动 1=静止
+                                                        //2 bit, 动态超限 0=遮挡 1=正常
+                                                        //3 bit, 后超界 0=遮挡 1=正常
+                                                        //4 bit, 前超界 0=遮挡 1=正常
+                                                        //5 bit, 左超界 0=遮挡 1=正常
+                                                        //6 bit, 右超界 0=遮挡 1=正常
+                                                        //7 bit, 车高小车， 0=遮挡 1=正常，AB单元为1480， C单元为1780，
+    required int32			sensor_2 = 8;				        //传感器状态的集合1
+                                                        //0 bit, 车高中车， 0=遮挡 1=正常，AB单元为1500， C单元为1800，
+                                                        //1 bit, 车高大车， 0=遮挡 1=正常，AB单元为2050， C单元为2050，
+                                                        //2 bit, 有车检测 0=无车 1=有车
+                                                        //3 bit, 车轮1检测 0=无车 1=有车, AB特有
+                                                        //4 bit, 车轮2检测 0=无车 1=有车, AB特有
+                                                        //5 bit, 预留
+                                                        //6 bit, 预留
+                                                        //7 bit, 预留
+    required int32           plc_passway_enable=9;               //出入口 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+}
+
+//厦门搬运器状态消息
+message dispatch_node_statu{
+    required CarrierStatu                            statu=1;
+    required int32                                   idle_stop_floor=2;          //空闲时停留位置
+    required park_table                              running_pack_info=3;        //正在执行的停车表单
+    required pick_table                              running_pick_info=4;        //正在执行的取车表单
+    required int32                                   unit_id = 5;                //单元号, 1~3
+
+    required int32                                   plc_heartbeat = 6;                      //plc心跳信息
+    required int32                                   plc_mode_status = 7;                    //plc状态的集合
+                                                                                    //0 bit, 手动模式, 维修模式
+                                                                                    //1 bit, 自动模式
+                                                                                    //2 bit, 自动运行中
+                                                                                    //3 bit, 复位
+                                                                                    //4 bit, 预留
+                                                                                    //5 bit, 预留
+                                                                                    //6 bit, 预留
+                                                                                    //7 bit, 预留
+    required int32                                   plc_passway_status = 8;                 //plc 出入口状态
+                                                                                    //0 bit, 入口1 可进车
+                                                                                    //1 bit, 入口1 维护
+                                                                                    //2 bit, 入口2 可进车
+                                                                                    //3 bit, 入口2 维护
+                                                                                    //4 bit, 出口1 可出车
+                                                                                    //5 bit, 出口1 维护
+                                                                                    //6 bit, 出口2 可出车
+                                                                                    //7 bit, 出口2 维护
+    required int32                                   plc_carrier_status = 9;                 //搬运器状态 0=故障 1=存车 2=取车 3=空闲 4=维护
+    required int32                                   plc_inlet_1_status = 10;                //入口1 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+    required int32                                   plc_inlet_2_status = 11;                //入口2 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+    required int32                                   plc_outlet_3_status = 12;               //出口3 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+    required int32                                   plc_outlet_4_status = 13;               //出口4 0=不可进车, 1=可进车, 2=维护不可进车, 3=维护可进车
+
+    repeated dispatch_plc_passway_status    dispatch_plc_passway_status_vector = 14; //plc出入口状态结构体,  数组下标0~1是入口, 数组下标2~3是出口
+
+}
+
+message terminal_node_statu{
+    required int32 terminal_id = 1;
+    required int32 import_id = 2;
+    required string car_number = 3;
+}

proto/def.grpc.proto

@@ -12,7 +12,8 @@ message Image{
   int32 width=1;
   int32 height=2;
   int32 channel=3;
-  bytes data=4;
+  bool encode = 4;
+  bytes data= 5;
 }
 message PointCloud{
   int32 size=1;
@@ -27,7 +28,8 @@ message Line{
   int32 end=2;
 }
 message SegBox{
-  repeated Line lines=1;
+  float confidence = 1;
+  repeated Line lines= 2;
 }
 message LabelYolo{
   int32 label=1;

vzense/device_tof3d.cpp

@@ -38,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());
@@ -342,6 +342,21 @@ Error_manager DeviceTof3D::ConnectDevice(const DeviceAzimuth &azimuth) {
     return {};
 }
 
+Error_manager DeviceTof3D::StartStreamDevice(VzDeviceHandle handle) {
+    if (handle == nullptr) {
+        return {ERROR, CRITICAL_ERROR};
+    }
+
+    auto t_vz_status = VZ_StartStream(handle);
+    if (t_vz_status != VzReturnStatus::VzRetOK) {
+        LOG(WARNING) << "VZ_StartStream failed status:" << t_vz_status;
+    } else {
+        LOG(INFO) << "VZ_StartStream success status:" << t_vz_status;
+    }
+
+    return {};
+}
+
 Error_manager DeviceTof3D::DisConnectDevice(VzDeviceHandle handle) {
     if (handle == nullptr) {
         return {ERROR, CRITICAL_ERROR};
@@ -364,6 +379,21 @@ Error_manager DeviceTof3D::DisConnectDevice(VzDeviceHandle handle) {
     return {};
 }
 
+Error_manager DeviceTof3D::CloseStreamDevice(VzDeviceHandle handle) {
+    if (handle == nullptr) {
+        return {ERROR, CRITICAL_ERROR};
+    }
+
+    auto t_vz_status = VZ_StopStream(handle);
+    if (t_vz_status != VzReturnStatus::VzRetOK) {
+        LOG(WARNING) << "VZ_StopStream failed status:" << t_vz_status;
+    } else {
+        LOG(INFO) << "VZ_StopStream success status:" << t_vz_status;
+    }
+
+    return {};
+}
+
 Error_manager DeviceTof3D::getDepthAndIrPicture(VzDeviceHandle handle, VzFrame &depthFrame, VzFrame &irFrame) {
     if (handle == nullptr) {
         return {ERROR, CRITICAL_ERROR};
@@ -498,7 +528,7 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
     cv::Mat t_point_img;
     VzFrame depthFrame = {0};
     VzFrame irFrame = {0};
-    VzVector3f *worldV = new VzVector3f[480 * 640];
+    VzVector3f worldV[480 * 640];// = new VzVector3f;
 
     auto t_start_time = std::chrono::steady_clock::now();
     std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
@@ -531,35 +561,11 @@ void DeviceTof3D::run(const DeviceAzimuth &azimuth) {
             if (vz_ret == SUCCESS) {
                 // 灰度图转换
                 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);
-//                    }
-//                }
-
                 memcpy(depthInfo.pointMat.data, worldV, 480 * 640 * sizeof(VzVector3f));
-
                 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.";
+            LOG_EVERY_N(WARNING, 1000) << device.etc->ip() << " thread was killed.";
         }
     }
 
@@ -577,3 +583,30 @@ void DeviceTof3D::stop() {
         device.t = nullptr;
     }
 }
+
+Error_manager DeviceTof3D::Stop() {
+    for (auto &device: m_devices_list) {
+        if (device.condit->is_alive()) {
+            device.condit->kill_all();
+            device.t->join();
+            delete device.t;
+            device.t = nullptr;
+            CloseStreamDevice(device.handle);
+        }
+    }
+    return {};
+}
+
+Error_manager DeviceTof3D::Continue() {
+    for (auto &device: m_devices_list) {
+        if (device.condit->is_alive()) {
+            continue;
+        }
+        StartStreamDevice(device.handle);
+        device.condit->reset();
+        device.t = new std::thread(&DeviceTof3D::run, this, device.etc->azimuth());
+        device.condit->notify_all(true);
+    }
+
+    return {};
+}

vzense/device_tof3d.h

@@ -54,6 +54,10 @@ public:
 
     Error_manager Release();
 
+    Error_manager Stop();
+
+    Error_manager Continue();
+
     Error_manager updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans);
 
     cv::Mat getDeviceIrMat();
@@ -75,8 +79,12 @@ protected:
 
     Error_manager ConnectDevice(const DeviceAzimuth &azimuth);
 
+    Error_manager StartStreamDevice(VzDeviceHandle handle);
+
     Error_manager DisConnectDevice(VzDeviceHandle handle);
 
+    Error_manager CloseStreamDevice(VzDeviceHandle handle);
+
     Error_manager getDepthAndIrPicture(VzDeviceHandle handle, VzFrame &depthFrame, VzFrame &irFrame);
 
     static Error_manager DepthFrame2Mat(VzFrame &frame, cv::Mat &mat);