1、添加进程变量类；

2、添加grpc功能；
3、删除管理代码；

CMakeLists.txt

@@ -3,7 +3,7 @@ set(SON_PROJECT_NAME tof3d)
 message("========== Load son project ${SON_PROJECT_NAME} ==========" )
 
 unset(OPTION_ENABLE_TENSORRT_DETECT_CODE CACHE)
-option(OPTION_ENABLE_TENSORRT_DETECT_CODE "" ON)
+option(OPTION_ENABLE_TENSORRT_DETECT_CODE "" OFF)
 message("<=${SON_PROJECT_NAME}=> OPTION_ENABLE_TENSORRT_DETECT_CODE: " ${OPTION_ENABLE_TENSORRT_DETECT_CODE})
 
 if (OPTION_ENABLE_TENSORRT_DETECT_CODE)
@@ -56,6 +56,12 @@ else ()
     aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/detect/onnx detect)
 endif ()
 
+#find_package(absl REQUIRED)
+find_package(gRPC CONFIG REQUIRED)
+#find_package(PkgConfig REQUIRED)
+#pkg_search_module(GRPC REQUIRED grpc)
+#pkg_search_module(GRPCPP REQUIRED grpc++)
+
 option(OPTION_COMMUNICATION_WITH_PLC "plc通信" OFF)
 message("<=${SON_PROJECT_NAME}=> OPTION_COMMUNICATION_WITH_PLC: " ${OPTION_COMMUNICATION_WITH_PLC})
 
@@ -84,25 +90,26 @@ include_directories(
 
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication communication)
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/vzense vzense)
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/vzense/stream stream)
 
 set(SON_PROJECT_SOURCE_LIST ${SON_PROJECT_SOURCE_LIST}
         ${detect}
         ${communication}
         ${vzense}
-        ${CMAKE_CURRENT_LIST_DIR}/tof3d_manager.h
-        ${CMAKE_CURRENT_LIST_DIR}/tof3d_manager.cpp
-        ${CMAKE_CURRENT_LIST_DIR}/tof3d_config.pb.h
-        ${CMAKE_CURRENT_LIST_DIR}/tof3d_config.pb.cc
+        ${stream}
         ${CMAKE_CURRENT_LIST_DIR}/tof3DMain.cpp
 )
 
+message("${absl_LIBRARIES}")
 set(SON_PROJECT_DEPEND_LIST ${SON_PROJECT_DEPEND_LIST}
         liblog
         libthread
         librabbitmq
         libmessage
         ${NebulaSDK_PATH}/lib/libNebula_api.so
-        ${PROTOBUF_LIBRARIES}
+        ${absl_LIBRARIES}
+        protobuf::libprotobuf
+        gRPC::grpc++_reflection
         ${OpenCV_LIBRARIES}
         ${PCL_LIBRARIES}
         ${ALL_LIBS}

main.cpp

@@ -1,7 +0,0 @@
-#include <iostream>
-
-int main() {
-    printf("----------------------------------------------------------\n");
-    printf("----------------------------------------------------------\n");
-    return EXIT_SUCCESS;
-}

tof3DMain.cpp

@@ -3,13 +3,30 @@
 //
 #include <iostream>
 #include "log/log.h"
-#include "tof3d_manager.h"
+#include "vzense/device_tof3d.h"
 #include "communication/rabbitmq_communication.h"
 
 int main(int argc, char * argv[]) {
     // 初始化日志系统
     ZX::InitGlog(PROJECT_NAME, ETC_PATH PROJECT_NAME "/LogInfo/");
 
+    // 初始化相机处理程序
+    tof3dManagerParams tof3d_etc;
+    if (loadProtobufFile(ETC_PATH PROJECT_NAME "/tof3d_manager.json", tof3d_etc) == SUCCESS) {
+        LOG(INFO) << tof3d_etc.DebugString();
+    } else {
+        LOG(WARNING) << "tof3dManager read param form %s error.", ETC_PATH PROJECT_NAME "/tof3d_manager.json";
+        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));
+    }
+
+    DeviceTof3D::iter()->Init(device_mp, true);
+
     /*LOG(INFO) << "begain init rabbitmq.";
     auto ret = RabbitmqCommunicationTof3D::get_instance_references().rabbitmq_init_from_protobuf(ETC_PATH"Tof3d/rabbitmq.prototxt");
     if (ret != SUCCESS) {
@@ -18,12 +35,9 @@ int main(int argc, char * argv[]) {
     }
     RabbitmqCommunicationTof3D::get_instance_references().set_encapsulate_status_cycle_time(100);
 */
-    tof3dManager tof3d;
-    auto err = tof3d.init();
-    LOG(INFO) << err.to_string();
 
     while (1) {
         usleep(2000 * 1000);
     }
-    return err.get_error_code();
+    return EXIT_SUCCESS;
 }

tof3d_manager.cpp

@@ -1,23 +0,0 @@
-//
-// Created by zx on 2023/10/7.
-//
-
-#include "tof3d_manager.h"
-
-Error_manager tof3dManager::init() {
-    if (loadProtobufFile(ETC_PATH PROJECT_NAME "/tof3d_manager.json", m_tof_etc) == SUCCESS) {
-        LOG(INFO) << m_tof_etc.DebugString();
-    } else {
-        return {FAILED, NEGLIGIBLE_ERROR, "tof3dManager read param form %s error.", ETC_PATH PROJECT_NAME "/tof3d_manager.json"};
-    }
-
-    auto list = m_tof_etc.vzense_tof3d_devices();
-    std::map<std::string, tof3dVzenseEtc> device_mp;
-    for (auto &device: m_tof_etc.vzense_tof3d_devices()) {
-        device_mp.insert(std::pair<std::string, tof3dVzenseEtc>(device.ip(), device));
-    }
-
-    DeviceTof3D::iter()->Init(device_mp, true);
-
-    return {};
-}

tof3d_manager.h

@@ -1,19 +0,0 @@
-#pragma once
-
-#include "protobuf/load_protobuf.hpp"
-#include "tof3d_config.pb.h"
-#include "vzense/device_tof3d.h"
-#include "error_code/error_code.hpp"
-
-#include "communication/rabbitmq_communication.h"
-class tof3dManager {
-public:
-    tof3dManager() = default;
-    ~tof3dManager() = default;
-
-    Error_manager init();
-protected:
-
-private:
-    tof3dManagerParams m_tof_etc;
-};

vzense/device_tof3d.cpp

@@ -18,6 +18,8 @@ Error_manager DeviceTof3D::Init(const VzEtcMap &tp_tof3d, bool search) {
         SearchDevice(5);
     }
 
+    // 启动grpc服务
+    m_grpc_server.Listenning("192.168.2.55", 9876);
     cv::namedWindow("ret", cv::WINDOW_AUTOSIZE);
 
     ConnectAllEtcDevices();
@@ -443,72 +445,46 @@ Error_manager DeviceTof3D::IrFrame2Mat(VzFrame &frame, cv::Mat &mat) {
     return {};
 }
 
-#include <sys/stat.h>
+#include "file/pathcreator.h"
 
 void DeviceTof3D::receiveFrameThread(const std::string &ip) {
     LOG(INFO) << ip << " in thread " << std::this_thread::get_id();
     auto iter = mp_device_info.find(ip);
     auto t_iter = mp_thread_info.find(ip);
 
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_car_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_wheel_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-    std::vector<Object> segment_results;
-
-    const std::string path = ETC_PATH PROJECT_NAME "/image/";
-    std::vector<std::string> imagePathList;
-    int image_index = 0;
-   if (IsFile(path)) {
-        std::string suffix = path.substr(path.find_last_of('.') + 1);
-        if (suffix == "jpg" || suffix == "jpeg" || suffix == "png") {
-            imagePathList.push_back(path);
-        } else {
-            printf("suffix %s is wrong !!!\n", suffix.c_str());
-            std::abort();
-        }
-    } else if (IsFolder(path)) {
-        cv::glob(path + "/*.jpg", imagePathList);
-    }
-
     while (t_iter->second.condit->is_alive()) {
         t_iter->second.condit->wait();
         if (t_iter->second.condit->is_alive()) {
             Error_manager ret;
             if (iter->second->handle) {
-                t_car_cloud->clear();
-                t_wheel_cloud->clear();
+                DeviceMat depthInfo;
                 VzFrame depthFrame = {0};
-                cv::Mat depthMat(480, 640, CV_16UC1);
-                if (getDepthFrame(ip, depthFrame) == SUCCESS) {
-                    segment_results.clear();
-                    if (Frame2Mat(depthFrame, depthMat) == SUCCESS) {
-                        image_index = (image_index + 1) > (imagePathList.size() - 1) ? 0 : image_index + 1;
-                        depthMat = cv::imread(imagePathList[image_index]);
-                        detector->detect(depthMat, segment_results, depthMat);
-                        cv::imshow("ret", depthMat);
-                        cv::waitKey(1);
-                    }
 
-                    std::vector<cv::Point> mat_seg_ret;
-                    for (auto &result: segment_results) {
-                        LOG(INFO) << result.prob;
-                        if (result.prob > 0.5) {
-                            mat_seg_ret = detector->getPointsFromObj(result);
-                            for (auto &pt: mat_seg_ret) {
-                                depthMat.at<cv::Vec3b>(pt) = cv::Vec3b(255, 255, 255) - depthMat.at<cv::Vec3b>(pt);
-                            }
-                            cv::imshow("ret", depthMat);
-                            cv::waitKey(1);
+                // 图像获取
+                if (getDepthFrame(ip, depthFrame) == SUCCESS && Frame2Mat(depthFrame, depthInfo.depthMat) == SUCCESS) {
+                    // 点云转换
+                    VzSensorIntrinsicParameters cameraParam = {};
+                    VZ_GetSensorIntrinsicParameters(iter->second->handle, VzToFSensor, &cameraParam);
+
+                    const uint16_t *pDepthFrameData = (uint16_t *) depthFrame.pFrameData;
+                    for (int cols = 0; cols < depthInfo.pointMat.cols; cols++) {
+                        for (int rows = 0; rows < depthInfo.pointMat.rows; rows++) {
+                            VzDepthVector3 depthPoint = {cols, rows, pDepthFrameData[rows * depthInfo.pointMat.cols + cols]};
+                            VzVector3f worldV = {};
+                            VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
+                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] = worldV.x * 0.001;
+                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[1] = worldV.y * 0.001;
+                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[2] = worldV.z * 0.001;
+//                            if (depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0] > 1) {
+//                                LOG(INFO) << depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[0];
+//                            }
+                            depthInfo.pointMat.at<cv::Vec4f>(rows, cols)[3] = 0;
                         }
                     }
-
-                    if (!mat_seg_ret.empty()) {
-                        segFrame2CarAndWheel(depthFrame, mat_seg_ret, t_car_cloud, t_wheel_cloud);
-                        //                        viewer.removeAllPointClouds(view_port[0]);
-                        //                        viewer.removeAllPointClouds(view_port[1]);
-                        //                        viewer.addPointCloud(t_car_cloud, "car_cloud_", view_port[0]);
-                        //                        viewer.addPointCloud(t_wheel_cloud, "wheel_cloud_", view_port[1]);
-                        //                        viewer.spinOnce();
-                    }
+//                    cv::imshow(iter->first, depthInfo.pointMat);
+//                    cv::waitKey(1);
+                    // 将图片和点云存储起来
+                    m_device_mat[iter->second->etc.azimuth()].reset(depthInfo, 1);
                 } else {
                     ret.compare_and_merge_up({FAILED, MINOR_ERROR, "Device %s VZ_GetFrame failed.", ip.c_str()});
                 }
@@ -532,20 +508,35 @@ void DeviceTof3D::receiveFrameThread(const std::string &ip) {
 void DeviceTof3D::detectThread() {
     LOG(INFO) << "detect thread running in " << std::this_thread::get_id();
 
-    // int view_port[2];
-    // pcl::visualization::PCLVisualizer viewer("viewer_0");
-    // viewer.createViewPort(0.0, 0.0, 0.5, 1, view_port[0]);
-    // viewer.createViewPort(0.5, 0.0, 1, 1, view_port[1]);
-    // viewer.addText("car_cloud", 10, 10, 20, 0, 1, 0, "car_cloud", view_port[0]);
-    // viewer.addText("wheel_cloud", 10, 10, 20, 0, 1, 0, "wheel_cloud", view_port[1]);
-    // viewer.addCoordinateSystem(1, "car_axis", view_port[0]);
-    // viewer.addCoordinateSystem(1, "wheel_axis", view_port[1]);
+     int view_port[2];
+     pcl::visualization::PCLVisualizer viewer("viewer_0");
+     viewer.createViewPort(0.0, 0.0, 0.5, 1, view_port[0]);
+     viewer.createViewPort(0.5, 0.0, 1, 1, view_port[1]);
+     viewer.addText("car_cloud", 10, 10, 20, 0, 1, 0, "car_cloud", view_port[0]);
+     viewer.addText("wheel_cloud", 10, 10, 20, 0, 1, 0, "wheel_cloud", view_port[1]);
+     viewer.addCoordinateSystem(1, "car_axis", view_port[0]);
+     viewer.addCoordinateSystem(1, "wheel_axis", view_port[1]);
 
     pcl::PointCloud<pcl::PointXYZ>::Ptr t_car_cloud(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr t_wheel_cloud(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;
 
+    const std::string path = ETC_PATH PROJECT_NAME "/image/";
+    std::vector<std::string> imagePathList;
+    int image_index = 0;
+    if (PathCreator::IsFile(path)) {
+        std::string suffix = path.substr(path.find_last_of('.') + 1);
+        if (suffix == "jpg" || suffix == "jpeg" || suffix == "png") {
+            imagePathList.push_back(path);
+        } else {
+            printf("suffix %s is wrong !!!\n", suffix.c_str());
+            std::abort();
+        }
+    } else if (PathCreator::IsFolder(path)) {
+        cv::glob(path + "/*.jpg", imagePathList);
+    }
+
     while (detect_thread.condit->is_alive()) {
 //        detect_thread.condit->wait();
         t_car_cloud->clear();
@@ -556,15 +547,72 @@ void DeviceTof3D::detectThread() {
 //        LOG(INFO) << "last wheel cost time is " << cost.count() * 1000 << " ms";
 
         if (detect_thread.condit->is_alive()) {
+            DetectResult result;
+            ::JetStream::ResFrame frame;
+            t_car_cloud->clear();
+            t_wheel_cloud->clear();
+            DeviceMat t_device_mat[DeviceAzimuth::MAX];
+            for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
+                if (!m_device_mat[device_index].timeout()) {
+                    t_device_mat[device_index].empty = false;
+                    t_device_mat[device_index] = m_device_mat[device_index].Get();
+                    grpcVzenseMat(frame, (DeviceAzimuth)device_index, t_device_mat[device_index].depthMat);
+                }
+            }
 
+            for (int device_index = 0; device_index < DeviceAzimuth::MAX; device_index++) {
+                if (t_device_mat[device_index].empty) {continue;}
+
+                std::vector<Object> objs;
+//                detector->detect(t_device_mat[device_index].depthMat, objs);
+                image_index = image_index > imagePathList.size() - 2 ? 0 : image_index + 1;
+                t_device_mat[device_index].depthMat = cv::imread(imagePathList[image_index]);
+                detector->detect(t_device_mat[device_index].depthMat, objs);
+                if (objs.size() == 1 && objs[0].prob > 0.5   ) {
+                    LOG(INFO) << device_index << " objs[0].prob " << objs[0].prob;
+                    auto seg_points = detector->getPointsFromObj(objs[0]);
+                    for (auto &pt: seg_points) {
+                        t_device_mat[device_index].pointMat.at<cv::Vec4f>(pt)[3] = objs[0].prob;
+                    }
+                    LOG(INFO) << device_index << " grpcVzensePointMat";
+                    grpcVzensePointMat(frame, (DeviceAzimuth)device_index, t_device_mat[device_index].pointMat);
+                    LOG(INFO) << device_index << " grpcVzensePointMat";
+
+                    for (int rows = 0; rows < t_device_mat[device_index].pointMat.rows; rows++) {
+                        for (int cols = 0; cols < t_device_mat[device_index].pointMat.cols; cols++) {
+                            t_car_cloud->emplace_back(
+                                    t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[0],
+                                    t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[1],
+                                    t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[2]);
+                            if (t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[3] > 0) {
+                                t_wheel_cloud->emplace_back(
+                                        t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[0],
+                                        t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[1],
+                                        t_device_mat[device_index].pointMat.at<cv::Vec4f>(rows, cols)[2]);
+                            }
+                        }
+                    }
+                }
+            }
 
+            viewer.removeAllPointClouds(view_port[0]);
+            viewer.removeAllPointClouds(view_port[1]);
+            viewer.addPointCloud(t_car_cloud, "t_car_cloud", view_port[0]);
+            viewer.addPointCloud(t_wheel_cloud, "t_wheel_cloud", view_port[1]);
+            viewer.spinOnce();
 //            cost = std::chrono::steady_clock::now() - t_start_time;
 //            LOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
-            if (!t_car_cloud->empty() && !t_wheel_cloud->empty()) {
-                CarPoseOptimize(t_car_cloud);
-                WheelCloudOptimize(t_wheel_cloud);
-            }
+//            if (!t_car_cloud->empty() && !t_wheel_cloud->empty()) {
+//                CarPoseOptimize(t_car_cloud);
+//                WheelCloudOptimize(t_wheel_cloud);
+//            }
 
+            frame.mutable_measure_info()->set_x(1);
+            frame.mutable_measure_info()->set_y(1);
+            frame.mutable_measure_info()->set_width(1);
+            frame.mutable_measure_info()->set_wheelbase(1);
+            frame.mutable_measure_info()->set_error("Just test.");
+            m_grpc_server.ResetData(frame);
         }
     }
     LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
@@ -827,7 +875,7 @@ Error_manager DeviceTof3D::segFrame2CarAndWheel(VzFrame &depth_frame, std::vecto
             VZ_ConvertDepthToPointCloud(iter->second->handle, &depthPoint, &worldV, 1, &cameraParam);
             wheel_cloud->emplace_back(worldV.x, worldV.y, worldV.z);
         }
-        LOG(INFO) << wheel_cloud->size();
+//        LOG(INFO) << wheel_cloud->size();
 
         VzFrame &srcFrame = depth_frame;
         const int len = srcFrame.width * srcFrame.height;
@@ -846,7 +894,7 @@ Error_manager DeviceTof3D::segFrame2CarAndWheel(VzFrame &depth_frame, std::vecto
             }
             delete[] worldV;
             worldV = nullptr;
-            LOG(INFO) << "Save point cloud successful to cloud: " << car_cloud->size();
+//            LOG(INFO) << "Save point cloud successful to cloud: " << car_cloud->size();
         }
 
 //        for (int i = (depth_frame.height - WINDOW_SIZE)/2, offset = i * depth_frame.width; i < (depth_frame.height + WINDOW_SIZE)/2; i++)
@@ -874,3 +922,71 @@ Error_manager DeviceTof3D::WheelCloudOptimize(pcl::PointCloud<pcl::PointXYZ>::Pt
 Error_manager DeviceTof3D::CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud) {
     return Error_manager();
 }
+
+Error_manager DeviceTof3D::grpcVzenseMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth,cv::Mat &mat) {
+    switch (azimuth) {
+        case LF:
+            frame.mutable_images()->mutable_img1()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
+            break;
+        case RF:
+            frame.mutable_images()->mutable_img2()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
+            break;
+        case LR:
+            frame.mutable_images()->mutable_img3()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
+            break;
+        case RR:
+            frame.mutable_images()->mutable_img4()->CopyFrom(StreamRpcServer::cvMat2JetImage(mat));
+            break;
+        default:
+            break;
+    }
+    return {};
+}
+
+Error_manager DeviceTof3D::grpcVzensePointMat(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth,cv::Mat &mat) {
+    switch (azimuth) {
+        case LF:
+            frame.mutable_clouds()->mutable_cloud1()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
+            LOG(INFO) << "frame.clouds().cloud1().size(): " << frame.clouds().cloud1().size();
+            break;
+        case RF:
+            frame.mutable_clouds()->mutable_cloud2()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
+            LOG(INFO) << "frame.clouds().cloud2().size(): " << frame.clouds().cloud2().size();
+            break;
+        case LR:
+            frame.mutable_clouds()->mutable_cloud3()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
+            LOG(INFO) << "frame.clouds().cloud3().size(): " << frame.clouds().cloud3().size();
+            break;
+        case RR:
+            frame.mutable_clouds()->mutable_cloud4()->CopyFrom(StreamRpcServer::cvPointMat2JetPointCloud(mat));
+            LOG(INFO) << "frame.clouds().cloud4().size(): " << frame.clouds().cloud4().size();
+            break;
+        default:
+            break;
+    }
+    return {};
+}
+
+Error_manager DeviceTof3D::grpcCloud(JetStream::ResFrame &frame, const DeviceAzimuth &azimuth, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
+    // 传递识别结果
+    switch (azimuth) {
+        case LF:
+            frame.mutable_clouds()->mutable_cloud1()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
+            break;
+        case RF:
+            frame.mutable_clouds()->mutable_cloud2()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
+            break;
+        case LR:
+            frame.mutable_clouds()->mutable_cloud3()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
+            break;
+        case RR:
+            frame.mutable_clouds()->mutable_cloud4()->CopyFrom(StreamRpcServer::pcl2JetPointCloud(cloud));
+            break;
+        default:
+            break;
+    }
+
+    return {};
+}
+
+

vzense/device_tof3d.h

@@ -10,18 +10,26 @@
 #include "VzenseNebula_api.h"
 #include "log/log.h"
 #include "thread/thread_condition.h"
+#include "stream/streamServer.h"
+#include "tool/timedlockdata.hpp"
 
 #include "tof3d_config.pb.h"
 
 #ifdef ENABLE_TENSORRT_DETECT
 #include "detect/tensorrt/wheel-detector.h"
 #else
+
 #include "detect/onnx/wheel-detector.h"
+
 #endif
 
 
 class DeviceTof3D {
 public:
+    struct DetectResult {
+
+    };
+
     struct tof3dVzenseInfo {
         VzDeviceHandle handle = nullptr;
         bool is_connect = false;
@@ -31,10 +39,10 @@ public:
     };
 
     using VzEtcMap = std::map<std::string, tof3dVzenseEtc>;
-    using tof3dVzenseInfoMap = std::map<std::string, tof3dVzenseInfo*>;
+    using tof3dVzenseInfoMap = std::map<std::string, tof3dVzenseInfo *>;
 public:
-    static DeviceTof3D* iter() {
-        static DeviceTof3D* instance = nullptr;
+    static DeviceTof3D *iter() {
+        static DeviceTof3D *instance = nullptr;
         if (instance == nullptr) {
             instance = new DeviceTof3D();
         }
@@ -46,17 +54,17 @@ public:
     // TODO
     void run();
 
-    Error_manager Init(const VzEtcMap &tp_tof3d, bool search= true);
+    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 ConnectDevice(const std::string &ip, bool open_stream = true);
 
-    Error_manager ConnectAllDevices(bool open_stream=true);
+    Error_manager ConnectAllDevices(bool open_stream = true);
 
-    Error_manager ConnectAllEtcDevices(bool open_stream=true);
+    Error_manager ConnectAllEtcDevices(bool open_stream = true);
 
     Error_manager DisConnectDevice(const std::string &ip);
 
@@ -72,7 +80,8 @@ public:
 
     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
+    DepthFrame2PointCloud(const std::string &ip, VzFrame &depthFrame, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud);
 
     Error_manager updateTof3dEtc();
 
@@ -105,21 +114,54 @@ protected:
     // 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 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(pcl::PointCloud<pcl::PointXYZ>::Ptr &wheel_cloud);
 
     // 车身点云优化
     Error_manager CarPoseOptimize(pcl::PointCloud<pcl::PointXYZ>::Ptr &car_cloud);
+
+    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;
+        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) {}
+        ThreadInfo() {
+            t = nullptr;
+            condit = nullptr;
+        }
+
+        ThreadInfo(std::thread *m_t, Thread_condition *m_condit) : t(m_t), condit(m_condit) {}
     };
+
+    struct DeviceMat {
+        bool empty;
+        cv::Mat depthMat;
+        cv::Mat pointMat;
+
+        DeviceMat() {
+            empty = true;
+            depthMat.create(480, 640, CV_16UC1);
+            pointMat.create(480, 640, CV_32FC4);
+        }
+
+        DeviceMat& operator=(const DeviceMat &p) {
+            this->depthMat = p.depthMat.clone();
+            this->pointMat = p.pointMat.clone();
+            return *this;
+        }
+    };
+    TimedLockData<DeviceMat> m_device_mat[DeviceAzimuth::MAX];
+
     VzReturnStatus m_vz_status = VzRetOthers;
 
     tof3dVzenseInfoMap mp_device_info;
@@ -127,6 +169,7 @@ private:
 
     ThreadInfo detect_thread;
     TensorrtWheelDetector *detector;
+    StreamRpcServer m_grpc_server;
 };
 
 

vzense/stream/def.grpc.proto

@@ -0,0 +1,63 @@
+syntax = "proto3";
+package JetStream;
+
+message RequestCmd
+{
+  int32 Id=1;  // 对于获取对应id的相机数据。1,2,3,4,其他表示所有
+                //对于打开视频流
+}
+
+message Response{
+  string info=2;
+}
+
+message Image{
+  int32 width=1;
+  int32 height=2;
+  int32 channel=3;
+  bytes data=4;
+}
+
+message PointCloud{
+  int32 size=1;
+  bytes data=2;
+}
+
+message ResImage{
+  Image img1=1;
+  Image img2=2;
+  Image img3=3;
+  Image img4=4;
+}
+message ResCloud{
+  PointCloud cloud1=1;
+  PointCloud cloud2=2;
+  PointCloud cloud3=3;
+  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;
+}
+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){}
+}

vzense/stream/streamServer.cpp

@@ -0,0 +1,255 @@
+
+#include "streamServer.h"
+#include <unistd.h>
+CustomServer::CustomServer(){
+    update_=false;
+}
+
+CustomServer::~CustomServer(){
+    openDataStream_=false;
+    openImageStream_=true;
+}
+
+
+void CustomServer::update(const ::JetStream::ResFrame& frame){
+    frame_.CopyFrom(frame);
+    update_=true;
+}
+
+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;
+
+}
+
+
+::grpc::Status CustomServer::OpenMeasureDataStream(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
+            ::grpc::ServerWriter< ::JetStream::MeasureInfo>* writer){
+    openDataStream_=true;
+    printf("打开测量数据流\n");
+    while(openDataStream_){
+        if(waitForUpdata(0.3)){
+            writer->Write(frame_.measure_info());
+        }else{
+            ::JetStream::MeasureInfo info;
+            info.set_error("没有数据");
+            writer->Write(info);
+        }
+
+        if(context->IsCancelled()){
+            printf("Data Stream canceled\n");
+            return ::grpc::Status::OK;
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+    printf("关闭测量数据流\n");
+    return ::grpc::Status::OK;
+}
+
+
+::grpc::Status CustomServer::OpenImageStream(::grpc::ServerContext* context, 
+            const ::JetStream::RequestCmd* request, ::grpc::ServerWriter< ::JetStream::ResImage>* writer){
+
+    openImageStream_=true;
+    printf("打开视频流\n");
+    while(openImageStream_){
+        if(update_){
+        
+            writer->Write(frame_.images());
+            update_=false;
+        }
+        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;
+
+}
+
+
+::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(0.3)){
+        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(0.3)==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;
+    }
+    printf("frame_.clouds().cloud2().size() %d\n", frame_.clouds().cloud2().size());
+    return ::grpc::Status::OK;
+
+}
+
+::grpc::Status CustomServer::GetImage(::grpc::ServerContext* context, const ::JetStream::RequestCmd* request, 
+            ::JetStream::ResImage* response){
+    if(waitForUpdata(0.3)==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;
+}
+
+
+
+StreamRpcServer::StreamRpcServer(){
+    thread_=nullptr;
+}
+StreamRpcServer::~StreamRpcServer(){
+    
+    if(thread_!= nullptr){
+        server_->Shutdown();
+        if(thread_->joinable()){
+            thread_->join();
+        delete thread_;
+        }
+    }
+}
+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;
+    printf(" listenning ........\n");
+    server->server_->Wait();
+    printf(" server closesd.....\n");
+}
+
+void StreamRpcServer::ResetData(const ::JetStream::ResFrame& frame){
+    service_.update(frame);
+}
+
+
+::JetStream::Image StreamRpcServer::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());
+    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;
+}
+
+::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));
+        }
+    }
+
+    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));
+
+    ::JetStream::PointCloud cloud;
+    cloud.set_size(size);
+    cloud.set_data(data,size*4*sizeof(short));
+    free(data);
+
+    return cloud;
+}

vzense/stream/streamServer.h

@@ -0,0 +1,65 @@
+#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"
+
+class CustomServer :public JetStream::StreamServer::Service
+{
+private:
+    
+    bool openDataStream_=false;
+    bool openImageStream_=false;
+    ::JetStream::ResFrame frame_;
+    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);
+
+protected:
+    bool waitForUpdata(float tm);
+};
+
+
+class StreamRpcServer{
+
+private:
+    grpc::ServerBuilder builder_;
+    CustomServer service_;
+    std::unique_ptr<grpc::Server> server_;
+    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);
+
+protected:
+    static void WaitThread(void* p);
+};
+
+#endif

tof3d_config.proto

@@ -1,10 +1,11 @@
 syntax = "proto2";
 
 enum DeviceAzimuth {
-  LF = 1;
-  RF = 2;
-  LR = 3;
-  RR = 4;
+  LF = 0;
+  RF = 1;
+  LR = 2;
+  RR = 3;
+  MAX = 4;
 }
 
 message Tof3dVzenseBuiltInParams {