---

CMakeLists.txt

@@ -51,7 +51,7 @@ if (OPTION_ENABLE_TENSORRT_DETECT_CODE)
             ${TensorRT_LIBRARIES}
     )
 
-    aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect/tensorrt_onnx yolov8_detect)
+    aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect yolov8_detect)
 else ()
     aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect/onnx_detect yolov8_detect)
 endif ()
@@ -81,17 +81,19 @@ include_directories(
         ${NebulaSDK_PATH}/include
 )
 
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/proto proto)
 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)
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect/ceres_detect ceres_detect)
 
 set(SON_PROJECT_SOURCE_LIST ${SON_PROJECT_SOURCE_LIST}
+        ${proto}
         ${vzense}
         ${communication}
         ${communication_grpc}

communication/communication.proto

@@ -1,76 +0,0 @@
-syntax = "proto2";
-
-enum Tof3dFunctional {
-  SearchDevice = 1;
-  ConnectDevice = 2;
-  ConnectAllEtcDevices = 3;
-  ConnectAllSearchDevices = 4;
-  DisConnectDevice = 5;
-  DisConnectAllEtcDevices = 6;
-  DisConnectAllSearchDevices = 7;
-  getDepthFrame = 8;
-  getIrFrame = 9;
-  getDepthAndIrPicture = 10;
-  getDepthPointCloud = 11;
-  DepthFrame2PointCloud = 12;
-  Frame2Mat = 13;
-}
-
-enum Tof3dVzFrameType {
-  Tof3dVzDepthFrame = 0;
-  Tof3dVzIRFrame = 1;
-}
-
-enum Tof3dVzPixelFormat {
-  Tof3dVzPixelFormatDepthMM16 = 0;
-  Tof3dVzPixelFormatGray8 = 2;
-}
-
-message Tof3dVzFrame {
-  required uint32 frameIndex = 1;
-  required Tof3dVzFrameType frameType = 2;
-  required Tof3dVzPixelFormat pixelFormat = 3;
-  repeated uint32 pFrameData = 4;
-  required uint32 dataLen = 5;
-  required float exposureTime = 6;
-  required uint32 depthRange = 7;
-  required uint32 width = 8;
-  required uint32 height = 9;
-  required uint64 deviceTimestamp = 10;
-}
-
-message Tof3dVzVector3f {
-  required int32 x = 1;
-  required int32 y = 2;
-  required int32 z = 3;
-}
-
-message Tof3dMat {
-  repeated uint32 data = 1;
-  required int32 size = 2;
-  required int32 rows = 3;
-  required int32 cols = 4;
-  required uint32 type = 5;
-  required uint64 stamp = 10;
-}
-
-message Tof3dFunctionalParams {
-  optional int64 error_code = 1;
-  optional int64 error_level = 2;
-  optional string error_string = 3;
-  optional string ip = 4;
-  optional double search_time = 5;
-  optional bool open_stream = 6;
-  optional Tof3dVzFrame depthFrame = 7;
-  optional Tof3dVzFrame irFrame = 8;
-  repeated Tof3dVzVector3f depthCloud = 9;
-  repeated Tof3dVzVector3f irCloud = 10;
-  optional Tof3dMat depthMat = 11;
-  optional Tof3dMat irMat = 12;
-}
-
-message Tof3dCommunicatonProtocol
-{
-  required Tof3dFunctional func = 1;
-  required Tof3dFunctionalParams func_params = 2;
-}

communication/communication_manager.cpp

@@ -3,3 +3,17 @@
 //
 
 #include "communication_manager.h"
+
+Error_manager CommunicationManager::Init(const CommunicationManagerConfig &config) {
+
+    if (config.grpc_enable()) {
+        m_grpc_server = new StreamRpcServer;
+        m_grpc_server->Listenning(config.grpc_server_ip(), 9876);
+    }
+
+    if (config.rabbitmq_enable()) {
+        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();
+}

communication/communication_manager.h

@@ -1,14 +1,25 @@
-//
-// Created by zx on 2023/11/24.
-//
+#include "error_code/error_code.hpp"
+#include "rabbitmq/rabbitmq_communication.h"
+#include "grpc/streamServer.h"
+#include "proto/communication.pb.h"
 
-#ifndef ALLPROJECT_COMMUNICATION_MANAGER_H
-#define ALLPROJECT_COMMUNICATION_MANAGER_H
+class CommunicationManager {
+public:
+    static CommunicationManager *iter() {
+        static CommunicationManager *instance = nullptr;
+        if (instance == nullptr) {
+            instance = new CommunicationManager();
+        }
+        return instance;
+    }
+    ~CommunicationManager() = default;
 
+    Error_manager Init(const CommunicationManagerConfig &config);
+protected:
 
-class communication_manager {
+private:
+    StreamRpcServer *m_grpc_server = nullptr;
 
 };
 
 
-#endif //ALLPROJECT_COMMUNICATION_MANAGER_H

communication/grpc/streamServer.cpp

@@ -1,56 +1,44 @@
 
 #include "streamServer.h"
 #include <unistd.h>
-CustomServer::CustomServer(){
-    update_=false;
-}
-
-CustomServer::~CustomServer(){
-    openDataStream_=false;
-    openImageStream_=true;
-}
-
+#include "communication/transitData.h"
 
-void CustomServer::update(const ::JetStream::ResFrame& frame){
-    frame_.CopyFrom(frame);
-    update_=true;
+CustomServer::CustomServer() {
+    update_ = false;
 }
 
-bool CustomServer::waitForUpdata(float tm){
-    update_=false;
-    //等待最新数据
-    auto tp=std::chrono::steady_clock::now();
-    double time = 0;
-    while(time<tm && update_==false ){
-        std::this_thread::sleep_for(std::chrono::milliseconds(10));
-        auto now=std::chrono::steady_clock::now();
-        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - tp);
-        time = double(duration.count()) * std::chrono::milliseconds::period::num / 
-                std::chrono::milliseconds::period::den;
-    }
-    if(update_==false){
-        printf("在%fs内没有等待最新数据\n",tm);
-        return false;
-    }
-    return true;
-
+CustomServer::~CustomServer() {
+    openDataStream_ = false;
+    openImageStream_ = true;
 }
 
 
-::grpc::Status CustomServer::OpenMeasureDataStream(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::grpc::ServerWriter< ::JetStream::MeasureInfo>* writer){
-    openDataStream_=true;
+::grpc::Status
+CustomServer::OpenMeasureDataStream(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                    ::grpc::ServerWriter<::JetStream::MeasureInfo> *writer) {
+    openDataStream_ = true;
     printf("打开测量数据流\n");
-    while(openDataStream_){
-        if(waitForUpdata()){
-            writer->Write(frame_.measure_info());
-        }else{
-            ::JetStream::MeasureInfo info;
-            info.set_error("没有数据");
-            writer->Write(info);
+    while (openDataStream_) {
+        TransitData::MeasureInfo info;
+        if (TransitData::get_instance_pointer()->getMeasureInfo(info, 0.3)) {
+            ::JetStream::MeasureInfo jetInfo;
+            jetInfo.set_x(info.x);
+            jetInfo.set_y(info.y);
+            jetInfo.set_trans_x(info.transx);
+            jetInfo.set_trans_y(info.transy);
+            jetInfo.set_theta(info.theta);
+            jetInfo.set_width(info.width);
+            jetInfo.set_wheelbase(info.wheelbase);
+            jetInfo.set_ftheta(info.ftheta);
+            jetInfo.set_border_plc(info.border_plc);
+            jetInfo.set_border_display(info.border_display);
+            jetInfo.set_error(info.error);
+
+            writer->Write(jetInfo);
+
         }
 
-        if(context->IsCancelled()){
+        if (context->IsCancelled()) {
             printf("Data Stream canceled\n");
             return ::grpc::Status::OK;
         }
@@ -61,23 +49,33 @@ bool CustomServer::waitForUpdata(float tm){
 }
 
 
-::grpc::Status CustomServer::OpenImageStream(::grpc::ServerContext* context, 
-            const ::JetStream::RequestCmd* request, ::grpc::ServerWriter< ::JetStream::ResImage>* writer){
+::grpc::Status CustomServer::OpenImageStream(::grpc::ServerContext *context,
+                                             const ::JetStream::RequestCmd *request,
+                                             ::grpc::ServerWriter<::JetStream::ResImage> *writer) {
 
-    openImageStream_=true;
+    openImageStream_ = true;
     printf("打开视频流\n");
-    while(openImageStream_){
-        if(update_){
-        
-            writer->Write(frame_.images());
-            update_=false;
+
+    while (openImageStream_) {
+        ::JetStream::ResImage jetIamages;
+        bool ret[4] = {0};
+        cv::Mat image[4];
+        for (int i = 0; i < 4; i++) {
+            ret[i] = TransitData::get_instance_pointer()->getImage(image[i], i, 0.3);
         }
-        if(context->IsCancelled()){
+        if (ret[0]) jetIamages.mutable_img1()->CopyFrom(cvMat2JetImage(image[0]));
+        if (ret[1]) jetIamages.mutable_img2()->CopyFrom(cvMat2JetImage(image[1]));
+        if (ret[2]) jetIamages.mutable_img3()->CopyFrom(cvMat2JetImage(image[2]));
+        if (ret[3]) jetIamages.mutable_img4()->CopyFrom(cvMat2JetImage(image[3]));
+
+        writer->Write(jetIamages);
+
+        if (context->IsCancelled()) {
             printf(" canceled\n");
             return ::grpc::Status::OK;
         }
         std::this_thread::sleep_for(std::chrono::milliseconds(33));
-        
+
     }
     printf("关闭视频流\n");
     return ::grpc::Status::OK;
@@ -85,168 +83,195 @@ bool CustomServer::waitForUpdata(float tm){
 }
 
 
-::grpc::Status CustomServer::CloseImageStream(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-        ::JetStream::Response* response){
-    openImageStream_=false;
+::grpc::Status CustomServer::CloseImageStream(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                              ::JetStream::Response *response) {
+    openImageStream_ = false;
     return ::grpc::Status::OK;
-    
-}
-::grpc::Status CustomServer::CloseMeasureDataStream(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-        ::JetStream::Response* response){
-    openDataStream_=false;
-    return ::grpc::Status::OK;
-    
-}
 
-
-::grpc::Status CustomServer::Detect(::grpc::ServerContext* context, 
-            const ::JetStream::RequestCmd* request, ::JetStream::ResFrame* response){
-    if(waitForUpdata()){
-        response->CopyFrom(frame_);
-        return ::grpc::Status::OK;
-    }else{
-        return ::grpc::Status(::grpc::StatusCode::DATA_LOSS,"0.3s内没有等到最新数据");
-    }
-    
 }
 
-::grpc::Status CustomServer::GetCloud(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::JetStream::ResCloud* response){
-    if(waitForUpdata()==false){
-        return ::grpc::Status(::grpc::StatusCode::DATA_LOSS,"0.3s内没有等到最新数据");
-    }
-    int id=request->id();
-    switch (id)
-    {
-    case 1:response->mutable_cloud1()->CopyFrom(frame_.clouds().cloud1());break;
-    case 2: response->mutable_cloud2()->CopyFrom(frame_.clouds().cloud2());break;
-    case 3: response->mutable_cloud3()->CopyFrom(frame_.clouds().cloud3());break;
-    case 4: response->mutable_cloud4()->CopyFrom(frame_.clouds().cloud4());break;
-    default:
-        response->CopyFrom(frame_.clouds());
-        break;
-    }
+::grpc::Status
+CustomServer::CloseMeasureDataStream(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                     ::JetStream::Response *response) {
+    openDataStream_ = false;
     return ::grpc::Status::OK;
 
 }
 
-::grpc::Status CustomServer::GetImage(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::JetStream::ResImage* response){
-    if(waitForUpdata()==false){
-        return ::grpc::Status(::grpc::StatusCode::DATA_LOSS,"0.3s内没有等到最新数据");
-    }
-    int id=request->id();
-    switch (id)
-    {
-    case 1:response->mutable_img1()->CopyFrom(frame_.images().img1());break;
-    case 2: response->mutable_img2()->CopyFrom(frame_.images().img2());break;
-    case 3: response->mutable_img3()->CopyFrom(frame_.images().img3());break;
-    case 4: response->mutable_img4()->CopyFrom(frame_.images().img4());break;
-    default:
-        response->CopyFrom(frame_.images());
-        break;
-    }
-    return ::grpc::Status::OK;
-}
 
+::grpc::Status CustomServer::GetCloud(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                      ::JetStream::ResCloud *response) {
 
+    int id = request->id();
 
-StreamRpcServer::StreamRpcServer(){
-    thread_=nullptr;
-}
-StreamRpcServer::~StreamRpcServer(){
-    
-    if(thread_!= nullptr){
-        server_->Shutdown();
-        if(thread_->joinable()){
-            thread_->join();
-        delete thread_;
+    std::vector<cv::Point> mask;
+    cv::Mat imageCL;
+
+    ::JetStream::ResCloud jetClouds;
+    ::JetStream::PointCloud cloud;
+    if (TransitData::get_instance_pointer()->getImageCL(imageCL, id, 0.3)) {
+
+//        for (int cols = 0; cols < 640 - 0; cols++) {
+//            for (int rows = 0; rows < 480 - 0; rows++) {
+//                imageCL.at<cv::Vec4f>(rows, cols)[0] > 5. ? printf("%.f\n", imageCL.at<cv::Vec4f>(rows, cols)[0]) : false;
+//            }
+//        }
+
+        int size = imageCL.rows * imageCL.cols;
+        char *data = (char *) malloc(size * 4 * sizeof(short));
+
+        memset(data, 0, size * 4 * sizeof(short));
+        if (TransitData::get_instance_pointer()->getMask(mask, id, 0.3)) {
+            printf("%d %d\n", mask[0].x, mask[0].y);
+            for (auto i : mask) {
+                imageCL.at<cv::Vec4f>(i)[3] = 0.95;
+            }
         }
+
+        int count = 0;
+        cv::Mat mat = cv::Mat::zeros(480, 640, CV_8U);
+
+        for (int i = 0; i < imageCL.rows; ++i) {
+            for (int j = 0; j < imageCL.cols; ++j) {
+                cv::Vec4f pt = imageCL.at<cv::Vec4f>(i, j);
+                if (fabs(pt[0] < 1e-6) && fabs(pt[1]) < 1e-6 && fabs(pt[2]) < 1e-6)
+                    continue;
+                else {
+                    short xyz[4] = {0};
+                    xyz[0] = short(pt[0] / 5. * 32750);
+                    xyz[1] = short(pt[1] / 5. * 32750);
+                    xyz[2] = short(pt[2] / 5. * 32750);
+                    xyz[3] = short(pt[3] / 5. * 32750.);
+                    if (pt[3] == 1.0) {
+                        mat.at<uchar>(i ,j) = 255;
+//                        printf("%d %d %d %d %d %d\n", xyz[0], xyz[1], xyz[2], xyz[3], i, j);
+                    }
+//                        if (fabs(pt[0]) > 5 || fabs(pt[1]) > 5 || fabs(pt[2]) > 5 || fabs(pt[3]) > 5) {
+//                            std::cout << pt[0] << " " << pt[1] << " " << pt[2] << " " << pt[3] << "\n";
+//                        }
+                    memcpy(data + count * 4 * sizeof(short), xyz, 4 * sizeof(short));
+                    count++;
+
+                }
+            }
+        }
+//        cv::imwrite("/home/zx/sss.jpeg", mat);
+//        cv::imshow("22222", mat);
+//        cv::waitKey(0);
+        cloud.set_size(count);
+        cloud.set_data(data, count * 4 * sizeof(short));
+        free(data);
+
+        switch (id + 1) {
+            case 1:
+                jetClouds.mutable_cloud1()->CopyFrom(cloud);
+                break;
+            case 2:
+                jetClouds.mutable_cloud2()->CopyFrom(cloud);
+                break;
+            case 3:
+                jetClouds.mutable_cloud3()->CopyFrom(cloud);
+                break;
+            case 4:
+                jetClouds.mutable_cloud4()->CopyFrom(cloud);
+                break;
+            default:
+                break;
+        }
+        response->CopyFrom(jetClouds);
+        return ::grpc::Status::OK;
+    } else {
+        return ::grpc::Status(::grpc::StatusCode::DATA_LOSS, "0.3s内没有等到最新数据");
     }
-}
-void StreamRpcServer::Listenning(std::string ip,int port){
-    char ipport[64]={0};
-    sprintf(ipport,"%s:%d",ip.c_str(),port);
-    builder_.AddListeningPort(ipport, grpc::InsecureServerCredentials());
-    builder_.RegisterService(&service_);
 
-    server_=builder_.BuildAndStart();
-    thread_=new std::thread(&StreamRpcServer::WaitThread,this);
-}
 
-void StreamRpcServer::WaitThread(void* p){
-    StreamRpcServer* server=(StreamRpcServer*)p;
-    server->server_->Wait();
 }
 
-void StreamRpcServer::ResetData(const ::JetStream::ResFrame& frame){
-    service_.update(frame);
+::grpc::Status CustomServer::GetImage(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                      ::JetStream::ResImage *response) {
+    int id = request->id();
+    cv::Mat image;
+    ::JetStream::Image jetImgs[4];
+    if (id == -1) {
+        for (int i = 0; i < 4; ++i) {
+            if (TransitData::get_instance_pointer()->getImage(image, i, 0.3)) {
+                jetImgs[i].CopyFrom(cvMat2JetImage(image));
+            }
+        }
+        response->mutable_img1()->CopyFrom(jetImgs[0]);
+        response->mutable_img2()->CopyFrom(jetImgs[1]);
+        response->mutable_img3()->CopyFrom(jetImgs[2]);
+        response->mutable_img4()->CopyFrom(jetImgs[3]);
+        return ::grpc::Status::OK;
+    }
+    ::JetStream::Image jetImage;
+
+    if (TransitData::get_instance_pointer()->getImage(image, id, 0.3)) {
+        jetImage.CopyFrom(cvMat2JetImage(image));
+        switch (id) {
+            case 0:
+                response->mutable_img1()->CopyFrom(jetImage);
+                break;
+            case 1:
+                response->mutable_img2()->CopyFrom(jetImage);
+                break;
+            case 2:
+                response->mutable_img3()->CopyFrom(jetImage);
+                break;
+            case 3:
+                response->mutable_img4()->CopyFrom(jetImage);
+                break;
+            default:
+                break;
+        }
+        return ::grpc::Status::OK;
+    } else {
+        return ::grpc::Status(::grpc::StatusCode::DATA_LOSS, "0.3s内没有等到最新数据");
+    }
+
 }
 
 
-::JetStream::Image StreamRpcServer::cvMat2JetImage(cv::Mat& image){
+::JetStream::Image CustomServer::cvMat2JetImage(cv::Mat &image) {
     ::JetStream::Image img;
     img.set_width(image.cols);
     img.set_height(image.rows);
     img.set_channel(image.channels());
-    img.set_data(image.data,image.cols*image.rows*image.channels());
+    img.set_data(image.data, image.cols * image.rows * image.channels());
     return img;
 }
 
-::JetStream::PointCloud StreamRpcServer::pcl2JetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr pcl){
-
-    int size=pcl->size();
-    char* data=(char*)malloc(size*4*sizeof(short));
-    memset(data,0,size*4*sizeof(short));
-    for(int i=0;i<size;++i){
-        if(fabs(pcl->points[i].x)>=5 || fabs(pcl->points[i].y)>=5
-            ||fabs(pcl->points[i].z)>=5)
-            continue;
-        short xyz[4]={0};
-        xyz[0]=short(pcl->points[i].x/5.*32750);
-        xyz[1]=short(pcl->points[i].y/5.*32750);
-        xyz[2]=short(pcl->points[i].z/5.*32750);
-        xyz[3]=short(pcl->points[i].intensity/5.*32750.);
-        memcpy(data+i*4*sizeof(short),xyz,4*sizeof(short));
 
-    }
-
-    ::JetStream::PointCloud cloud;
-    cloud.set_size(pcl->size());
-    cloud.set_data(data,size*4*sizeof(short));
-    free(data);
-
-    return cloud;
+StreamRpcServer::StreamRpcServer() {
+    thread_ = nullptr;
 }
 
-::JetStream::PointCloud StreamRpcServer::cvPointMat2JetPointCloud(cv::Mat &image) {
-    std::vector<short> point_data;
-    for (int rows = 0; rows < image.rows; rows++) {
-        for (int cols = 0; cols < image.cols; cols++) {
-            if (fabs(image.at<cv::Vec4f>(rows, cols)[0]) > 5 ||
-                fabs(image.at<cv::Vec4f>(rows, cols)[1]) > 5 ||
-                fabs(image.at<cv::Vec4f>(rows, cols)[2]) > 5
-                ) { continue;}
-            if (fabs(image.at<cv::Vec4f>(rows, cols)[0]) == 0 &&
-                fabs(image.at<cv::Vec4f>(rows, cols)[1]) == 0 &&
-                fabs(image.at<cv::Vec4f>(rows, cols)[2]) == 0
-                ) {continue;}
-            point_data.emplace_back(short(image.at<cv::Vec4f>(rows, cols)[0]/5.*32750));
-            point_data.emplace_back(short(image.at<cv::Vec4f>(rows, cols)[1]/5.*32750));
-            point_data.emplace_back(short(image.at<cv::Vec4f>(rows, cols)[2]/5.*32750));
-            point_data.emplace_back(short(image.at<cv::Vec4f>(rows, cols)[3]/5.*32750));
+StreamRpcServer::~StreamRpcServer() {
+
+    if (thread_ != nullptr) {
+        server_->Shutdown();
+        if (thread_->joinable()) {
+            thread_->join();
+            delete thread_;
         }
     }
+}
 
-    int size=point_data.size()/4;
-    char* data=(char*)malloc(size*4*sizeof(short));
-    memset(data,0,size*4*sizeof(short));
-    memcpy(data, point_data.data(), size*4*sizeof(short));
+void StreamRpcServer::Listenning(std::string ip, int port) {
+    char ipport[64] = {0};
+    printf("listenning grpc %s ", ip.c_str());
+    sprintf(ipport, "%s:%d", ip.c_str(), port);
+    builder_.AddListeningPort(ipport, grpc::InsecureServerCredentials());
+    builder_.RegisterService(&service_);
 
-    ::JetStream::PointCloud cloud;
-    cloud.set_size(size);
-    cloud.set_data(data,size*4*sizeof(short));
-    free(data);
+    server_ = builder_.BuildAndStart();
+    thread_ = new std::thread(&StreamRpcServer::WaitThread, this);
+}
 
-    return cloud;
+void StreamRpcServer::WaitThread(void *p) {
+    StreamRpcServer *server = (StreamRpcServer *) p;
+    printf(" listenning ........\n");
+    server->server_->Wait();
+    printf(" server closesd.....\n");
 }
+

communication/grpc/streamServer.h

@@ -1,65 +1,68 @@
 #ifndef __SERVER__GRPC__STREAM__HH_H_
 #define __SERVER__GRPC__STREAM__HH_H_
+
 #include "opencv2/opencv.hpp"
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <thread>
 #include <grpcpp/grpcpp.h>
-#include "def.grpc.grpc.pb.h"
-#include "def.grpc.pb.h"
+#include "proto/def.grpc.grpc.pb.h"
+#include "proto/def.grpc.pb.h"
 
-class CustomServer :public JetStream::StreamServer::Service
-{
+class CustomServer : public JetStream::StreamServer::Service {
 private:
-    
-    bool openDataStream_=false;
-    bool openImageStream_=false;
-    ::JetStream::ResFrame frame_;
+
+    bool openDataStream_ = false;
+    bool openImageStream_ = false;
     bool update_;
 public:
     CustomServer();
+
     ~CustomServer();
-    void update(const ::JetStream::ResFrame& frame);
-    ::grpc::Status OpenImageStream(::grpc::ServerContext* context, 
-            const ::JetStream::RequestCmd* request, ::grpc::ServerWriter< ::JetStream::ResImage>* writer);
-    virtual ::grpc::Status OpenMeasureDataStream(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::grpc::ServerWriter< ::JetStream::MeasureInfo>* writer);
-    virtual ::grpc::Status Detect(::grpc::ServerContext* context, 
-            const ::JetStream::RequestCmd* request, ::JetStream::ResFrame* response);
-    virtual ::grpc::Status CloseImageStream(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::JetStream::Response* response);
-    virtual ::grpc::Status CloseMeasureDataStream(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::JetStream::Response* response);
-    virtual ::grpc::Status GetCloud(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::JetStream::ResCloud* response);
-    virtual ::grpc::Status GetImage(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
-            ::JetStream::ResImage* response);
+
+    ::grpc::Status OpenImageStream(::grpc::ServerContext *context,
+                                   const ::JetStream::RequestCmd *request,
+                                   ::grpc::ServerWriter<::JetStream::ResImage> *writer);
+
+    virtual ::grpc::Status OpenMeasureDataStream(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                                 ::grpc::ServerWriter<::JetStream::MeasureInfo> *writer);
+
+    virtual ::grpc::Status CloseImageStream(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                            ::JetStream::Response *response);
+
+    virtual ::grpc::Status
+    CloseMeasureDataStream(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                           ::JetStream::Response *response);
+
+    virtual ::grpc::Status GetCloud(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                    ::JetStream::ResCloud *response);
+
+    virtual ::grpc::Status GetImage(::grpc::ServerContext *context, const ::JetStream::RequestCmd *request,
+                                    ::JetStream::ResImage *response);
 
 protected:
-    bool waitForUpdata(float tm = 1);
+    static ::JetStream::Image cvMat2JetImage(cv::Mat &image);
 };
 
 
-class StreamRpcServer{
+class StreamRpcServer {
 
 private:
     grpc::ServerBuilder builder_;
     CustomServer service_;
     std::unique_ptr<grpc::Server> server_;
-    std::thread* thread_;
-    
+    std::thread *thread_;
+
 public:
     StreamRpcServer();
+
     ~StreamRpcServer();
-    void Listenning(std::string ip,int port);
-    void ResetData(const ::JetStream::ResFrame& frame);
 
-    static ::JetStream::Image cvMat2JetImage(cv::Mat& image);
-    static ::JetStream::PointCloud pcl2JetPointCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr pcl);
-    static ::JetStream::PointCloud cvPointMat2JetPointCloud(cv::Mat& image);
+    void Listenning(std::string ip, int port);
+
 
 protected:
-    static void WaitThread(void* p);
+    static void WaitThread(void *p);
 };
 
-#endif
+#endif

communication/rabbitmq/Boundary.cpp

@@ -0,0 +1,151 @@
+//
+// Created by zx on 23-11-27.
+//
+
+#include "Boundary.h"
+#include <pcl/point_types.h>
+#include <fcntl.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+bool Boundary::read_proto_param(std::string prototxt_path, ::google::protobuf::Message &protobuf_parameter) {
+    int fd = open(prototxt_path.c_str(), O_RDONLY);
+    if (fd == -1) return false;
+    FileInputStream *input = new FileInputStream(fd);
+    bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
+    delete input;
+    close(fd);
+    return success;
+}
+
+bool Boundary::init(const std::string &file) {
+    return read_proto_param(file, limit_);
+}
+
+bool Boundary::init(JetStream::Limit limit) {
+    limit_ = limit;
+    return true;
+}
+
+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;
+    //计算四轮位置
+    pcl::PointXYZ pt[4];
+    pt[0] = pcl::PointXYZ(-info.width() / 2.0, info.wheelbase() / 2.0, 0);
+    pt[1] = pcl::PointXYZ(info.width() / 2.0, info.wheelbase() / 2.0, 0);
+    pt[2] = pcl::PointXYZ(-info.width() / 2.0, -info.wheelbase() / 2.0, 0);
+    pt[3] = pcl::PointXYZ(info.width() / 2.0, -info.wheelbase() / 2.0, 0);
+    for (int i = 0; i < 4; ++i) {
+        //计算四轮位置
+        float x = pt[i].x * cos(info.theta()) - pt[i].y * sin(info.theta()) + info.x();
+        float y = pt[i].x * sin(info.theta()) + pt[i].y * cos(info.theta()) + info.y();
+        //判断是否在盖板上
+        // 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;
+            } else {
+                if (x > 0) return eRight;
+                else return eLeft;
+            }
+        }
+        //判断左右
+        if (x < bound.left()) return eLeft;
+        if (x > bound.right()) return eRight;
+    }
+    //判断摆正后，后轮是否能够的着
+    if (info.trans_y() - 0.5 * info.wheelbase() <= bound.buttom()) return eBack;
+
+    if (fabs(info.ftheta()) > bound.fwheelangle()) return eWheelAngle;
+
+    return eNormal;
+
+}
+
+void Boundary::limit(JetStream::MeasureInfo &info) {
+
+    float trans_x = info.x() * cos(info.theta()) - info.y() * sin(info.theta());
+    float trans_y = info.x() * sin(info.theta()) + info.y() * cos(info.theta());
+    info.set_trans_x(trans_x);
+    info.set_trans_y(trans_y);
+    //判断轴距和宽度是否超
+    if (info.width() >= limit_.maxwidth()) {
+        info.set_border_plc(int(eWidth));
+        info.set_border_display(int(eWidth));
+        return;
+    }
+    if (info.wheelbase() >= limit_.maxwheelbase()) {
+        info.set_border_plc(int(eWheelBase));
+        info.set_border_display(int(eWheelBase));
+        return;
+    }
+
+    info.set_border_plc(int(limit_boundary(info, limit_.plc_limit())));
+    info.set_border_display(int(limit_boundary(info, limit_.display_limit())));
+
+}
+
+void Boundary::transMeasureInfo(JetStream::MeasureInfo &in, measure_info &out) {
+    // 如果地面雷达状态已经被修改过，直接返回
+    if (out.ground_status() != Measure_OK) {
+        return;
+    }
+
+    // 设备状态
+    if (in.border_display() != Range_status::Range_correct || in.border_plc() != Range_status::Range_correct) {
+        out.set_ground_status((MeasureStatu::Measure_Border));
+    }
+
+    out.set_cx(in.trans_x());                                 // 车辆中心坐标x
+    out.set_cy(in.trans_y());
+    out.set_theta(in.theta() * 180.0 / M_PI);
+    out.set_length(0);
+    out.set_width(in.width());
+    out.set_wheelbase(in.wheelbase());
+    out.set_front_theta(in.ftheta() * 180.0 / M_PI);
+    out.set_border_statu(in.border_plc());
+    out.set_is_stop(0);
+
+    if (DetectManager::iter()->carMoveStatus() == FAILED) {
+        out.set_motion_statu(1);    // 静止3秒
+    } else {
+        out.set_motion_statu(0);    // 运动
+    }
+    out.set_move_distance(0);
+}
+
+void Boundary::transMeasureInfo(JetStream::MeasureInfo &in, TransitData::MeasureInfo &out) {
+    out.x = in.x();
+    out.y = in.y();
+    out.transx = in.trans_x();
+    out.transy = in.trans_y();
+    out.theta = in.theta();
+    out.width = in.width();
+    out.wheelbase = in.wheelbase();
+    out.ftheta = in.ftheta();
+    out.border_plc = in.border_plc();
+    out.border_display = in.border_display();
+    out.error = in.error();
+}
+
+void Boundary::transMeasureInfo(DetectManager::DetectResult &in, JetStream::MeasureInfo &out) {
+    out.set_x(in.car.wheels_center_x);
+    out.set_y(in.car.wheels_center_y);
+    out.set_theta(in.car.theta);
+    out.set_width(in.car.wheel_width);
+    out.set_wheelbase(in.car.wheel_base);
+    out.set_ftheta(in.car.front_wheels_theta);
+}

communication/rabbitmq/Boundary.h

@@ -0,0 +1,52 @@
+//
+// Created by zx on 23-11-27.
+//
+
+#ifndef CERES_TEST_BOUNDARY_H
+#define CERES_TEST_BOUNDARY_H
+
+#include "proto/def.grpc.pb.h"
+#include "proto/communication.pb.h"
+#include "communication/transitData.h"
+#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   //前轮角未回正
+};
+
+class Boundary {
+public:
+    Boundary() = default;
+
+    bool init(const std::string &file);
+
+    bool init(JetStream::Limit limit);
+
+    ~Boundary();
+
+    void limit(JetStream::MeasureInfo &info);
+
+    void transMeasureInfo(JetStream::MeasureInfo &in, measure_info &out);
+
+    void transMeasureInfo(JetStream::MeasureInfo &in, TransitData::MeasureInfo &out);
+
+    void transMeasureInfo(DetectManager::DetectResult &in, JetStream::MeasureInfo &out);
+
+private:
+    BoundRet limit_boundary(JetStream::MeasureInfo info, JetStream::Boundary bound);
+
+    bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message &protobuf_parameter);
+
+    JetStream::Limit limit_;
+};
+
+#endif //CERES_TEST_BOUNDARY_H

communication/rabbitmq/rabbitmq_communication.cpp

@@ -29,9 +29,49 @@ Error_manager RabbitmqCommunicationTof3D::execute_time_consume_msg(Rabbitmq_mess
 
 //定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
 Error_manager RabbitmqCommunicationTof3D::auto_encapsulate_status() {
+    static int heart = 0;
+    if (++heart > 999) {
+        heart = 0;
+    }
+
+    measure_info rabbit_measure_info;
+    DetectManager::DetectResult detect_measure_info;
+
+    // 设备状态
+    if (DeviceTof3D::iter()->getDeviceStatus() != SUCCESS) {
+        rabbit_measure_info.set_ground_status(MeasureStatu::Lidar_Disconnect);
+    }
+    MeasureStatu measure_status = DetectManager::iter()->getMeasureResult(detect_measure_info);
+    if (measure_status != MeasureStatu::Measure_OK) {
+        rabbit_measure_info.set_ground_status(measure_status);
+    }
+
+    // 超界判断
+    JetStream::MeasureInfo info;
+    m_boundary.transMeasureInfo(detect_measure_info, info);
+    m_boundary.limit(info);
+
+    // 保存grpc数据中间块
+    TransitData::MeasureInfo grpc_measure_info;
+    m_boundary.transMeasureInfo(info, grpc_measure_info);
+    grpc_measure_info.error.append("heart: " + std::to_string(heart));
+    grpc_measure_info.error.append(detect_measure_info.info());
+    TransitData::get_instance_pointer()->setMeasureInfo(grpc_measure_info);
+
+    // 发送rabbitmq消息
+    m_boundary.transMeasureInfo(info, rabbit_measure_info);
+    encapsulate_status_msg(rabbit_measure_info.DebugString(), 0);
+
     return {};
 }
 
+Error_manager RabbitmqCommunicationTof3D::rabbitmq_init_from_protobuf(std::string prototxt_path) {
+    m_boundary.init(ETC_PATH PROJECT_NAME "/limit.prototxt") ?
+    printf("boundary init success.\n") :
+    printf("boundary init failed form %s.\n", ETC_PATH PROJECT_NAME "/limit.prototxt");
+    return Rabbitmq_base::rabbitmq_init_from_protobuf(prototxt_path);
+}
+
 
 
 

communication/rabbitmq/rabbitmq_communication.h

@@ -2,6 +2,8 @@
 
 #include "thread/singleton.h"
 #include "rabbitmq/rabbitmq_base.h"
+#include "detect/detect_manager.h"
+#include "Boundary.h"
 
 class RabbitmqCommunicationTof3D : public Singleton<RabbitmqCommunicationTof3D>, public Rabbitmq_base {
 // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
@@ -13,6 +15,8 @@ private:
 
 public:
     //必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+    Error_manager rabbitmq_init_from_protobuf(std::string prototxt_path);
+
     RabbitmqCommunicationTof3D(const RabbitmqCommunicationTof3D &other) = delete;
 
     RabbitmqCommunicationTof3D &operator=(const RabbitmqCommunicationTof3D &other) = delete;
@@ -44,5 +48,5 @@ public:
     std::mutex m_lock;                                //锁,
 
 private:
-
+    Boundary m_boundary;
 };

communication/transitData.cpp

@@ -0,0 +1,72 @@
+//
+// Created by zx on 23-11-28.
+//
+
+#include "transitData.h"
+
+TransitData::TransitData() {
+}
+
+void TransitData::setImage(cv::Mat image, int id) {
+    if (id < 0 || id >= NUM_) return;
+    mutex_.lock();
+    image_[id] = image.clone();
+    mutex_.unlock();
+}
+
+void TransitData::setImageCL(cv::Mat &image, int id) {
+    if (id < 0 || id >= NUM_) return;
+    mutex_.lock();
+    imageCL_[id] = image.clone();
+    mutex_.unlock();
+}
+
+void TransitData::setMask(std::vector<cv::Point> mask, int id) {
+    if (id < 0 || id >= NUM_) return;
+    mutex_.lock();
+    mask_[id] = mask;
+    mutex_.unlock();
+}
+
+void TransitData::setMeasureInfo(MeasureInfo info) {
+    mutex_.lock();
+    measureInfo_ = info;
+    mutex_.unlock();
+}
+
+bool TransitData::getImage(cv::Mat &image, int id, float timeout) {
+    if (id < 0 || id >= NUM_) return false;
+    if (image_[id].timeout(timeout))
+        return false;
+    mutex_.lock();
+    image = image_[id]();
+    mutex_.unlock();
+    return true;
+}
+
+bool TransitData::getImageCL(cv::Mat &image, int id, float timeout) {
+    if (id < 0 || id >= NUM_) return false;
+    if (imageCL_[id].timeout(timeout))
+        return false;
+    mutex_.lock();
+    image = imageCL_[id]().clone();
+    mutex_.unlock();
+    return true;
+}
+
+bool TransitData::getMask(std::vector<cv::Point> &mask, int id, float timeout) {
+    if (id < 0 || id >= NUM_) return false;
+    if (mask_[id].timeout(timeout))
+        return false;
+    mask = mask_[id]();
+    return true;
+}
+
+bool TransitData::getMeasureInfo(MeasureInfo &info, float timeout) {
+    if (measureInfo_.timeout(timeout))
+        return false;
+    mutex_.lock();
+    info = measureInfo_();
+    mutex_.unlock();
+    return true;
+}

communication/transitData.h

@@ -0,0 +1,79 @@
+//
+// Created by zx on 23-11-28.
+//
+
+#ifndef CERES_TEST_TransitData_H
+#define CERES_TEST_TransitData_H
+
+#include <opencv2/opencv.hpp>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include "tool/timedlockdata.hpp"
+#include "thread/singleton.h"
+#include <mutex>
+
+#define NUM_ 4
+
+class TransitData : public Singleton<TransitData> {
+    friend Singleton<TransitData>;
+public:
+    struct MeasureInfo {
+        float x;
+        float y;
+        float transx;
+        float transy;
+        float theta;
+        float width;
+        float wheelbase;
+        float ftheta;
+        int border_plc;
+        int border_display;
+        std::string error;
+
+        MeasureInfo &operator=(const MeasureInfo &info) {
+            x = info.x;
+            y = info.y;
+            theta = info.theta;
+            transx = info.transx;
+            transy = info.transy;
+            width = info.width;
+            wheelbase = info.wheelbase;
+            ftheta = info.ftheta;
+            border_plc = info.border_plc;
+            border_display = info.border_display;
+            error = info.error;
+            return *this;
+        }
+    };
+
+public:
+
+    void setImage(cv::Mat image, int id);
+
+    void setImageCL(cv::Mat &image, int id);
+
+    void setMask(std::vector<cv::Point> mask, int id);
+
+    void setMeasureInfo(MeasureInfo info);
+
+    bool getImage(cv::Mat &image, int id, float timeout);
+
+    bool getImageCL(cv::Mat &image, int id, float timeout);
+
+    bool getMask(std::vector<cv::Point> &mask, int id, float timeout);
+
+    bool getMeasureInfo(MeasureInfo &info, float timeout);
+
+private:
+    TransitData();
+
+    std::mutex mutex_;
+    TimedLockData<cv::Mat> image_[NUM_];
+    TimedLockData<cv::Mat> imageCL_[NUM_];
+
+    TimedLockData<std::vector<cv::Point>> mask_[NUM_];
+    TimedLockData<MeasureInfo> measureInfo_;
+
+};
+
+#endif //CERES_TEST_TransitData_H

detect/ceres_detect/car_pose_detector.h

@@ -36,6 +36,7 @@ public:
         float front_wheels_theta = 0;
         float theta = 0;
         float wheel_base = 0;
+        float loss = 0;
 
         void reset() {
             wheels_center_x = 0;
@@ -47,12 +48,13 @@ public:
             wheel_length = 0;
             front_wheels_theta = 0;
             wheel_base = 0;
+            loss = 0;
         }
 
         std::string info() {
             char str[512];
-            sprintf(str, "x:%0.3f y:%0.3f theta:%0.3f w:%0.3f l:%0.3f ww:%0.3f wl:%0.3f ft:%0.3f wb:%0.3f",
-                    wheels_center_x, wheels_center_y, theta, width, length, wheel_width, wheel_length, front_wheels_theta, wheel_base);
+            sprintf(str, "x:%0.3f y:%0.3f theta:%0.3f w:%0.3f l:%0.3f ww:%0.3f wl:%0.3f ft:%0.3f wb:%0.3f loss:%0.3f\n",
+                    wheels_center_x, wheels_center_y, theta, width, length, wheel_width, wheel_length, front_wheels_theta, wheel_base, loss);
             return str;
         }
     };

detect/ceres_detect/detect_wheel_ceres.cpp

@@ -127,11 +127,116 @@ public:
 
 };
 
-class CostLineFunc{
+class CostRYFunc{
 private:
+    std::vector<double>   line_y_;
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_rear_cloud;      //左后点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_rear_cloud;     //右后点云
+
+public:
+    CostRYFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
+        pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
+        std::vector<double> liney)
+    {
+      line_y_=liney;
+      m_left_front_cloud=left_front;
+      m_right_front_cloud=right_front;
+      m_left_rear_cloud=left_rear;
+      m_right_rear_cloud=right_rear;
+
+    }
+    template <typename T>
+    bool operator()(const T* const variable, T* residual) const {
+      T cy = variable[0];
+      T theta = variable[1];
+      T length = variable[2];
+
+      //整车旋转
+      Eigen::Rotation2D<T> rotation(theta);
+      Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+      //左前偏移
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(-0 / 2.0, length / 2.0);
+      //右前偏移
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(0 / 2.0, length / 2.0);
+      //左后偏移
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-0 / 2.0, -length / 2.0);
+      //右后偏移
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(0 / 2.0, -length / 2.0);
+
+
+      ceres::Grid1D<double> grid_y(line_y_.data(),0,line_y_.size());
+      ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);
+      T weight=T(0.3);
+//左前轮
+
+      int left_front_num = m_left_front_cloud->size();
+      for (int i = 0; i < m_left_front_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_front_cloud->points[i].x)),
+                                           (T(m_left_front_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_left_front;
+        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[i]);
+        residual[i]=residual[i]*weight;
+
+      }
+      //右前轮
+      int right_front_num = m_right_front_cloud->size();
+      for (int i = 0; i < m_right_front_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_front_cloud->points[i].x) ),
+                                           (T(m_right_front_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> point_rotation = rotation_matrix * point +tp- wheel_center_normal_right_front;
+
+        interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[left_front_num+i]);
+        residual[left_front_num+i]=residual[left_front_num+i]*weight;
+      }
+      //左后轮
+      int left_rear_num = m_left_rear_cloud->size();
+
+      for (int i = 0; i < m_left_rear_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_left_rear_cloud->points[i].x)),
+                                           (T(m_left_rear_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point +tp- wheel_center_normal_left_rear;
+        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
+                                &residual[(left_front_num+right_front_num)+i]);
+
+      }
+
+      //右后轮
+
+      for (int i = 0; i < m_right_rear_cloud->size(); ++i) {
+        const Eigen::Matrix<T, 2, 1> point((T(m_right_rear_cloud->points[i].x)),
+                                           (T(m_right_rear_cloud->points[i].y)));
+        const Eigen::Matrix<T, 2, 1> tp(T(0),cy);
+        //减去经过车辆旋转计算的左前中心
+        const Eigen::Matrix<T, 2, 1> tanslate_point = rotation_matrix * point +tp- wheel_center_normal_right_rear;
+
+        interpolator_y.Evaluate(tanslate_point[1]*T(1000.)+T(1000.),
+                                &residual[left_front_num+right_front_num+left_rear_num+i]);
+
+      }
+
+      return true;
+    }
+
+private:
+
+};
+
+class CostXYWFFunc{
     std::vector<double>   line_l_;
     std::vector<double>   line_r_;
     std::vector<double>   line_y_;
+    double length_;
+    double theta_;
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
@@ -139,13 +244,17 @@ private:
     pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_rear_cloud;     //右后点云
 
 public:
-    CostLineFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
-        pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
-                 std::vector<double> linel,std::vector<double> liner,std::vector<double> liney)
+    CostXYWFFunc(pcl::PointCloud<pcl::PointXYZ>::Ptr left_front,pcl::PointCloud<pcl::PointXYZ>::Ptr right_front,
+                 pcl::PointCloud<pcl::PointXYZ>::Ptr left_rear,pcl::PointCloud<pcl::PointXYZ>::Ptr right_rear,
+                 std::vector<double> linel,std::vector<double> liner,std::vector<double> liney,
+                 double theta,double length)
     {
       line_l_=linel;
       line_r_=liner;
       line_y_=liney;
+      length_=length;
+      theta_=theta;
+
       m_left_front_cloud=left_front;
       m_right_front_cloud=right_front;
       m_left_rear_cloud=left_rear;
@@ -156,37 +265,36 @@ public:
     bool operator()(const T* const variable, T* residual) const {
       T cx = variable[0];
       T cy = variable[1];
-      T theta = variable[2];
-      T length = variable[3];
-      T width = variable[4];
-      T theta_front = variable[5];
+      T width = variable[2];
+      T theta_front = variable[3];
+      T theta=T(theta_);
 
       //整车旋转
       Eigen::Rotation2D<T> rotation(theta);
       Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
 
       //左前偏移
-      Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(-width / 2.0, length / 2.0);
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_left_front(-width / 2.0, T(length_) / 2.0);
       //右前偏移
-      Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(width / 2.0, length / 2.0);
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_right_front(width / 2.0, T(length_) / 2.0);
       //左后偏移
-      Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-width / 2.0, -length / 2.0);
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_left_rear(-width / 2.0, -T(length_) / 2.0);
       //右后偏移
-      Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(width / 2.0, -length / 2.0);
+      Eigen::Matrix<T, 2, 1> wheel_center_normal_right_rear(width / 2.0, -T(length_) / 2.0);
 
       //前轮旋转
       Eigen::Rotation2D<T> rotation_front(theta_front);
       Eigen::Matrix<T, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
 
-
+      T weight=T(0.5);
       ceres::Grid1D<double> grid_l(line_l_.data(),0,line_l_.size());
       ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_l(grid_l);
       ceres::Grid1D<double> grid_r(line_r_.data(),0,line_r_.size());
       ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_r(grid_r);
+
       ceres::Grid1D<double> grid_y(line_y_.data(),0,line_y_.size());
       ceres::CubicInterpolator<ceres::Grid1D<double>> interpolator_y(grid_y);
 
-
       //左前轮
       int left_front_num = m_left_front_cloud->size();
       for (int i = 0; i < m_left_front_cloud->size(); ++i) {
@@ -199,6 +307,8 @@ public:
         interpolator_l.Evaluate(point_rotation[0]*T(1000.)+T(200.),&residual[2*i]);
         interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[2*i+1]);
 
+        residual[2*i]=residual[2*i]*weight;
+
       }
       //右前轮
       int right_front_num = m_right_front_cloud->size();
@@ -212,6 +322,7 @@ public:
 
         interpolator_r.Evaluate(point_rotation[0]*T(1000.)+T(1000.),&residual[2*left_front_num + 2*i]);
         interpolator_y.Evaluate(point_rotation[1]*T(1000.)+T(1000.),&residual[2*left_front_num+2*i+1]);
+        residual[2*left_front_num + 2*i]=residual[2*left_front_num + 2*i]*weight;
 
       }
       //左后轮
@@ -246,93 +357,14 @@ public:
       return true;
     }
 
-private:
-
 };
 
-void save(pcl::PointCloud<pcl::PointXYZ> m_left_front_cloud,pcl::PointCloud<pcl::PointXYZ> m_right_front_cloud,
-          pcl::PointCloud<pcl::PointXYZ> m_left_rear_cloud,pcl::PointCloud<pcl::PointXYZ> m_right_rear_cloud,
-          double& cx,double& cy,double& theta,double& length,double& width,double& front_theta){
-  //整车旋转
-  Eigen::Rotation2D<double> rotation(theta);
-  Eigen::Matrix<double, 2, 2> rotation_matrix = rotation.toRotationMatrix();
-
-  //左前偏移
-  Eigen::Matrix<double, 2, 1> wheel_center_normal_left_front(-width / 2.0, length / 2.0);
-  //右前偏移
-  Eigen::Matrix<double, 2, 1> wheel_center_normal_right_front(width / 2.0, length / 2.0);
-  //左后偏移
-  Eigen::Matrix<double, 2, 1> wheel_center_normal_left_rear(-width / 2.0, -length / 2.0);
-  //右后偏移
-  Eigen::Matrix<double, 2, 1> wheel_center_normal_right_rear(width / 2.0, -length / 2.0);
-
-  //前轮旋转
-  Eigen::Rotation2D<double> rotation_front(front_theta);
-  Eigen::Matrix<double, 2, 2> rotation_matrix_front = rotation_front.toRotationMatrix();
-
-  FILE* pf1=fopen("./1.txt","w");
-  FILE* pf2=fopen("./2.txt","w");
-  FILE* pf3=fopen("./3.txt","w");
-  FILE* pf4=fopen("./4.txt","w");
-
-  //左前轮
-  int left_front_num = m_left_front_cloud.size();
-  for (int i = 0; i < m_left_front_cloud.size(); ++i) {
-    const Eigen::Matrix<double, 2, 1> point((double(m_left_front_cloud[i].x) + cx),
-                                       (double(m_left_front_cloud[i].y) + cy));
-    //减去经过车辆旋转计算的左前中心
-    const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_front;
-    //旋转
-    const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-    fprintf(pf1,"%f %f %f 255 255 255\n",point_rotation[0],point_rotation[1],m_left_front_cloud[i].z);
-
-  }
-  //右前轮
-  int right_front_num = m_right_front_cloud.size();
-  for (int i = 0; i < m_right_front_cloud.size(); ++i) {
-    const Eigen::Matrix<double, 2, 1> point((double(m_right_front_cloud[i].x) + cx),
-                                       (double(m_right_front_cloud[i].y) + cy));
-    //减去经过车辆旋转计算的左前中心
-    const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_front;
-    //旋转
-    const Eigen::Matrix<double, 2, 1> point_rotation = rotation_matrix_front * tanslate_point;
-    fprintf(pf2,"%f %f %f 255 255 255\n",point_rotation[0],point_rotation[1],m_right_front_cloud[i].z);
-  }
-  //左后轮
-  int left_rear_num = m_left_rear_cloud.size();
-
-  for (int i = 0; i < m_left_rear_cloud.size(); ++i) {
-    const Eigen::Matrix<double, 2, 1> point((double(m_left_rear_cloud[i].x) + cx),
-                                       (double(m_left_rear_cloud[i].y) + cy));
-    //减去经过车辆旋转计算的左前中心
-    const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_left_rear;
-    fprintf(pf3,"%f %f %f 255 255 255\n",tanslate_point[0],tanslate_point[1],m_left_rear_cloud[i].z);
-
-  }
-
-  //右后轮
-
-  for (int i = 0; i < m_right_rear_cloud.size(); ++i) {
-    const Eigen::Matrix<double, 2, 1> point((double(m_right_rear_cloud[i].x) + cx),
-                                       (double(m_right_rear_cloud[i].y) + cy));
-    //减去经过车辆旋转计算的左前中心
-    const Eigen::Matrix<double, 2, 1> tanslate_point = rotation_matrix * point - wheel_center_normal_right_rear;
-    fprintf(pf4,"%f %f %f 255 255 255\n",tanslate_point[0],tanslate_point[1],m_right_rear_cloud[i].z);
-  }
-  fclose(pf1);
-  fclose(pf2);
-  fclose(pf3);
-  fclose(pf4);
-
-}
-
-
 detect_wheel_ceres::detect_wheel_ceres()
 {
   FILE* filel= fopen("./cuve_l.txt","w");
   for(int i=0;i<1200;i++){
     double x=double(i-200)*0.001;
-    double y=std::max(1.0/(1.+exp(-400.*(-x-0.02))) , 1.-exp(-4*x*x));
+    double y=std::max(1.0/(1.+exp(-400.*(-x-0.02))) , 1.-exp(-x*x));
     m_line_l.push_back(y);
     fprintf(filel,"%f %f 0\n",x,y);
 
@@ -342,7 +374,7 @@ detect_wheel_ceres::detect_wheel_ceres()
   FILE* filer= fopen("./cuve_r.txt","w");
   for(int i=0;i<1200;i++){
     double x=double(i-1000)*0.001;
-    double y=std::max(1.0/(1.+exp(-400.*(x-0.02))) , 1.-exp(-2*x*x));
+    double y=std::max(1.0/(1.+exp(-400.*(x-0.02))) , 1.-exp(-x*x));
     m_line_r.push_back(y);
     fprintf(filer,"%f %f 0\n",x,y);
 
@@ -352,9 +384,10 @@ detect_wheel_ceres::detect_wheel_ceres()
   FILE* filey= fopen("./cuve_y.txt","w");
   for(int i=0;i<2000;i++){
     double x=double(i-1000)*0.001;
-    double y= 1.-exp(-x*x);
-    m_line_y.push_back(y);
-    fprintf(filey,"%f %f 0\n",x,y);
+    m_line_y.push_back(1.-exp(-0.5*x*x));
+    m_line_y_2stp.push_back(1.-exp(-2*x*x));
+    fprintf(filey,"%f %f 0\n",x,1.-exp(-0.5*x*x));
+    fprintf(filey,"%f %f 0\n",x,1.-exp(-2*x*x));
 
   }
   fclose(filey);
@@ -363,25 +396,23 @@ detect_wheel_ceres::detect_wheel_ceres()
 }
 detect_wheel_ceres::~detect_wheel_ceres(){}
 
+bool detect_wheel_ceres::detect_dynP(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){
+  m_left_front_cloud= filter(cl1,1.5);
+  m_right_front_cloud=filter(cl2,1.5);
+  m_left_rear_cloud= filter(cl3,1.5);
+  m_right_rear_cloud= filter(cl4,1.5);
 
-
-bool detect_wheel_ceres::detect(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){
-  m_left_front_cloud=cl1;
-  m_right_front_cloud=cl2;
-  m_left_rear_cloud=cl3;
-  m_right_rear_cloud=cl4;
-
-
-  double variable[] = {cx,cy,theta,wheel_base,width,front_theta};
+  double variable[] = {cx,cy,theta,front_theta};
   auto t1=std::chrono::steady_clock::now();
   // 第二部分：构建寻优问题
   ceres::Problem problem;
   ceres::CostFunction* cost_function =new
-      ceres::AutoDiffCostFunction<CostLineFunc, ceres::DYNAMIC, 6>(
-      new CostLineFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
-                       m_line_l,m_line_r,m_line_y),
+      ceres::AutoDiffCostFunction<CostDynFunc, ceres::DYNAMIC, 4>(
+      new CostDynFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
+                       m_line_l,m_line_r,m_line_y,width,wheel_base),
       2*(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size()));
   problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
@@ -398,45 +429,99 @@ bool detect_wheel_ceres::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl::Poi
 
   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;
+  double time= double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
 
-  double loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
-  printf("loss: %.3f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
-                                                 m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);
+  loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
 
   cx=variable[0];
   cy=variable[1];
   theta=variable[2];
-  wheel_base=variable[3];
-  width=variable[4];
-  front_theta=variable[5];
-
-
-
+  front_theta=variable[3];
+  if(loss>0.05){
+    return false;
+  }
 
   //save(*m_left_front_cloud,*m_right_front_cloud,*m_left_rear_cloud,*m_right_rear_cloud,cx,cy,theta,wheel_base,width,front_theta);
 
   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,
+                                      float& cx,float& cy,float& theta,float& wheel_base,float& width,
+                                      float& front_theta,float& loss){
+  m_left_front_cloud= filter(cl1,1.5);
+  m_right_front_cloud=filter(cl2,1.5);
+  m_left_rear_cloud= filter(cl3,1.5);
+  m_right_rear_cloud= filter(cl4,1.5);
 
+  bool ryl=detect_RYL(cl1,cl2,cl3,cl4,cx,cy,theta,wheel_base,width,front_theta,loss);
+  if(ryl==false) return ryl;
+  bool xwf=detect_XWF(cl1,cl2,cl3,cl4,cx,cy,theta,wheel_base,width,front_theta,loss);
+  return xwf;
 }
 
+bool detect_wheel_ceres::detect_RYL(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_wheel_ceres::detect_dynP(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){
-  m_left_front_cloud=cl1;
-  m_right_front_cloud=cl2;
-  m_left_rear_cloud=cl3;
-  m_right_rear_cloud=cl4;
 
-  double variable[] = {cx,cy,theta,front_theta};
+
+
+  double variable[] = {cy,theta,wheel_base};
   auto t1=std::chrono::steady_clock::now();
   // 第二部分：构建寻优问题
   ceres::Problem problem;
   ceres::CostFunction* cost_function =new
-      ceres::AutoDiffCostFunction<CostDynFunc, ceres::DYNAMIC, 4>(
-      new CostDynFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
-                       m_line_l,m_line_r,m_line_y,width,wheel_base),
+      ceres::AutoDiffCostFunction<CostRYFunc, ceres::DYNAMIC, 3>(
+      new CostRYFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,m_line_y_2stp),
+      (m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size()));
+  problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+
+  //第三部分： 配置并运行求解器
+  ceres::Solver::Options options;
+  options.use_nonmonotonic_steps=false;
+  options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+  options.max_num_iterations=100;
+  options.num_threads=2;
+  options.minimizer_progress_to_stdout = false;//输出到cout
+  ceres::Solver::Summary summary;//优化信息
+  ceres::Solve(options, &problem, &summary);//求解!!!
+
+  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;
+
+  loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
+  /*printf("loss: %.5f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
+                                                 m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);*/
+
+  //cy=variable[0];
+  theta=variable[1];
+  wheel_base=variable[2];
+  if(loss>0.05){
+    return false;
+  }
+return true;
+
+}
+
+bool detect_wheel_ceres::detect_XWF(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){
+
+  double variable[] = {cx,cy,width,front_theta};
+  auto t1=std::chrono::steady_clock::now();
+  // 第二部分：构建寻优问题
+  ceres::Problem problem;
+  CostXYWFFunc* costFuc=new CostXYWFFunc(m_left_front_cloud,m_right_front_cloud,m_left_rear_cloud,m_right_rear_cloud,
+                                  m_line_l,m_line_r,m_line_y,theta,wheel_base);
+  ceres::CostFunction* cost_function =new
+      ceres::AutoDiffCostFunction<CostXYWFFunc, ceres::DYNAMIC, 4>(costFuc,
       2*(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size()));
   problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
 
@@ -445,7 +530,7 @@ bool detect_wheel_ceres::detect_dynP(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl
   ceres::Solver::Options options;
   options.use_nonmonotonic_steps=false;
   options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
-  options.max_num_iterations=1000;
+  options.max_num_iterations=100;
   options.num_threads=2;
   options.minimizer_progress_to_stdout = false;//输出到cout
   ceres::Solver::Summary summary;//优化信息
@@ -453,18 +538,42 @@ bool detect_wheel_ceres::detect_dynP(pcl::PointCloud<pcl::PointXYZ>::Ptr cl1,pcl
 
   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;
+  double time=double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den;
 
-  double loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
-  printf("loss: %.3f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
-  m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);
+  loss=summary.final_cost/(m_left_front_cloud->size()+m_right_front_cloud->size()+m_left_rear_cloud->size()+m_right_rear_cloud->size());
+  /*printf("loss: %.5f size:%d  time:%.5fs\n",loss,m_left_front_cloud->size()+m_right_front_cloud->size()+
+                                                 m_left_rear_cloud->size()+m_right_rear_cloud->size(),time);*/
 
   cx=variable[0];
   cy=variable[1];
-  theta=variable[2];
+  width=variable[2];
   front_theta=variable[3];
+  if(loss>0.05){
+    return false;
+  }
+  return true;
 
-  //save(*m_left_front_cloud,*m_right_front_cloud,*m_left_rear_cloud,*m_right_rear_cloud,cx,cy,theta,wheel_base,width,front_theta);
+}
 
-  return true;
-}
+pcl::PointCloud<pcl::PointXYZ>::Ptr detect_wheel_ceres::filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float r){
+  if(cloud->size()==0)
+    return cloud;
+  float sumx=0;
+  for(int i=0;i<cloud->size();++i){
+    sumx+=cloud->points[i].x;
+  }
+  float avg=sumx/float(cloud->size());
+  float stdd=0.0;
+  for(int i=0;i<cloud->size();++i){
+    stdd+=pow(cloud->points[i].x-avg,2);
+  }
+  stdd=sqrt(stdd/(float(cloud->size())));
+  if(stdd<0.15)
+    return cloud;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<pcl::PointXYZ>);
+  for(int i=0;i<cloud->size();++i){
+    if(fabs(cloud->points[i].x-avg)<r*stdd)
+      out->push_back(cloud->points[i]);
+  }
+  return out;
+}

detect/ceres_detect/detect_wheel_ceres.h

@@ -17,19 +17,29 @@ public:
     detect_wheel_ceres();
     ~detect_wheel_ceres();
 
-    bool detect(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);
+    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,
+                      float& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
 
     /*
      * 只检测动态参数， 固定参数已知
      */
     bool detect_dynP(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& cx,float& cy,float& theta,float& wheel_base,float& width,float& front_theta,float& loss);
 
 private:
+    bool detect_RYL(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_XWF(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::PointCloud<pcl::PointXYZ>::Ptr filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float r);
+
+
     pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_front_cloud;     //左前点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr          m_right_front_cloud;    //右前点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr          m_left_rear_cloud;      //左后点云
@@ -41,6 +51,7 @@ public:
 
     std::vector<double>                     m_line_l;
     std::vector<double>                     m_line_r;
+    std::vector<double>                     m_line_y_2stp;
     std::vector<double>                     m_line_y;
 
 };

detect/ceres_detect/wheel_detector.h

@@ -23,6 +23,7 @@ public:
         pcl::PointXYZ center;
         float confidence = 0;
         float theta = 0;
+        float loss = 0;
 
         void reset() {
             haveCloud = false;
@@ -33,11 +34,13 @@ public:
             center.z = 0;
             confidence = 0;
             theta = 0;
+            loss = 0;
         }
 
         std::string info() {
             char str[512];
-            sprintf(str, "x: %.3f, y: %.3f, z: %.3f, theta: %.3f", center.x, center.y, center.z, theta);
+            sprintf(str, "x: %.3f, y: %.3f, z: %.3f, theta: %.3f loss: %.3f conf: %.3f\n",
+                    center.x, center.y, center.z, theta, loss, confidence);
             return str;
         }
     };

detect/detect_manager.cpp

@@ -3,3 +3,217 @@
 //
 
 #include "detect_manager.h"
+
+Error_manager DetectManager::Init() {
+
+#ifdef ENABLE_TENSORRT_DETECT
+    detector = new TensorrtWheelDetector(ETC_PATH PROJECT_NAME "/model.engine", ETC_PATH PROJECT_NAME "/class.txt");
+#else
+    detector = new TensorrtWheelDetector(ETC_PATH PROJECT_NAME "/model.onnx", ETC_PATH PROJECT_NAME "/class.txt");
+#endif
+
+    m_thread = new std::thread(&DetectManager::run, this);
+    m_condit.notify_all(true);
+
+    return Error_manager();
+}
+
+void DetectManager::run() {
+    LOG(INFO) << "detect thread running in " << std::this_thread::get_id();
+
+    DetectResult m_detect_result_record;
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_wheel_cloud;
+    for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
+        vct_wheel_cloud.emplace_back(new pcl::PointCloud<pcl::PointXYZ>);
+    }
+
+    auto t_start_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
+
+    while (m_condit.is_alive()) {
+        m_condit.wait();
+        // 参数初始化
+        t_start_time = std::chrono::steady_clock::now();
+
+        if (m_condit.is_alive()) {
+            DetectResult detect_result;
+            cv::Mat merge_mat = cv::Mat::zeros(480 * 2, 640 * 2, CV_8UC1);
+            DeviceTof3D::DeviceTof3DSaveInfo t_device_mat[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
+            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+                t_device_mat[device_index] = DeviceTof3D::iter()->getDeviceSaveInfo((DeviceAzimuth)device_index);
+                t_device_mat[device_index].irMat.copyTo(merge_mat(cv::Rect((device_index & 0x1) * 640,
+                                                                           ((device_index & 0x2) >> 1) * 480, 640, 480)));
+            }
+
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
+            // 模型识别
+            std::vector<Object> objs;
+            cv::cvtColor(merge_mat, merge_mat, cv::COLOR_GRAY2RGB);
+            detector->detect(merge_mat, objs, merge_mat);
+            cv::imshow("merge_mat", merge_mat);
+            cv::waitKey(1);
+            if (objs.empty()) {
+                detect_statu = MeasureStatu::Measure_Empty_Clouds;
+                m_detect_result_record.reset();
+                continue;
+            }
+
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
+            // 标记识别点云
+            for (auto &obj: objs) {
+                auto seg_points = detector->getPointsFromObj(obj);
+                int device_index = (int(seg_points[0].x / 640) * 0x01) | (int(seg_points[0].y / 480) << 1);
+                detect_result.wheel[device_index].confidence = obj.prob;
+
+                if (obj.prob > 0.9) {
+                    detect_result.wheel[device_index].detectOk = true;
+                    vct_wheel_cloud[device_index]->clear();
+                    float d = MAX(obj.rect.height, obj.rect.width) / 2;
+                    pcl::PointXYZ point;
+                    int x_alpha = (device_index & 0x1);
+                    int y_alpha = ((device_index & 0x2) >> 1);
+                    for (auto &pt: seg_points) {    // use 7.5~9ms
+                        pt.x -= x_alpha * t_device_mat[device_index].irMat.cols;
+                        pt.y -= y_alpha * t_device_mat[device_index].irMat.rows;
+                        t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[3] = obj.prob;
+                        point.x = t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[0];
+                        point.y = t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[1];
+                        point.z = t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[2];
+                        vct_wheel_cloud[device_index]->push_back(point);
+                    }
+
+                    TransitData::get_instance_pointer()->setMask(seg_points, device_index);
+                }
+            }
+
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
+            // 点云处理
+            if (!objs.empty()) {
+                WheelCloudOptimize(vct_wheel_cloud, detect_result);
+                cost = std::chrono::steady_clock::now() - t_start_time;
+                DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
+                bool ceres_detect_ret = false;
+                detect_wheel_ceres wheel_detect;
+                // 采用上次测量结果的数据
+                if (!m_detect_result_time_lock_data.timeout(0.5)) {
+                    detect_result = m_detect_result_time_lock_data();
+                    detect_result.car.wheel_width = MIN(detect_result.car.wheel_width + 0.4, 2.5);
+                } else {
+                    detect_result.car.wheel_width = 3;
+                    detect_result.car.wheel_base = 2.7;
+                }
+
+                // 根据车轮数采用不同的优化方式
+                if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
+                    objs.size() > 2) {
+                    ceres_detect_ret = wheel_detect.detect_2Step(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 < 1.0 && objs.size() == 2) || objs.size() < 2) {
+                        detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
+                        detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
+                    } else if (objs.size() == 2) {
+                        std::vector<int> have_cloud;
+                        for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+                            if (!vct_wheel_cloud[device_index]->empty()) {
+                                have_cloud.push_back(device_index);
+                            }
+                        }
+
+                        if (have_cloud[0] % 2 == have_cloud[1] %2 ) {
+                            detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
+                        } else {
+                            detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
+                        }
+                    }
+                } else {
+                    ceres_detect_ret = wheel_detect.detect_dynP(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, m_detect_result_record.car.wheel_base,
+                                         m_detect_result_record.car.wheel_width,detect_result.car.front_wheels_theta, detect_result.car.loss);
+                    detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
+                    detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
+                }
+
+                // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
+                float move_distance = detect_result.car.wheels_center_y - m_detect_result_record.car.wheels_center_y;
+                if (fabs(move_distance) > 0.1) {
+                    move_statu = true;
+                } else if (move_statu()) {
+                    move_statu = false;
+                }
+
+                if (ceres_detect_ret) {
+                    // 记录下测量结果
+                    m_detect_result_record = detect_result;
+                    detect_statu = Measure_OK;
+                    m_detect_result_time_lock_data = m_detect_result_record;
+                } else {
+                    detect_statu = Measure_Failture;
+                }
+
+            }
+            cost = std::chrono::steady_clock::now() - t_start_time;
+            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
+
+            // 算法结果处理
+//            static float min_wheel_width = 1e8, max_wheel_width = -1e8, min_wheel_base = 1e8, max_wheel_base = -1e8;
+//            if (m_detect_result_record.car.wheel_width > 1.0) {
+//                min_wheel_width = std::min(min_wheel_width, m_detect_result_record.car.wheel_width);
+//                max_wheel_width = std::max(max_wheel_width, m_detect_result_record.car.wheel_width);
+//            }
+//            if (m_detect_result_record.car.wheel_base > 1.0) {
+//                min_wheel_base = std::min(min_wheel_base, m_detect_result_record.car.wheel_base);
+//                max_wheel_base = std::max(max_wheel_base, m_detect_result_record.car.wheel_base);
+//            }
+
+//            LOG(INFO) << "\n" << m_detect_result_record.info();
+            usleep(1000 * 100);
+        }
+    }
+    LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
+}
+
+
+Error_manager DetectManager::WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result) {
+    //下采样
+    pcl::VoxelGrid<pcl::PointXYZ> vox;                     //创建滤波对象
+    vox.setLeafSize(0.03f, 0.03f, 0.03f);     //设置滤波时创建的体素体积为1cm的立方体
+    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
+        if (vct_wheel_cloud[device_index]->empty()) { continue;}
+
+        vox.setInputCloud(vct_wheel_cloud[device_index]);                   //设置需要过滤的点云给滤波对象
+        vox.filter(*vct_wheel_cloud[device_index]);                         //执行滤波处理，存储输出
+    }
+
+    return {};
+}
+
+MeasureStatu DetectManager::getMeasureResult(DetectManager::DetectResult &result) {
+    if (detect_statu() != MeasureStatu::Measure_OK) {
+        return detect_statu();
+    }
+    if (m_detect_result_time_lock_data.timeout()) {
+        return MeasureStatu::Measure_Failture;
+    }
+    result = m_detect_result_time_lock_data();
+    return MeasureStatu::Measure_OK;
+}
+
+Error_manager DetectManager::carMoveStatus() {
+    if (move_statu.timeout(3) && !move_statu()) {
+        return {FAILED, NORMAL};
+    }
+    return {};
+}

detect/detect_manager.h

@@ -1,14 +1,76 @@
-//
-// Created by zx on 2023/11/24.
-//
+#pragma once
 
-#ifndef ALLPROJECT_DETECT_MANAGER_H
-#define ALLPROJECT_DETECT_MANAGER_H
+#include <thread>
 
+#include "error_code/error_code.hpp"
+#include "ceres_detect/detect_wheel_ceres.h"
+#include "proto/enum_type.pb.h"
+#include "proto/detect_message.pb.h"
+#include "thread/thread_condition.h"
+#include "vzense/device_tof3d.h"
 
-class detect_manager {
+#ifdef ENABLE_TENSORRT_DETECT
+#include "detect/tensorrt_detect/wheel-detector.h"
+#else
+#include "detect/onnx_detect/wheel-detector.h"
+#endif
+#include "detect/ceres_detect/car_pose_detector.h"
+#include "detect/ceres_detect/wheel_detector.h"
+#include "detect/ceres_detect/detect_wheel_ceres.h"
+#include "communication/transitData.h"
 
+class DetectManager {
+public:
+    struct DetectResult {
+        WheelCeresDetector::WheelDetectResult wheel[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
+        Car_pose_detector::CarDetectResult car;
+
+        void reset() {
+            for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
+                wheel[i].reset();
+            }
+            car.reset();
+        }
+
+        std::string info() {
+            std::string str = car.info();
+            for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
+                str += wheel[i].info();
+            }
+            return str;
+        }
+    };
+
+public:
+    static DetectManager *iter() {
+        static DetectManager *instance = nullptr;
+        if (instance == nullptr) {
+            instance = new DetectManager();
+        }
+        return instance;
+    }
+
+    ~DetectManager() = default;
+
+    Error_manager Init();
+
+    MeasureStatu getMeasureResult(DetectResult &result);
+
+    Error_manager carMoveStatus();
+protected:
+    void run();
+
+    // 车轮点云优化
+    Error_manager WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result);
+
+private:
+    std::thread *m_thread;
+    Thread_condition m_condit;
+
+    TimedLockData<bool> move_statu;
+    TimedLockData<MeasureStatu> detect_statu;
+    TensorrtWheelDetector *detector;
+    TimedLockData<DetectResult> m_detect_result_time_lock_data;
 };
 
 
-#endif //ALLPROJECT_DETECT_MANAGER_H

proto/communication.proto

@@ -0,0 +1,27 @@
+syntax = "proto2";
+import "enum_type.proto";
+
+/*测量信息*/
+message measure_info {
+    required float cx=1;                                 // 车辆中心坐标x
+    required float cy=2;                                 // 车辆中心坐标y
+    required float theta=3;                              // 车身偏转角度（相对于y轴，左正右负）
+    required float length=4;                             // 车身长度（厦门四个雷达，含有该值，楚天两个雷达，该值为0）
+    required float width=5;                              // 车身宽度（左右两侧轮子最大宽度）
+    optional float height=6;                             // 车身高度
+    required float wheelbase=7;                          // 车辆前后轴距
+    required float front_theta=8;                        // 车辆前轮偏转角度
+    required int32 border_statu=9;                       // 超界状态， 位运算
+    required MeasureStatu ground_status=10;              // 测量状态，0=正常, 1=空, 2=测量失败, 3=超界, 4=终端超界， 5=雷达断连
+    required int32 is_stop=11;                           // <是否可停> 1为可停，0为不可停
+    required int32 motion_statu=12;                      // 运动状态，0=运动， 1=静止（只有三秒内都是静止才会写1，只要瞬间触发运动就会写0）
+    required float move_distance=13;                     // 前进距离
+}
+
+message CommunicationManagerConfig {
+    optional bool rabbitmq_enable = 1 [default = true];
+    optional string rabbitmq_config_file = 2 [default = "/rabbitmq.prototxt"];   // 安装目录下的路径，并非绝对路径或者bin文件开始的相对路径
+    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];
+}

communication/grpc/def.grpc.proto

@@ -4,7 +4,7 @@ package JetStream;
 message RequestCmd
 {
   int32 Id=1;  // 对于获取对应id的相机数据。1,2,3,4,其他表示所有
-                //对于打开视频流
+  //对于打开视频流
 }
 
 message Response{
@@ -36,28 +36,49 @@ message ResCloud{
   PointCloud cloud4=4;
 }
 message MeasureInfo {
-  float x=1;
-  float y=2;
-  float theta=3;
-  float width=4;
-  float wheelbase=5;
-  float ftheta=6;
-  int32 border=7;
-  string error=8;
+  optional float x=1;
+  optional float y=2;
+  optional float trans_x=3;
+  optional float trans_y=4;
+  optional float theta=5;
+  optional float width=6;
+  optional float wheelbase=7;
+  optional float ftheta=8;
+  // 超界说明，此参数没有表示正常。
+  // 1:超宽，2：轴距超长，
+  // 3：前超界，4：后超界，5：左，6：右，7：左倾角过大，8：右倾角过大，9：前轮角过大
+  optional int32 border_plc=9;
+  optional int32 border_display=10;
+  optional string error=11;
 }
 message ResFrame{
   ResImage images=1;
   ResCloud clouds=2;
-
   MeasureInfo measure_info=3;
-}
 
+}
 service StreamServer{
   rpc OpenImageStream(RequestCmd) returns(stream ResImage){}
   rpc OpenMeasureDataStream(RequestCmd) returns(stream MeasureInfo){}
-  rpc Detect(RequestCmd) returns(ResFrame){}
   rpc CloseImageStream(RequestCmd) returns(Response){}
   rpc CloseMeasureDataStream(RequestCmd) returns(Response){}
   rpc GetCloud(RequestCmd) returns(ResCloud){}
   rpc GetImage(RequestCmd) returns(ResImage){}
 }
+message Boundary{
+  float radius=1; //转台半径
+  float left=2;
+  float right=3;
+  float buttom=4;
+  float minAngle=5;
+  float maxAngle=6;
+  float fwheelAngle=7;
+}
+message Limit{
+  Boundary plc_limit=1;
+  Boundary display_limit=2;
+  float maxWidth=3;
+  float maxWheelBase=4;
+}
+
+

proto/detect_message.proto

@@ -0,0 +1,17 @@
+syntax = "proto2";
+
+// yolov8配置
+message Yolov8ProcessParams {
+  // 保存图片设置
+  optional bool save_mat = 1 [default = false];
+  optional bool draw_rect = 2 [default = false];
+  optional float save_confidence = 3 [default = 0.8];
+  optional float save_aspect_ratio_min = 4 [default = 0.9];
+  optional float save_aspect_ratio_max = 5 [default = 1.1];
+}
+
+// 模型参数配置
+message modelParams {
+
+}
+

proto/enum_type.proto

@@ -0,0 +1,39 @@
+syntax = "proto2";
+
+// 相机坐标
+enum DeviceAzimuth {
+  DEVICE_AZIMUTH_LF = 0;
+  DEVICE_AZIMUTH_RF = 1;
+  DEVICE_AZIMUTH_LR = 2;
+  DEVICE_AZIMUTH_RR = 3;
+  DEVICE_AZIMUTH_MAX = 4;
+}
+
+// 超界状态
+enum Range_status {
+    Range_correct = 0x0000;                 // 未超界
+    Range_front = 0x0001;                   // 前超界
+    Range_back = 0x0002;                    // 后超界
+    Range_left = 0x0004;                    // 左超界
+    Range_right = 0x0008;                   // 右超界
+    Range_bottom = 0x0010;                  // 底盘超界
+    Range_top = 0x0020;                     // 车顶超界
+    Range_car_width = 0x0040;               // 车宽超界
+    Range_car_wheelbase = 0x0080;           // 轴距超界
+    Range_angle_anti_clock = 0x0100;        // 左(逆时针)旋转超界
+    Range_angle_clock = 0x0200;             // 右(顺时针)旋转超界
+    Range_steering_wheel_nozero = 0x0400;   // 方向盘未回正
+    Range_car_moving = 0x0800;              // 车辆移动为1,静止为0
+}
+
+// 测量结果
+enum MeasureStatu {
+    Measure_OK = 0;                 // 测量完成
+    Measure_Empty_Clouds = 1;       // 测量结果：测量区域数据为空
+    Measure_Failture = 2;           // 测量结果：测量区域数据无法检测出车辆
+    Measure_Border = 3;             // 测量结果：对于PLC, 车辆存在超界，具体超界状态对比Range_status
+    Measure_Terminal_Border = 4;    // 测量结果：对于终端, 车辆存在超界，具体超界状态对比Range_status
+    Lidar_Disconnect = 5;           // 测量雷达：失去连接
+
+    Measure_Statu_Max = 6;
+}

proto/measure_manager.proto

@@ -0,0 +1,14 @@
+syntax = "proto2";
+// 加载3D相机设备
+import "vzense.proto";
+// 加载算法测量
+import "detect_message.proto";
+// 加载通信配置
+import "communication.proto";
+
+message tof3dManagerParams
+{
+  repeated tof3dVzenseEtc vzense_tof3d_devices = 1;
+  required Yolov8ProcessParams yolo = 2;
+  required CommunicationManagerConfig communication = 3;
+}

vzense/tof3d_config.proto

@@ -1,13 +1,17 @@
 syntax = "proto2";
+import "enum_type.proto";
 
-enum DeviceAzimuth {
-  LF = 0;
-  RF = 1;
-  LR = 2;
-  RR = 3;
-  MAX = 4;
+// 相机点云坐标转换
+message CoordinateTransformation3D {
+  optional float x = 1 [default = 0];
+  optional float y = 2 [default = 0];
+  optional float z = 3 [default = 0];
+  optional float roll = 4 [default = 0];
+  optional float pitch = 5 [default = 0];
+  optional float yaw = 6 [default = 0];
 }
 
+// 相机内参
 message Tof3dVzenseBuiltInParams {
   optional uint32 work_mode = 1 [default = 0x00];
   optional uint32 irgmmgain = 2 [default = 255];
@@ -25,47 +29,11 @@ message Tof3dVzenseBuiltInParams {
   optional bool enable_hdr_mode = 15 [default = true];
 }
 
-message Yolov8ProcessParams {
-  // 保存图片设置
-  optional bool save_mat = 1 [default = false];
-  optional bool draw_rect = 2 [default = false];
-  optional float save_confidence = 3 [default = 0.8];
-  optional float save_aspect_ratio_min = 4 [default = 0.9];
-  optional float save_aspect_ratio_max = 5 [default = 1.1];
-}
-
-message RabbitmqCommunicationParams {
-  required bool enable_rabbitmq = 4;
-  required string rabbitmq_ex = 5;
-  required string rabbitmq_route_key = 6;
-}
-
-message MqttCommunicationParams {
-  required bool enable_mqtt = 7;
-  required string mqtt_topic = 8;
-}
-
-message CoordinateTransformation3D {
-  optional float x = 1 [default = 0];
-  optional float y = 2 [default = 0];
-  optional float z = 3 [default = 0];
-  optional float roll = 4 [default = 0];
-  optional float pitch = 5 [default = 0];
-  optional float yaw = 6 [default = 0];
-}
-
+// 相机参数
 message tof3dVzenseEtc {
-  required bool enable_device = 1;  // 设备启动才进行以下处理
+  optional bool enable_device = 1 [default = true];  // 设备启动才进行以下处理
   required string ip = 2 [default = "192.168.1.101"];
-  required Tof3dVzenseBuiltInParams bip = 3;
+  optional Tof3dVzenseBuiltInParams bip = 3;
   required DeviceAzimuth azimuth = 4;
   optional CoordinateTransformation3D trans= 5;
-  required Yolov8ProcessParams yolo = 6;
-//  optional RabbitmqCommunicationParams rabbitmq_param = 6;
-//  optional MqttCommunicationParams mqtt_param = 7;
-}
-
-message tof3dManagerParams
-{
-  repeated tof3dVzenseEtc vzense_tof3d_devices = 1;
 }

tof3DMain.cpp

@@ -4,7 +4,10 @@
 #include <iostream>
 #include "log/log.h"
 #include "vzense/device_tof3d.h"
-#include "communication/rabbitmq/rabbitmq_communication.h"
+#include "detect/detect_manager.h"
+#include "proto/measure_manager.pb.h"
+#include "protobuf/load_protobuf.hpp"
+#include "communication/communication_manager.h"
 
 int main(int argc, char * argv[]) {
     // 初始化日志系统
@@ -13,31 +16,33 @@ int main(int argc, char * argv[]) {
     // 初始化相机处理程序
     tof3dManagerParams tof3d_etc;
     if (loadProtobufFile(ETC_PATH PROJECT_NAME "/tof3d_manager.json", tof3d_etc) == SUCCESS) {
-        LOG(INFO) << tof3d_etc.DebugString();
+        auto ret = DeviceTof3D::iter()->Init(tof3d_etc.vzense_tof3d_devices());
+        if (ret != SUCCESS) {
+            LOG(ERROR) << ret.to_string();
+            return EXIT_FAILURE;
+        }
     } else {
-        LOG(WARNING) << "tof3dManager read param form %s error.", ETC_PATH PROJECT_NAME "/tof3d_manager.json";
+        LOG(WARNING) << "tof3dManager read param form " << ETC_PATH PROJECT_NAME "/tof3d_manager.json" << " error.";
         return EXIT_FAILURE;
     }
 
-    auto list = tof3d_etc.vzense_tof3d_devices();
-    std::map<std::string, tof3dVzenseEtc> device_mp;
-    for (auto &device: tof3d_etc.vzense_tof3d_devices()) {
-        device_mp.insert(std::pair<std::string, tof3dVzenseEtc>(device.ip(), device));
-    }
+    // 初始化测量程序
+    DetectManager::iter()->Init();
 
-    DeviceTof3D::iter()->Init(device_mp, true);
+    // 通信
+    CommunicationManager::iter()->Init(tof3d_etc.communication());
 
-    /*LOG(INFO) << "begain init rabbitmq.";
-    auto ret = RabbitmqCommunicationTof3D::get_instance_references().rabbitmq_init_from_protobuf(ETC_PATH"Tof3d/rabbitmq.prototxt");
-    if (ret != SUCCESS) {
-        LOG(ERROR) << "system communication mq init failed: " << ret.to_string();
-        return -1;
+    // 加载看门狗 TODO：添加看门狗，监视线程
+    LOG(INFO) << "---------- all init complate ----------";
+    while (true) {
+//        auto mat = DeviceTof3D::iter()->getDevicePointsMat(DeviceAzimuth::DEVICE_AZIMUTH_LF);
+//        if (mat.empty()) {
+//            LOG(INFO) << "get a empty mat";
+//        } else {
+//            cv::imshow("merge_mat", mat);
+//        }
+        usleep(100 * 1000);
     }
-    RabbitmqCommunicationTof3D::get_instance_references().set_encapsulate_status_cycle_time(100);
-*/
 
-    while (1) {
-        usleep(2000 * 1000);
-    }
     return EXIT_SUCCESS;
 }

vzense/back/device_tof3d_b.h

@@ -0,0 +1,197 @@
+#pragma once
+
+#include <thread>
+#include <boost/chrono/duration.hpp>
+#include <opencv2/opencv.hpp>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/approximate_voxel_grid.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/common/transforms.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include "error_code/error_code.hpp"
+#include "VzenseNebula_api.h"
+#include "log/log.h"
+#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 "proto/vzense.pb.h"
+
+#ifdef ENABLE_TENSORRT_DETECT
+#include "detect/tensorrt_detect/wheel-detector.h"
+#else
+#include "detect/onnx_detect/wheel-detector.h"
+#endif
+#include "detect/ceres_detect/car_pose_detector.h"
+#include "detect/ceres_detect/wheel_detector.h"
+#include "detect/ceres_detect/detect_wheel_ceres.h"
+
+class DeviceTof3D {
+public:
+    struct DetectResult {
+        WheelCeresDetector::WheelDetectResult wheel[DeviceAzimuth::MAX];
+        Car_pose_detector::CarDetectResult car;
+
+        void reset() {
+            for (int i = 0; i < DeviceAzimuth::MAX; i++) {
+                wheel[i].reset();
+            }
+            car.reset();
+        }
+
+        std::string info() {
+            std::string str = car.info();
+            return str;
+        }
+    };
+
+    struct tof3dVzenseInfo {
+        VzDeviceHandle handle = nullptr;
+        bool is_connect = false;
+        bool is_start_stream = false;
+        VzDeviceInfo info;
+        tof3dVzenseEtc etc;
+    };
+
+    using VzEtcMap = std::map<std::string, tof3dVzenseEtc>;
+    using tof3dVzenseInfoMap = std::map<std::string, tof3dVzenseInfo *>;
+public:
+    static DeviceTof3D *iter() {
+        static DeviceTof3D *instance = nullptr;
+        if (instance == nullptr) {
+            instance = new DeviceTof3D();
+        }
+        return instance;
+    }
+
+    ~DeviceTof3D() = default;
+
+    Error_manager Init(const VzEtcMap &tp_tof3d, bool search = true);
+
+    Error_manager reInit(const VzEtcMap &etc);
+
+    Error_manager SearchDevice(const double &time);
+
+    Error_manager ConnectDevice(const std::string &ip, bool open_stream = true);
+
+    Error_manager ConnectAllDevices(bool open_stream = true);
+
+    Error_manager ConnectAllEtcDevices(bool open_stream = true);
+
+    Error_manager DisConnectDevice(const std::string &ip);
+
+    Error_manager DisConnectAllEtcDevices();
+
+    Error_manager DisConnectAllDevices();
+
+    Error_manager getDepthFrame(const std::string &ip, VzFrame &depthFrame);
+
+    Error_manager getIrFrame(const std::string &ip, VzFrame &irFrame);
+
+    Error_manager getDepthAndIrPicture(const std::string &ip, VzFrame &depthFrame, VzFrame &irFrame);
+
+    Error_manager getDepthPointCloud(const std::string &ip, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    Error_manager
+    DepthFrame2PointCloud(const std::string &ip, VzFrame &depthFrame, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+
+    Error_manager updateTof3dEtc();
+
+    Error_manager setTof3dParams(const std::string &ip);
+
+    Error_manager getTof3dParams(const std::string &ip);
+
+    static Error_manager Frame2Mat(VzFrame &frame, cv::Mat &mat);
+
+protected:
+    DeviceTof3D() = default;
+
+    static Error_manager DepthFrame2Mat(VzFrame &frame, cv::Mat &mat);
+
+    static Error_manager IrFrame2Mat(VzFrame &frame, cv::Mat &mat);
+
+    void receiveFrameThread(const std::string &ip);
+
+    void detectThread();
+
+    static void HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex);
+
+    bool drawBox(cv::Mat &mat, cv::Rect &box, cv::Scalar &color, std::string className, float confidence);
+
+    Error_manager loadEtc(const VzEtcMap &etc);
+
+    void stopWorking();
+
+    // // 传入深度图，得到车轮在mat中的坐标
+    // Error_manager segmentTensorRT(cv::Mat &depth_mat, std::vector<cv::Point> &wheel_cv_cloud);
+
+    // 根据传入坐标，分离depth frame的数据点
+    Error_manager segFrame2CarAndWheel(VzFrame &depth_frame, std::vector<cv::Point> &wheel_cv_cloud,
+                                       pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud,
+                                       pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud);
+
+    // 车轮点云优化
+    Error_manager WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result);
+
+    // 车身点云优化
+    Error_manager CarPoseOptimize(pcl::visualization::PCLVisualizer &viewer, int *view_port, pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud, DetectResult &result);
+
+    Error_manager grpcVzenseMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, cv::Mat &mat);
+
+    Error_manager grpcVzensePointMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, cv::Mat &mat);
+
+    Error_manager grpcCloud(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud);
+
+private:
+    struct ThreadInfo {
+        std::thread *t;
+        Thread_condition *condit;
+
+        ThreadInfo() {
+            t = nullptr;
+            condit = nullptr;
+        }
+
+        ThreadInfo(std::thread *m_t, Thread_condition *m_condit) : t(m_t), condit(m_condit) {}
+    };
+
+    struct DeviceMat {
+        cv::Mat depthMat;
+        cv::Mat irMat;
+        cv::Mat pointMat;
+
+        DeviceMat() {
+            irMat = cv::Mat::zeros(480, 640, CV_8UC1);
+            depthMat = cv::Mat::zeros(480, 640, CV_16UC1);
+            pointMat = cv::Mat::zeros(480, 640, CV_32FC4);
+        }
+
+        DeviceMat& operator=(const DeviceMat &p) {
+            this->depthMat = p.depthMat.clone();
+            this->pointMat = p.pointMat.clone();
+            this->irMat = p.irMat.clone();
+            return *this;
+        }
+    };
+    TimedLockData<DeviceMat> m_device_mat[DeviceAzimuth::MAX];
+
+    VzReturnStatus m_vz_status = VzRetOthers;
+
+    tof3dVzenseInfoMap mp_device_info;
+    std::map<std::string, ThreadInfo> mp_thread_info;
+
+    ThreadInfo detect_thread;
+    TensorrtWheelDetector *detector;
+    StreamRpcServer m_grpc_server;
+
+    DetectResult m_detect_result_record;
+};
+
+

vzense/device_tof3d.h

@@ -10,60 +10,35 @@
 #include <pcl/filters/statistical_outlier_removal.h>
 #include <pcl/filters/approximate_voxel_grid.h>
 #include <pcl/filters/voxel_grid.h>
-#include <pcl/common/transforms.h>
 #include <pcl/visualization/pcl_visualizer.h>
 
+#include "log/log.h"
 #include "error_code/error_code.hpp"
 #include "VzenseNebula_api.h"
-#include "log/log.h"
 #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 "tof3d_config.pb.h"
-
-#ifdef ENABLE_TENSORRT_DETECT
-#include "detect/tensorrt_detect/wheel-detector.h"
-#else
-
-#include "detect/onnx_detect/wheel-detector.h"
-
-#endif
-#include "detect/ceres_detect/car_pose_detector.h"
-#include "detect/ceres_detect/wheel_detector.h"
-#include "detect/ceres_detect/detect_wheel_ceres.h"
+#include "proto/vzense.pb.h"
+#include "communication/transitData.h"
 
 class DeviceTof3D {
 public:
-    struct DetectResult {
-        WheelCeresDetector::WheelDetectResult wheel[DeviceAzimuth::MAX];
-        Car_pose_detector::CarDetectResult car;
-
-        void reset() {
-            for (int i = 0; i < DeviceAzimuth::MAX; i++) {
-                wheel[i].reset();
-            }
-            car.reset();
-        }
+    struct DeviceTof3DSaveInfo {
+        cv::Mat irMat;      // 灰度图
+        cv::Mat pointMat;   // 点云坐标信息
 
-        std::string info() {
-            std::string str = car.info();
-            return str;
+        DeviceTof3DSaveInfo() {
+            irMat = cv::Mat::zeros(480, 640, CV_8UC1);
+            pointMat = cv::Mat::zeros(480, 640, CV_32FC4);
         }
-    };
 
-    struct tof3dVzenseInfo {
-        VzDeviceHandle handle = nullptr;
-        bool is_connect = false;
-        bool is_start_stream = false;
-        VzDeviceInfo info;
-        tof3dVzenseEtc etc;
+        DeviceTof3DSaveInfo& operator=(const DeviceTof3DSaveInfo &p) {
+            this->pointMat = p.pointMat.clone();
+            this->irMat = p.irMat.clone();
+            return *this;
+        }
     };
 
-    using VzEtcMap = std::map<std::string, tof3dVzenseEtc>;
-    using tof3dVzenseInfoMap = std::map<std::string, tof3dVzenseInfo *>;
 public:
     static DeviceTof3D *iter() {
         static DeviceTof3D *instance = nullptr;
@@ -75,125 +50,65 @@ public:
 
     ~DeviceTof3D() = default;
 
-    Error_manager Init(const VzEtcMap &tp_tof3d, bool search = true);
-
-    Error_manager reInit(const VzEtcMap &etc);
-
-    Error_manager SearchDevice(const double &time);
-
-    Error_manager ConnectDevice(const std::string &ip, bool open_stream = true);
-
-    Error_manager ConnectAllDevices(bool open_stream = true);
-
-    Error_manager ConnectAllEtcDevices(bool open_stream = true);
-
-    Error_manager DisConnectDevice(const std::string &ip);
+    Error_manager Init(const google::protobuf::RepeatedPtrField<tof3dVzenseEtc> &tof_devices);
 
-    Error_manager DisConnectAllEtcDevices();
+    Error_manager updateTrans(const DeviceAzimuth &azimuth, const CoordinateTransformation3D &trans);
 
-    Error_manager DisConnectAllDevices();
+    cv::Mat getDeviceIrMat();
 
-    Error_manager getDepthFrame(const std::string &ip, VzFrame &depthFrame);
+    cv::Mat getDevicePointsMat(const DeviceAzimuth &azimuth);
 
-    Error_manager getIrFrame(const std::string &ip, VzFrame &irFrame);
+    DeviceTof3DSaveInfo getDeviceSaveInfo(const DeviceAzimuth &azimuth);
 
-    Error_manager getDepthAndIrPicture(const std::string &ip, VzFrame &depthFrame, VzFrame &irFrame);
-
-    Error_manager getDepthPointCloud(const std::string &ip, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+    Error_manager getDeviceStatus();
+protected:
+//    bool
+    Error_manager SearchDevice(const double &time = 10);
 
-    Error_manager
-    DepthFrame2PointCloud(const std::string &ip, VzFrame &depthFrame, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
+    bool isAllDevicesFound(uint32_t deviceCount);
 
-    Error_manager updateTof3dEtc();
+    Error_manager setTof3dParams(VzDeviceHandle handle, const Tof3dVzenseBuiltInParams &params);
 
-    Error_manager setTof3dParams(const std::string &ip);
+    Error_manager getTof3dParams(VzDeviceHandle handle, Tof3dVzenseBuiltInParams &params);
 
-    Error_manager getTof3dParams(const std::string &ip);
+    Error_manager ConnectDevice(const DeviceAzimuth &azimuth);
 
-    static Error_manager Frame2Mat(VzFrame &frame, cv::Mat &mat);
+    Error_manager DisConnectDevice(VzDeviceHandle handle);
 
-protected:
-    DeviceTof3D() = default;
+    Error_manager getDepthAndIrPicture(VzDeviceHandle handle, VzFrame &depthFrame, VzFrame &irFrame);
 
     static Error_manager DepthFrame2Mat(VzFrame &frame, cv::Mat &mat);
 
     static Error_manager IrFrame2Mat(VzFrame &frame, cv::Mat &mat);
 
-    void receiveFrameThread(const std::string &ip);
-
-    void detectThread();
+    static Error_manager Frame2Mat(VzFrame &frame, cv::Mat &mat);
 
     static void HotPlugStateCallback(const VzDeviceInfo *pInfo, int status, void *contex);
 
-    bool drawBox(cv::Mat &mat, cv::Rect &box, cv::Scalar &color, std::string className, float confidence);
-
-    Error_manager loadEtc(const VzEtcMap &etc);
-
-    void stopWorking();
-
-    // // 传入深度图，得到车轮在mat中的坐标
-    // Error_manager segmentTensorRT(cv::Mat &depth_mat, std::vector<cv::Point> &wheel_cv_cloud);
-
-    // 根据传入坐标，分离depth frame的数据点
-    Error_manager segFrame2CarAndWheel(VzFrame &depth_frame, std::vector<cv::Point> &wheel_cv_cloud,
-                                       pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud,
-                                       pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud);
-
-    // 车轮点云优化
-    Error_manager WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result);
-
-    // 车身点云优化
-    Error_manager CarPoseOptimize(pcl::visualization::PCLVisualizer &viewer, int *view_port, pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud, DetectResult &result);
-
-    Error_manager grpcVzenseMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, cv::Mat &mat);
-
-    Error_manager grpcVzensePointMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, cv::Mat &mat);
-
-    Error_manager grpcCloud(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud);
-
+    void run(const DeviceAzimuth &azimuth);
 private:
-    struct ThreadInfo {
-        std::thread *t;
-        Thread_condition *condit;
-
-        ThreadInfo() {
-            t = nullptr;
-            condit = nullptr;
-        }
-
-        ThreadInfo(std::thread *m_t, Thread_condition *m_condit) : t(m_t), condit(m_condit) {}
+    struct DeviceTof3DStatusInfo {
+        bool connected = false;
+        bool found = false;
     };
 
-    struct DeviceMat {
-        cv::Mat depthMat;
-        cv::Mat irMat;
-        cv::Mat pointMat;
-
-        DeviceMat() {
-            irMat = cv::Mat::zeros(480, 640, CV_8UC1);
-            depthMat = cv::Mat::zeros(480, 640, CV_16UC1);
-            pointMat = cv::Mat::zeros(480, 640, CV_32FC4);
-        }
-
-        DeviceMat& operator=(const DeviceMat &p) {
-            this->depthMat = p.depthMat.clone();
-            this->pointMat = p.pointMat.clone();
-            this->irMat = p.irMat.clone();
-            return *this;
-        }
+    struct DeviceTof3DParamsInfo {
+        DeviceTof3DStatusInfo status;
+        std::thread *t = nullptr;
+        VzDeviceHandle handle = nullptr;
+        Thread_condition *condit = nullptr;
+        const tof3dVzenseEtc *etc = nullptr;
+        std::mutex *handle_mutex = new std::mutex();
     };
-    TimedLockData<DeviceMat> m_device_mat[DeviceAzimuth::MAX];
 
-    VzReturnStatus m_vz_status = VzRetOthers;
-
-    tof3dVzenseInfoMap mp_device_info;
-    std::map<std::string, ThreadInfo> mp_thread_info;
-
-    ThreadInfo detect_thread;
-    TensorrtWheelDetector *detector;
-    StreamRpcServer m_grpc_server;
+private:
+    TimedLockData<DeviceTof3DSaveInfo> m_device_mat[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
+    std::vector<DeviceTof3DParamsInfo> m_devices_list;
+    TimedLockData<int> m_devices_status[DeviceAzimuth::DEVICE_AZIMUTH_MAX];
 
-    DetectResult m_detect_result_record;
+    std::mutex rotation_mutex;
+    Eigen::Matrix3f rotation_matrix3[4];
+    Eigen::Vector3f move[4];
 };
 
 

test_data.md

@@ -20,4 +20,19 @@
 
 作后轮
 
-有后轮
+有后轮
+
+
+
+
+
+
+
+1、jetson开发板与tof相机通信断连问题
+
+​	与tof厂商联调
+
+6、算法复验
+
+
+