Merge branch 'master' of http://111.33.136.149:3000/adc_pilot/modularization

src/detection/detection_localization_LeGO_LOAM/.gitignore

@@ -0,0 +1,73 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/detection/detection_localization_LeGO_LOAM/detection_localization_LeGO_LOAM.pro

@@ -0,0 +1,33 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if you use deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += main.cpp \
+    imageProjection.cpp
+
+HEADERS += \
+    utility.h
+
+!include(../../../include/common.pri ) {
+    error( "Couldn't find the common.pri file!" )
+}
+
+!include(../../../include/ivopencv.pri ) {
+    error( "Couldn't find the ivopencv.pri file!" )
+}
+
+!include(../../../include/ivpcl.pri ) {
+    error( "Couldn't find the ivpcl.pri file!" )
+}

src/detection/detection_localization_LeGO_LOAM/imageProjection.cpp

@@ -0,0 +1,546 @@
+// Copyright 2013, Ji Zhang, Carnegie Mellon University
+// Further contributions copyright (c) 2016, Southwest Research Institute
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// 1. Redistributions of source code must retain the above copyright notice,
+//    this list of conditions and the following disclaimer.
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+//    this list of conditions and the following disclaimer in the documentation
+//    and/or other materials provided with the distribution.
+// 3. Neither the name of the copyright holder nor the names of its
+//    contributors may be used to endorse or promote products derived from this
+//    software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// This is an implementation of the algorithm described in the following papers:
+//   J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time.
+//     Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014.
+//   T. Shan and B. Englot. LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain
+//      IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). October 2018.
+
+#include "utility.h"
+
+namespace iv {
+
+struct SegInfo
+{
+    int startRingIndex[N_SCAN];
+    int endRingIndex[N_SCAN];
+
+    float startOrientation;
+    float endOrientation;
+    float orientationDiff;
+
+    bool   segmentedCloudGroundFlag[N_SCAN*Horizon_SCAN]; // true - ground point, false - other points
+    unsigned int  segmentedCloudColInd[N_SCAN*Horizon_SCAN]; // point column index in range image
+    float segmentedCloudRange[N_SCAN*Horizon_SCAN]; // point range
+};
+}
+
+class ImageProjection{
+private:
+
+//    ros::NodeHandle nh;
+
+//    ros::Subscriber subLaserCloud;
+    
+//    ros::Publisher pubFullCloud;
+//    ros::Publisher pubFullInfoCloud;
+
+//    ros::Publisher pubGroundCloud;
+//    ros::Publisher pubSegmentedCloud;
+//    ros::Publisher pubSegmentedCloudPure;
+//    ros::Publisher pubSegmentedCloudInfo;
+//    ros::Publisher pubOutlierCloud;
+
+    pcl::PointCloud<PointType>::Ptr laserCloudIn;
+    pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;
+
+    pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
+    pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range
+
+    pcl::PointCloud<PointType>::Ptr groundCloud;
+    pcl::PointCloud<PointType>::Ptr segmentedCloud;
+    pcl::PointCloud<PointType>::Ptr segmentedCloudPure;
+    pcl::PointCloud<PointType>::Ptr outlierCloud;
+
+    PointType nanPoint; // fill in fullCloud at each iteration
+
+    cv::Mat rangeMat; // range matrix for range image
+    cv::Mat labelMat; // label matrix for segmentaiton marking
+    cv::Mat groundMat; // ground matrix for ground cloud marking
+    int labelCount;
+
+    float startOrientation;
+    float endOrientation;
+
+    iv::SegInfo segMsg;
+//    cloud_msgs::cloud_info segMsg; // info of segmented cloud
+//    std_msgs::Header cloudHeader;
+
+    std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
+
+    uint16_t *allPushedIndX; // array for tracking points of a segmented object
+    uint16_t *allPushedIndY;
+
+    uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
+    uint16_t *queueIndY;
+
+public:
+    ImageProjection()
+    {
+ //   ImageProjection():
+//        nh("~"){
+
+//        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
+
+//        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);
+//        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);
+
+//        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);
+//        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud", 1);
+//        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud_pure", 1);
+//        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info> ("/segmented_cloud_info", 1);
+//        pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2> ("/outlier_cloud", 1);
+
+        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
+        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
+        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
+        nanPoint.intensity = -1;
+
+        allocateMemory();
+        resetParameters();
+    }
+
+    void allocateMemory(){
+
+        laserCloudIn.reset(new pcl::PointCloud<PointType>());
+        laserCloudInRing.reset(new pcl::PointCloud<PointXYZIR>());
+
+        fullCloud.reset(new pcl::PointCloud<PointType>());
+        fullInfoCloud.reset(new pcl::PointCloud<PointType>());
+
+        groundCloud.reset(new pcl::PointCloud<PointType>());
+        segmentedCloud.reset(new pcl::PointCloud<PointType>());
+        segmentedCloudPure.reset(new pcl::PointCloud<PointType>());
+        outlierCloud.reset(new pcl::PointCloud<PointType>());
+
+        fullCloud->points.resize(N_SCAN*Horizon_SCAN);
+        fullInfoCloud->points.resize(N_SCAN*Horizon_SCAN);
+
+//        segMsg.startRingIndex.assign(N_SCAN, 0);
+//        segMsg.endRingIndex.assign(N_SCAN, 0);
+
+//        segMsg.segmentedCloudGroundFlag.assign(N_SCAN*Horizon_SCAN, false);
+//        segMsg.segmentedCloudColInd.assign(N_SCAN*Horizon_SCAN, 0);
+//        segMsg.segmentedCloudRange.assign(N_SCAN*Horizon_SCAN, 0);
+
+        std::pair<int8_t, int8_t> neighbor;
+        neighbor.first = -1; neighbor.second =  0; neighborIterator.push_back(neighbor);
+        neighbor.first =  0; neighbor.second =  1; neighborIterator.push_back(neighbor);
+        neighbor.first =  0; neighbor.second = -1; neighborIterator.push_back(neighbor);
+        neighbor.first =  1; neighbor.second =  0; neighborIterator.push_back(neighbor);
+
+        allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN];
+        allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN];
+
+        queueIndX = new uint16_t[N_SCAN*Horizon_SCAN];
+        queueIndY = new uint16_t[N_SCAN*Horizon_SCAN];
+    }
+
+    void resetParameters(){
+        laserCloudIn->clear();
+        groundCloud->clear();
+        segmentedCloud->clear();
+        segmentedCloudPure->clear();
+        outlierCloud->clear();
+
+        rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
+        groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0));
+        labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0));
+        labelCount = 1;
+
+        std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint);
+        std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint);
+    }
+
+    ~ImageProjection(){}
+
+//    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
+
+//        cloudHeader = laserCloudMsg->header;
+//        // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
+//        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
+//        // Remove Nan points
+//        std::vector<int> indices;
+//        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
+//        // have "ring" channel in the cloud
+//        if (useCloudRing == true){
+//            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
+//            if (laserCloudInRing->is_dense == false) {
+//                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
+//                ros::shutdown();
+//            }
+//        }
+//    }
+    
+//    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
+    void cloudHandler(){
+
+        // 1. Convert ros message to pcl point cloud
+//        copyPointCloud(laserCloudMsg);
+        // 2. Start and end angle of a scan
+ //       findStartEndAngle();
+        // 3. Range image projection
+        projectPointCloud();
+        // 4. Mark ground points
+        groundRemoval();
+        // 5. Point cloud segmentation
+        cloudSegmentation();
+        // 6. Publish all clouds
+ //       publishCloud();
+        // 7. Reset parameters for next iteration
+        resetParameters();
+    }
+
+//    void findStartEndAngle(){
+//        // start and end orientation of this cloud
+//        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
+//        segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
+//                                                     laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
+//        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
+//            segMsg.endOrientation -= 2 * M_PI;
+//        } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
+//            segMsg.endOrientation += 2 * M_PI;
+//        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
+//    }
+
+    void projectPointCloud(){
+        // range image projection
+        float verticalAngle, horizonAngle, range;
+        size_t rowIdn, columnIdn, index, cloudSize; 
+        PointType thisPoint;
+
+        cloudSize = laserCloudIn->points.size();
+
+        for (size_t i = 0; i < cloudSize; ++i){
+
+            thisPoint.x = laserCloudIn->points[i].x;
+            thisPoint.y = laserCloudIn->points[i].y;
+            thisPoint.z = laserCloudIn->points[i].z;
+            // find the row and column index in the iamge for this point
+            if (useCloudRing == true){
+                rowIdn = laserCloudInRing->points[i].ring;
+            }
+            else{
+                verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
+                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
+            }
+            if (rowIdn < 0 || rowIdn >= N_SCAN)
+                continue;
+
+            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
+
+            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
+            if (columnIdn >= Horizon_SCAN)
+                columnIdn -= Horizon_SCAN;
+
+            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
+                continue;
+
+            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
+            if (range < sensorMinimumRange)
+                continue;
+            
+            rangeMat.at<float>(rowIdn, columnIdn) = range;
+
+            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
+
+            index = columnIdn  + rowIdn * Horizon_SCAN;
+            fullCloud->points[index] = thisPoint;
+            fullInfoCloud->points[index] = thisPoint;
+            fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
+        }
+    }
+
+
+    void groundRemoval(){
+        size_t lowerInd, upperInd;
+        float diffX, diffY, diffZ, angle;
+        // groundMat
+        // -1, no valid info to check if ground of not
+        //  0, initial value, after validation, means not ground
+        //  1, ground
+        for (size_t j = 0; j < Horizon_SCAN; ++j){
+            for (size_t i = 0; i < groundScanInd; ++i){
+
+                lowerInd = j + ( i )*Horizon_SCAN;
+                upperInd = j + (i+1)*Horizon_SCAN;
+
+                if (fullCloud->points[lowerInd].intensity == -1 ||
+                    fullCloud->points[upperInd].intensity == -1){
+                    // no info to check, invalid points
+                    groundMat.at<int8_t>(i,j) = -1;
+                    continue;
+                }
+                    
+                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
+                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
+                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
+
+                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
+
+                if (abs(angle - sensorMountAngle) <= 10){
+                    groundMat.at<int8_t>(i,j) = 1;
+                    groundMat.at<int8_t>(i+1,j) = 1;
+                }
+            }
+        }
+        // extract ground cloud (groundMat == 1)
+        // mark entry that doesn't need to label (ground and invalid point) for segmentation
+        // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
+        for (size_t i = 0; i < N_SCAN; ++i){
+            for (size_t j = 0; j < Horizon_SCAN; ++j){
+                if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
+                    labelMat.at<int>(i,j) = -1;
+                }
+            }
+        }
+//        if (pubGroundCloud.getNumSubscribers() != 0){
+//            for (size_t i = 0; i <= groundScanInd; ++i){
+//                for (size_t j = 0; j < Horizon_SCAN; ++j){
+//                    if (groundMat.at<int8_t>(i,j) == 1)
+//                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+//                }
+//            }
+//        }
+    }
+
+    void cloudSegmentation(){
+        // segmentation process
+        for (size_t i = 0; i < N_SCAN; ++i)
+            for (size_t j = 0; j < Horizon_SCAN; ++j)
+                if (labelMat.at<int>(i,j) == 0)
+                    labelComponents(i, j);
+
+        int sizeOfSegCloud = 0;
+        // extract segmented cloud for lidar odometry
+        for (size_t i = 0; i < N_SCAN; ++i) {
+
+            segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;
+
+            for (size_t j = 0; j < Horizon_SCAN; ++j) {
+                if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
+                    // outliers that will not be used for optimization (always continue)
+                    if (labelMat.at<int>(i,j) == 999999){
+                        if (i > groundScanInd && j % 5 == 0){
+                            outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+                            continue;
+                        }else{
+                            continue;
+                        }
+                    }
+                    // majority of ground points are skipped
+                    if (groundMat.at<int8_t>(i,j) == 1){
+                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
+                            continue;
+                    }
+                    // mark ground points so they will not be considered as edge features later
+                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
+                    // mark the points' column index for marking occlusion later
+                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
+                    // save range info
+                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
+                    // save seg cloud
+                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+                    // size of seg cloud
+                    ++sizeOfSegCloud;
+                }
+            }
+
+            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
+        }
+        
+        // extract segmented cloud for visualization
+//        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
+//            for (size_t i = 0; i < N_SCAN; ++i){
+//                for (size_t j = 0; j < Horizon_SCAN; ++j){
+//                    if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
+//                        segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
+//                        segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
+//                    }
+//                }
+//            }
+//        }
+    }
+
+    void labelComponents(int row, int col){
+        // use std::queue std::vector std::deque will slow the program down greatly
+        float d1, d2, alpha, angle;
+        int fromIndX, fromIndY, thisIndX, thisIndY; 
+        bool lineCountFlag[N_SCAN] = {false};
+
+        queueIndX[0] = row;
+        queueIndY[0] = col;
+        int queueSize = 1;
+        int queueStartInd = 0;
+        int queueEndInd = 1;
+
+        allPushedIndX[0] = row;
+        allPushedIndY[0] = col;
+        int allPushedIndSize = 1;
+        
+        while(queueSize > 0){
+            // Pop point
+            fromIndX = queueIndX[queueStartInd];
+            fromIndY = queueIndY[queueStartInd];
+            --queueSize;
+            ++queueStartInd;
+            // Mark popped point
+            labelMat.at<int>(fromIndX, fromIndY) = labelCount;
+            // Loop through all the neighboring grids of popped grid
+            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
+                // new index
+                thisIndX = fromIndX + (*iter).first;
+                thisIndY = fromIndY + (*iter).second;
+                // index should be within the boundary
+                if (thisIndX < 0 || thisIndX >= N_SCAN)
+                    continue;
+                // at range image margin (left or right side)
+                if (thisIndY < 0)
+                    thisIndY = Horizon_SCAN - 1;
+                if (thisIndY >= Horizon_SCAN)
+                    thisIndY = 0;
+                // prevent infinite loop (caused by put already examined point back)
+                if (labelMat.at<int>(thisIndX, thisIndY) != 0)
+                    continue;
+
+                d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
+                              rangeMat.at<float>(thisIndX, thisIndY));
+                d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
+                              rangeMat.at<float>(thisIndX, thisIndY));
+
+                if ((*iter).first == 0)
+                    alpha = segmentAlphaX;
+                else
+                    alpha = segmentAlphaY;
+
+                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
+
+                if (angle > segmentTheta){
+
+                    queueIndX[queueEndInd] = thisIndX;
+                    queueIndY[queueEndInd] = thisIndY;
+                    ++queueSize;
+                    ++queueEndInd;
+
+                    labelMat.at<int>(thisIndX, thisIndY) = labelCount;
+                    lineCountFlag[thisIndX] = true;
+
+                    allPushedIndX[allPushedIndSize] = thisIndX;
+                    allPushedIndY[allPushedIndSize] = thisIndY;
+                    ++allPushedIndSize;
+                }
+            }
+        }
+
+        // check if this segment is valid
+        bool feasibleSegment = false;
+        if (allPushedIndSize >= 30)
+            feasibleSegment = true;
+        else if (allPushedIndSize >= segmentValidPointNum){
+            int lineCount = 0;
+            for (size_t i = 0; i < N_SCAN; ++i)
+                if (lineCountFlag[i] == true)
+                    ++lineCount;
+            if (lineCount >= segmentValidLineNum)
+                feasibleSegment = true;            
+        }
+        // segment is valid, mark these points
+        if (feasibleSegment == true){
+            ++labelCount;
+        }else{ // segment is invalid, mark these points
+            for (size_t i = 0; i < allPushedIndSize; ++i){
+                labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
+            }
+        }
+    }
+
+    
+//    void publishCloud(){
+//        // 1. Publish Seg Cloud Info
+//        segMsg.header = cloudHeader;
+//        pubSegmentedCloudInfo.publish(segMsg);
+//        // 2. Publish clouds
+//        sensor_msgs::PointCloud2 laserCloudTemp;
+
+//        pcl::toROSMsg(*outlierCloud, laserCloudTemp);
+//        laserCloudTemp.header.stamp = cloudHeader.stamp;
+//        laserCloudTemp.header.frame_id = "base_link";
+//        pubOutlierCloud.publish(laserCloudTemp);
+//        // segmented cloud with ground
+//        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
+//        laserCloudTemp.header.stamp = cloudHeader.stamp;
+//        laserCloudTemp.header.frame_id = "base_link";
+//        pubSegmentedCloud.publish(laserCloudTemp);
+//        // projected full cloud
+//        if (pubFullCloud.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*fullCloud, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubFullCloud.publish(laserCloudTemp);
+//        }
+//        // original dense ground cloud
+//        if (pubGroundCloud.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*groundCloud, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubGroundCloud.publish(laserCloudTemp);
+//        }
+//        // segmented cloud without ground
+//        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubSegmentedCloudPure.publish(laserCloudTemp);
+//        }
+//        // projected full cloud info
+//        if (pubFullInfoCloud.getNumSubscribers() != 0){
+//            pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
+//            laserCloudTemp.header.stamp = cloudHeader.stamp;
+//            laserCloudTemp.header.frame_id = "base_link";
+//            pubFullInfoCloud.publish(laserCloudTemp);
+//        }
+//    }
+
+
+};
+
+
+
+
+//int main(int argc, char** argv){
+
+//    ros::init(argc, argv, "lego_loam");
+    
+//    ImageProjection IP;
+
+//    ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");
+
+//    ros::spin();
+//    return 0;
+//}

src/detection/detection_localization_LeGO_LOAM/main.cpp

@@ -0,0 +1,8 @@
+#include <QCoreApplication>
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    return a.exec();
+}

src/detection/detection_localization_LeGO_LOAM/utility.h

@@ -0,0 +1,191 @@
+#ifndef _UTILITY_LIDAR_ODOMETRY_H_
+#define _UTILITY_LIDAR_ODOMETRY_H_
+
+
+//#include <ros/ros.h>
+
+//#include <sensor_msgs/Imu.h>
+//#include <sensor_msgs/PointCloud2.h>
+//#include <nav_msgs/Odometry.h>
+
+//#include "cloud_msgs/cloud_info.h"
+
+#include <opencv2/opencv.hpp>
+//#include <opencv/cv.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+//#include <pcl_ros/point_cloud.h>
+//#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/range_image/range_image.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <pcl/common/common.h>
+#include <pcl/registration/icp.h>
+
+//#include <tf/transform_broadcaster.h>
+//#include <tf/transform_datatypes.h>
+ 
+#include <vector>
+#include <cmath>
+#include <algorithm>
+#include <queue>
+#include <deque>
+#include <iostream>
+#include <fstream>
+#include <ctime>
+#include <cfloat>
+#include <iterator>
+#include <sstream>
+#include <string>
+#include <limits>
+#include <iomanip>
+#include <array>
+#include <thread>
+#include <mutex>
+
+#define PI 3.14159265
+
+using namespace std;
+
+typedef pcl::PointXYZI  PointType;
+
+extern const string pointCloudTopic = "/velodyne_points";
+extern const string imuTopic = "/imu/data";
+
+// Save pcd
+extern const string fileDirectory = "/tmp/";
+
+// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
+extern const bool useCloudRing = true; // if true, ang_res_y and ang_bottom are not used
+
+// VLP-16
+extern const int N_SCAN = 16;
+extern const int Horizon_SCAN = 1800;
+extern const float ang_res_x = 0.2;
+extern const float ang_res_y = 2.0;
+extern const float ang_bottom = 15.0+0.1;
+extern const int groundScanInd = 7;
+
+// HDL-32E
+// extern const int N_SCAN = 32;
+// extern const int Horizon_SCAN = 1800;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 41.33/float(N_SCAN-1);
+// extern const float ang_bottom = 30.67;
+// extern const int groundScanInd = 20;
+
+// VLS-128
+// extern const int N_SCAN = 128;
+// extern const int Horizon_SCAN = 1800;
+// extern const float ang_res_x = 0.2;
+// extern const float ang_res_y = 0.3;
+// extern const float ang_bottom = 25.0;
+// extern const int groundScanInd = 10;
+
+// Ouster users may need to uncomment line 159 in imageProjection.cpp
+// Usage of Ouster imu data is not fully supported yet (LeGO-LOAM needs 9-DOF IMU), please just publish point cloud data
+// Ouster OS1-16
+// extern const int N_SCAN = 16;
+// extern const int Horizon_SCAN = 1024;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 33.2/float(N_SCAN-1);
+// extern const float ang_bottom = 16.6+0.1;
+// extern const int groundScanInd = 7;
+
+// Ouster OS1-64
+// extern const int N_SCAN = 64;
+// extern const int Horizon_SCAN = 1024;
+// extern const float ang_res_x = 360.0/float(Horizon_SCAN);
+// extern const float ang_res_y = 33.2/float(N_SCAN-1);
+// extern const float ang_bottom = 16.6+0.1;
+// extern const int groundScanInd = 15;
+
+extern const bool loopClosureEnableFlag = false;
+extern const double mappingProcessInterval = 0.3;
+
+extern const float scanPeriod = 0.1;
+extern const int systemDelay = 0;
+extern const int imuQueLength = 200;
+
+extern const float sensorMinimumRange = 1.0;
+extern const float sensorMountAngle = 0.0;
+extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
+extern const int segmentValidPointNum = 5;
+extern const int segmentValidLineNum = 3;
+extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
+extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI;
+
+
+extern const int edgeFeatureNum = 2;
+extern const int surfFeatureNum = 4;
+extern const int sectionsTotal = 6;
+extern const float edgeThreshold = 0.1;
+extern const float surfThreshold = 0.1;
+extern const float nearestFeatureSearchSqDist = 25;
+
+
+// Mapping Params
+extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)
+extern const int   surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
+// history key frames (history submap for loop closure)
+extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
+extern const int   historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure
+extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
+
+extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized
+
+
+struct smoothness_t{ 
+    float value;
+    size_t ind;
+};
+
+struct by_value{ 
+    bool operator()(smoothness_t const &left, smoothness_t const &right) { 
+        return left.value < right.value;
+    }
+};
+
+/*
+    * A point cloud type that has "ring" channel
+    */
+struct PointXYZIR
+{
+    PCL_ADD_POINT4D
+    PCL_ADD_INTENSITY;
+    uint16_t ring;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
+                                   (float, x, x) (float, y, y)
+                                   (float, z, z) (float, intensity, intensity)
+                                   (uint16_t, ring, ring)
+)
+
+/*
+    * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp)
+    */
+struct PointXYZIRPYT
+{
+    PCL_ADD_POINT4D
+    PCL_ADD_INTENSITY;
+    float roll;
+    float pitch;
+    float yaw;
+    double time;
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
+                                   (float, x, x) (float, y, y)
+                                   (float, z, z) (float, intensity, intensity)
+                                   (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
+                                   (double, time, time)
+)
+
+typedef PointXYZIRPYT  PointTypePose;
+
+#endif

src/driver/driver_cloud_grpc_civetweb/driver_cloud_grpc_civetweb.pro

@@ -24,7 +24,12 @@ SOURCES += main.cpp \
     ../../include/msgtype/rawpic.pb.cc \
     grpccivet.cpp \
     ../../../thirdpartylib/civetweb/CivetServer.cpp \
-    ../../../thirdpartylib/civetweb/civetweb.c
+    ../../../thirdpartylib/civetweb/civetweb.c \
+    pichandler.cpp \
+    starthandler.cpp \
+    remotehandler.cpp \
+    ../../include/msgtype/gpsimu.pb.cc \
+    ../../include/msgtype/remotectrl.pb.cc
 
 
 !include(../../../include/common.pri ) {
@@ -51,7 +56,12 @@ HEADERS += \
     ../../include/msgtype/rawpic.pb.h \
     grpccivet.h \
     ../../../thirdpartylib/civetweb/CivetServer.h \
-    ../../../thirdpartylib/civetweb/civetweb.h
+    ../../../thirdpartylib/civetweb/civetweb.h \
+    pichandler.h \
+    starthandler.h \
+    remotehandler.h \
+    ../../include/msgtype/gpsimu.pb.h \
+    ../../include/msgtype/remotectrl.pb.h
 
 INCLUDEPATH += $$PWD/../../../thirdpartylib/civetweb
 DEFINES += NO_SSL

src/driver/driver_cloud_grpc_civetweb/frontend/dist/baidumap.js

@@ -0,0 +1,298 @@
+
+//document.getElementById("demo").innerHTML='hello baidu map';
+  // 百度地图API功能
+    var map = new BMap.Map("allmap");            // 创建Map实例
+
+    //添加拖拽和缩放功能
+    map.enableScrollWheelZoom(true);
+    map.enableDragging();   
+
+    //添加控件和比例尺
+    var top_right_control = new BMap.ScaleControl({ anchor: BMAP_ANCHOR_BOTTOM_LEFT });// 左下角，添加比例尺
+    var top_right_navigation = new BMap.NavigationControl({ anchor: BMAP_ANCHOR_BOTTOM_LEFT });  //左下角，添加默认缩放平移控件
+
+    map.addControl(top_right_control);
+    map.addControl(top_right_navigation);
+
+
+    //添加地图类型
+    var mapType1 = new BMap.MapTypeControl({ mapTypes: [BMAP_NORMAL_MAP, BMAP_HYBRID_MAP] });
+    var mapType2 = new BMap.MapTypeControl({ anchor: BMAP_ANCHOR_TOP_LEFT });
+
+    //添加地图类型和缩略图
+   
+    map.addControl(mapType1);          //2D图，卫星图
+    map.addControl(mapType2);          //左上角，默认地图控件
+
+    var drawtrail = 0;
+
+    var grotation = 0;
+
+
+    //创建点
+    //map.clearOverlays();
+    var point = new BMap.Point(117.355, 39.066);
+    var markerCur = new BMap.Marker(point);
+    var markerObj = new BMap.Marker(point);
+    var havepointlast = 0;
+    var pointlast = new BMap.Point(116,39);
+    var pointobj = new BMap.Point(116,39);
+    var bobjset = 0;
+    var gtracelist = []; 
+    var gtraceraw = [];
+    var tracenow = 0;
+    var tracenum = 0;
+    map.centerAndZoom(point, 18);
+    var convertor = new BMap.Convertor();
+    var polylineTrace;
+/*
+    var polylineTrace =new BMap.Polyline(gtracelist, {
+    enableEditing: false,//是否启用线编辑，默认为false
+    enableClicking: false,//是否响应点击事件，默认为true
+    strokeWeight:'4',//折线的宽度，以像素为单位
+    strokeOpacity: 0.8,//折线的透明度，取值范围0 - 1
+    strokeColor:"red" //折线颜色
+    });*/
+    //var marker = new BMap.Marker(point);  // 创建标注
+    //map.addOverlay(marker);               // 将标注添加到地图中
+    
+    //根据IP定位城市
+    function myFun(result) {
+        var cityName = result.name;
+        map.setCenter(cityName);
+    }
+    var myCity = new BMap.LocalCity();
+    myCity.get(myFun);
+
+	var icon = new BMap.Icon('car.png',new BMap.Size(15,30));
+        icon.imageSize = new BMap.Size(15,30);
+        icon.anchor = new BMap.Size(8,15);
+        
+
+    //showalert(testmsg);
+
+    //对传入的经纬度进行标注：纬度，经度
+   // var Latt = 116.404;
+   // var Lott = 39.915;
+
+   // theLocation(Latt, Lott);
+   // testAlert();
+
+    function SetDrawTrail(x)
+   {
+	
+	if(x == 1)drawtrail = 1;
+	else drawtrail = 0;
+   }    
+
+
+    function clear()
+    {
+	gtracelist = [];
+    }
+
+   
+        // 用经纬度设置地图中心点
+    function objLocation(Longitude,Latitude) {
+
+        var gpsPoint = new BMap.Point(Longitude, Latitude);
+
+	var pointArr = [];
+	pointArr.push(gpsPoint);
+        convertor.translate(pointArr, 1, 5, translateCallbackobj)
+        //gps坐标纠偏
+ //       BMap.Convertor.translate(gpsPoint, 0, translateCallbackobj);     //真实经纬度转成百度坐标
+
+
+    }
+
+    // 用经纬度设置地图中心点
+    function theLocation(Longitude,Latitude,rotation) {
+        
+        grotation = rotation;
+        var gpsPoint = new BMap.Point(Longitude, Latitude);
+
+        //gps坐标纠偏
+
+
+var pointArr = [];
+pointArr.push(gpsPoint);
+        convertor.translate(pointArr, 1, 5, translateCallback)
+//convertor.translate(gpsPoint, 0, translateCallback);     //真实经纬度转成百度坐标
+ //       BMap.Convertor.translate(gpsPoint, 0, translateCallback);     //真实经纬度转成百度坐标
+
+
+            //map.clearOverlays();
+            //var new_point = new BMap.Point(Longitude,Latitude );
+            //var marker = new BMap.Marker(new_point);  // 创建标注
+            //map.addOverlay(marker);              // 将标注添加到地图中
+            //map.panTo(new_point);
+            //marker.setAnimation(BMAP_ANIMATION_BOUNCE); //跳动的动画
+
+    }
+
+
+function settrace(numlist,num2list)                    //仅把qt传来的数组转换成可用的list
+{
+    //alert(numlist);
+    var num_list,num2_list;                         //以下为格式转换，分割成可用的数组
+    num_list = numlist.substring(1,numlist.length-1);
+    num2_list = num2list.substring(1,num2list.length-1);
+    num_list = num_list.split(",");
+    num2_list = num2_list.split(",");
+    //alert("the num_list is: "+num_list[0]+" "+num_list[1]);
+
+    
+    gtraceraw = [];
+    gtracelist = [];
+    for(i=0;i<num_list.length;i++)
+    {
+        point= new BMap.Point(num_list[i],num2_list[i]);
+        gtraceraw.push(point);                                  
+    }
+    tracenum = num_list.length;
+    tracenow = 0;
+
+    setTimeout("converttrace()", 30);// 调用画轨迹的函数
+
+    
+}
+
+function converttrace() 
+{
+    var tracelist = [];     //为轨迹做准备，把所有的点扔里面，但不对每个点标注
+    var index = 0;
+    for(i=tracenow;i<tracenum;i++)
+    {
+	tracelist.push(gtraceraw[i]);
+	index++;
+	if(index>=10)break;
+    }
+    if(index>0)
+    {
+	
+        convertor.translate(tracelist, 1,5,translateCallbacktrace);     //真实经纬度转成百度坐标
+     }
+
+}
+
+
+translateCallbacktrace = function (data){
+      if(data.status == 0) {
+        for (var i = 0; i < data.points.length; i++) {
+	gtracelist.push(data.points[i]);
+        }
+	tracenow = tracenow + data.points.length;
+      }
+
+    map.removeOverlay(polylineTrace);
+    polylineTrace =new BMap.Polyline(gtracelist, {
+    enableEditing: false,//是否启用线编辑，默认为false
+    enableClicking: false,//是否响应点击事件，默认为true
+    strokeWeight:'4',//折线的宽度，以像素为单位
+    strokeOpacity: 0.8,//折线的透明度，取值范围0 - 1
+    strokeColor:"red" //折线颜色
+    });
+    map.addOverlay(polylineTrace);          //增加折线
+      setTimeout("converttrace()", 250);// 调用画轨迹的函数
+	
+}
+
+
+
+
+    // 用经纬度设置地图中心点
+    function testAlert(msg) {
+
+        var str = new String;
+        str =  msg.toString()
+       // str = "test"
+
+        alert(str);
+    }
+
+    function enableZoomDrag()
+    {
+        //添加拖拽和缩放功能
+        map.enableScrollWheelZoom(true);
+        map.enableDragging();
+    }
+
+        //坐标转换完之后的回调函数
+    translateCallbackmap = function (point) {
+
+	gtracelist.push(point);       
+ //       map.setCenter(point);
+      //  marker.setAnimation(BMAP_ANIMATION_BOUNCE); //跳动的动画
+    }
+
+        //坐标转换完之后的回调函数
+    translateCallbacktr = function (point) {
+
+	gtracelist.push(point);       
+ //       map.setCenter(point);
+      //  marker.setAnimation(BMAP_ANIMATION_BOUNCE); //跳动的动画
+    }
+
+    //坐标转换完之后的回调函数
+    translateCallbackobj = function (data) {
+	if(data.status === 0) {
+         var point = data.points[0];
+	pointobj = point;
+	bobjset = 1;
+ //       map.clearOverlays();
+	map.removeOverlay(markerObj);
+        markerObj = new BMap.Marker(point);
+
+        map.addOverlay(markerObj);
+	}
+
+ //       map.setCenter(point);
+      //  marker.setAnimation(BMAP_ANIMATION_BOUNCE); //跳动的动画
+    }
+
+
+    //坐标转换完之后的回调函数
+    translateCallback = function (data) {
+
+if(data.status === 0) {
+         var point = data.points[0]
+        map.removeOverlay(markerCur);
+        markerCur = new BMap.Marker(point);
+        if(havepointlast == 1)
+	{
+		if(drawtrail == 1)
+		{
+		var line = new BMap.Polyline([pointlast, point], {strokeColor: "green", strokeWeight: 1, strokeOpacity: 1});
+		map.addOverlay(line);  
+		line.disableMassClear();
+		}
+	}
+        pointlast = point;
+        havepointlast = 1;
+
+	markerCur.setOffset(-30,-30);
+	markerCur.setIcon(icon); 
+	markerCur.setShadow(icon) 
+	markerCur.setRotation(grotation);
+        map.addOverlay(markerCur);
+        map.setCenter(point);
+      }
+//	return;
+//        map.clearOverlays();
+
+/*
+	if(bobjset == 1)
+	{
+		map.removeOverlay(markerObj);
+        	markerObj = new BMap.Marker(pointobj);
+
+        	map.addOverlay(markerObj);
+	}
+
+*/
+
+      //  marker.setAnimation(BMAP_ANIMATION_BOUNCE); //跳动的动画
+    }
+
+

src/driver/driver_cloud_grpc_civetweb/frontend/dist/index.html

@@ -0,0 +1,57 @@
+<!DOCTYPE html>
+<html>
+<head>
+<script type="text/javascript" src="http://api.map.baidu.com/api?v=2.0&ak=EVi8PXiYpKBGvBr7tgGMzcbdwzWWNf2o"></script>
+<script src="remote.js"></script>
+
+<meta charset="UTF-8">
+<title>ADCIV WEB UI. Designed Base CivetWeb.</title>
+
+</head>
+<body onload="load()">
+<!-- <div id='websock_text_field'>No websocket connection yet</div>-->
+<!--  <iframe id='image' src="" width="800" height="600"></iframe>  -->
+
+<form>
+<br>
+<label for="title" style="width:300px;display:inline-block;">远程控制系统</label>
+<input type="radio" style="width:100px;display:inline-block;height:30px;" name="ctrl" id="ctrlauto" value="auto" checked>
+<label style="width:100px;display:inline-block;">自动</label>
+<input type="radio" style="width:100px;display:inline-block;height:30px;" name="ctrl" id="ctrlman"  value="man">
+<label style="width:100px;display:inline-block;">远程</label>
+<input type="radio" style="width:100px;display:inline-block;height:30px;" name="shift"  id="shiftdrive" value="D" checked>
+<label style="width:100px;display:inline-block;">前进</label>
+<input type="radio" style="width:100px;display:inline-block;height:30px;" name="shift" id="shiftrear" value="R">
+<label style="width:100px;display:inline-block;">后退</label>
+<br>
+<br>
+<br>
+<label style="width:150px;display:inline-block;" >车速</label>
+<input type="range" name="slidervel" id="sildervel" value="0.0" max="10" min="0" step="0.1" style="width:900px"/>
+<label style="width:50px;display:inline-block;" > </label>
+<button type="button" onclick="onClickStop()" style="width:100px;"  >停车</button>
+<br>
+<br>
+<label style="width:150px;display:inline-block;" >转向</label>
+<input type="range" name="sliderwheel" id="silderwheel" value="0.5" max="1" min="0" step="0.01" style="width:900px;"/>
+<label style="width:50px;display:inline-block;" > </label>
+<button type="button" onclick="onClickCenter()" style="width:100px;"  >居中</button>
+</form> 
+<p>  </p>
+<canvas id="myCanvas2" width="1920" height="1080" style="border:0px solid #c3c3c3;">  
+Your browser does not support the canvas element.  
+</canvas>
+
+<p id="demo">This is a paragraph.</p> 
+
+ <div style="width:1000px;height:1000px;border:#ccc solid 1px;" id="allmap"></div>
+
+</body>
+
+</html>
+
+<script src="baidumap.js"></script>
+
+
+
+

src/driver/driver_cloud_grpc_civetweb/frontend/index.html → src/driver/driver_cloud_grpc_civetweb/frontend/dist/remote.js

@@ -1,9 +1,6 @@
-<!DOCTYPE html>
-<html>
-<head>
-<meta charset="UTF-8">
-<title>ADCIV WEB UI. Designed Base CivetWeb.</title>
-<script>
+
+
+
 
 //var i_width=window.document.body.clientWidth//网页可用宽度  
 //var i_height=window.document.body.clientHeight;
@@ -25,7 +22,10 @@ var oldwheel = 0.5;
 var nchange = 15;
 var conupserv = 0;
 
-	var n = 0;
+	var lat = 39.0;
+	var lon = 117.0;
+
+	var n = 323;
 	var ic = 0;
 	var irear = 0;
 	var ileft = 0;
@@ -49,22 +49,22 @@ var conupserv = 0;
 			valuex = valuex + 0.1;
 			document.getElementById("sildervel").value = valuex;  
                         console.log("w键按下了");
-			document.getElementById("demo").innerHTML=valuex;
+//			document.getElementById("demo").innerHTML=valuex;
                     }
                     if(event.keyCode==83){
 			document.getElementById("sildervel").value = parseFloat(document.getElementById("sildervel").value) - 0.1;  
                         console.log("s键按下了");
-			document.getElementById("demo").innerHTML=Date();
+//			document.getElementById("demo").innerHTML=Date();
                     }
                     if(event.keyCode==65){
 			document.getElementById("silderwheel").value = parseFloat(document.getElementById("silderwheel").value) - 0.01;
                         console.log("a键按下了");
-			document.getElementById("demo").innerHTML=Date();
+//			document.getElementById("demo").innerHTML=Date();
                     }
                     if(event.keyCode==68){
 			document.getElementById("silderwheel").value = parseFloat(document.getElementById("silderwheel").value) + 0.01;
                         console.log("d键按下了");
-			document.getElementById("demo").innerHTML=Date();
+//			document.getElementById("demo").innerHTML=Date();
                     }
                     if(event.keyCode==80){
 			document.getElementById("sildervel").value = 0;
@@ -102,6 +102,9 @@ function onClickCenter(){
 
 function updatectrl(){ 
 
+	lat = lat + 0.0001;
+	
+	
 	var vel = parseFloat(document.getElementById("sildervel").value);
 	var wheel = parseFloat(document.getElementById("silderwheel").value);
 	if((vel == oldvel)&&(wheel == oldwheel))
@@ -123,7 +126,7 @@ function updatectrl(){
 		oldvel = vel;
 		oldwheel = wheel;
 	}
-	document.getElementById("demo").innerHTML=nchange;
+//	document.getElementById("demo").innerHTML=nchange;
 }
 	function changeFrontImage()
 	{ 
@@ -409,6 +412,8 @@ if(conupserv == 1){
     connection.send(stritem);}
 }
 function load() {
+
+
 //	changeImage();
 
   var wsproto = (location.protocol === 'https:') ? 'wss:' : 'ws:';
@@ -416,48 +421,33 @@ function load() {
 //  websock_text_field = document.getElementById('websock_text_field');
   connection.onmessage = function (e) {
 	conupserv = 1;
+var text = e.data;
+//document.getElementById("demo").innerHTML=text;
+try {
+    var obj = JSON.parse(text);
+     if (obj.hasOwnProperty("Lon")) {
+ 	var Lon = obj.Lon;
+        var Lat = obj.Lat;
+	var Heading = obj.Heading;
+	theLocation(Lon,Lat,Heading);
+//  	document.getElementById("demo").innerHTML=Heading;
+     }
+     else
+     {
+//	document.getElementById("demo").innerHTML="no lon";
+     }	
+	
+
+} 
+catch (error) {
+ //   alert(error.message);
+
+}
    }
 }
 
 
 
-</script>
-</head>
-<body onload="load()">
-<!-- <div id='websock_text_field'>No websocket connection yet</div>-->
-<!--  <iframe id='image' src="" width="800" height="600"></iframe>  -->
-
-<form>
-<br>
-<label for="title" style="width:300px;display:inline-block;">远程控制系统</label>
-<input type="radio" style="width:100px;display:inline-block;height:30px;" name="ctrl" id="ctrlauto" value="auto" checked>
-<label style="width:100px;display:inline-block;">自动</label>
-<input type="radio" style="width:100px;display:inline-block;height:30px;" name="ctrl" id="ctrlman"  value="man">
-<label style="width:100px;display:inline-block;">远程</label>
-<input type="radio" style="width:100px;display:inline-block;height:30px;" name="shift"  id="shiftdrive" value="D" checked>
-<label style="width:100px;display:inline-block;">前进</label>
-<input type="radio" style="width:100px;display:inline-block;height:30px;" name="shift" id="shiftrear" value="R">
-<label style="width:100px;display:inline-block;">后退</label>
-<br>
-<br>
-<br>
-<label style="width:150px;display:inline-block;" >车速</label>
-<input type="range" name="slidervel" id="sildervel" value="0.0" max="10" min="0" step="0.1" style="width:900px"/>
-<label style="width:50px;display:inline-block;" > </label>
-<button type="button" onclick="onClickStop()" style="width:100px;"  >停车</button>
-<br>
-<br>
-<label style="width:150px;display:inline-block;" >转向</label>
-<input type="range" name="sliderwheel" id="silderwheel" value="0.5" max="1" min="0" step="0.01" style="width:900px;"/>
-<label style="width:50px;display:inline-block;" > </label>
-<button type="button" onclick="onClickCenter()" style="width:100px;"  >居中</button>
-</form> 
-<p>  </p>
-<canvas id="myCanvas2" width="1920" height="1080" style="border:0px solid #c3c3c3;">  
-Your browser does not support the canvas element.  
-</canvas>
-
-<p id="demo">This is a paragraph.</p> 
-
-</body>
-</html>
+
+
+

src/driver/driver_cloud_grpc_civetweb/grpccivet.cpp

@@ -1,7 +1,36 @@
 #include "grpccivet.h"
 
+#include <QFile>
+
 grpccivet::grpccivet()
 {
 //    mpgrpcpc = new grpcpc;
 //    mpgrpcpc->start();
+    mgrpc = new grpcpc("test");
+    mgrpc->start();
+
+    QFile xFile;
+    xFile.setFileName("./frontend/tvsnow.jpg");
+    if(xFile.open(QIODevice::ReadOnly))
+    {
+        mbasnow = xFile.readAll();
+
+    }
+    xFile.close();
+}
+
+grpccivet & grpccivet::GetInst()
+{
+    static grpccivet xgrpccivet;
+    return xgrpccivet;
+}
+
+grpcpc * grpccivet::Getgrpcpc()
+{
+    return mgrpc;
+}
+
+QByteArray grpccivet::GetSNOW()
+{
+    return mbasnow;
 }

src/driver/driver_cloud_grpc_civetweb/grpccivet.h

@@ -10,7 +10,20 @@ public:
     grpccivet();
 
 private:
-    grpcpc * mpgrpcpc;
+//    grpcpc * mpgrpcpc;
+    grpcpc * mgrpc;
+
+
+    QByteArray mbasnow;
+
+public:
+    static grpccivet & GetInst();
+
+public:
+    grpcpc * Getgrpcpc();
+    QByteArray GetSNOW();
 };
 
+#define ServiceGRPCCIVET grpccivet::GetInst()
+
 #endif // GRPCCIVET_H

src/driver/driver_cloud_grpc_civetweb/main.cpp

@@ -1,445 +1,20 @@
 #include <QCoreApplication>
 
+#include "starthandler.h"
+#include "pichandler.h"
+#include "remotehandler.h"
 
-#include <QMutex>
-#include <iostream>
-#include <QFile>
-#include <QJsonObject>
-#include <QJsonDocument>
-#include <QJsonArray>
 
 #include "xmlparam.h"
 
 #include "rawpic.pb.h"
 
-#include "CivetServer.h"
-#include <cstring>
-
-#ifdef _WIN32
-#include <windows.h>
-#else
-#include <unistd.h>
-#endif
 
 #include "grpccivet.h"
 
 #define DOCUMENT_ROOT "./frontend/dist"
 
-QByteArray gbasnow;
-
-
-grpcpc * ggrpc;
-
-class WsStartHandler : public CivetHandler
-{
-  public:
-    bool
-    handleGet(CivetServer *server, struct mg_connection *conn)
-    {
-
-    mg_printf(conn,
-              "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\nConnection: "
-              "close\r\n\r\n");
-
-    mg_printf(conn, "<!DOCTYPE html>\n");
-    mg_printf(conn, "<html>\n<head>\n");
-    mg_printf(conn, "<meta charset=\"UTF-8\">\n");
-    mg_printf(conn, "<title>ADC IV RemoteCtrl UI</title>\n");
-
-    QFile xFile;
-    xFile.setFileName("./frontend/index.html");
-    if(xFile.open(QIODevice::ReadOnly))
-    {
-        QByteArray ba = xFile.readAll();
-        mg_printf(conn,ba.data());
-
-    }
-
-    return 1;
-
-
-
-    }
-};
-
-class PicFrontHandler : public CivetHandler
-{
-  public:
-    bool
-    handleGet(CivetServer *server, struct mg_connection *conn)
-    {
-
-        static int ncount;
-
-        mg_printf(conn,
-                  "HTTP/1.1 200 OK\r\n"
-                  "Connection: close\r\n"
-                  "Max-Age: 0\r\n"
-                  "Expires: 0\r\n"
-                  "Cache-Control: no-cache, no-store, must-revalidate, private\r\n"
-                  "Pragma: no-cache\r\n"
-                  "Content-Type: multipart/x-mixed-replace; "
-                  "boundary=--BoundaryString\r\n"
-                  "\r\n");
-
-        mg_printf(conn,"<meta http-equiv=\"refresh\" content=\"1\">");
-        mg_printf(conn,
-        "<script type=\"text/javascript\">\r\n"
-            "function myrefresh() {\r\n"
-                "window.location.reload();\r\n"
-            "}\r\n"
-            "setTimeout('myrefresh()', 1000);\r\n"
-        "</script>\r\n");
-
-        QByteArray ba;
-
-
-        iv::vision::rawpic xrawpic;
-        if(ggrpc->GetRawPic(0,xrawpic) == 1)
-        {
-            ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-        }
-        else
-        {
-            ba = gbasnow;
-        }
-//        if(gLastUpdate > 0)
-//        {
-//        iv::vision::rawpic xrawpic;
-//        gMutex.lock();
-//        xrawpic.CopyFrom(mRawPic);
-//        gMutex.unlock();
-//        ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-
-//        }
-        mg_printf(conn,
-                  "--BoundaryString\r\n"
-                  "Content-type: image/jpeg\r\n"
-                  "Content-Length: %zu\r\n"
-                  "\r\n",
-                  ba.size());
-
-
-        mg_write(conn, ba.data(), ba.size());
-
-
-        mg_printf(conn, "\r\n\r\n");
-
-
-        ncount++;
-        printf("send pic. %d\n",ncount);
-
-        return true;
-    }
-};
-
-
-class PicRearHandler : public CivetHandler
-{
-  public:
-    bool
-    handleGet(CivetServer *server, struct mg_connection *conn)
-    {
-
-        static int ncount;
-
-        mg_printf(conn,
-                  "HTTP/1.1 200 OK\r\n"
-                  "Connection: close\r\n"
-                  "Max-Age: 0\r\n"
-                  "Expires: 0\r\n"
-                  "Cache-Control: no-cache, no-store, must-revalidate, private\r\n"
-                  "Pragma: no-cache\r\n"
-                  "Content-Type: multipart/x-mixed-replace; "
-                  "boundary=--BoundaryString\r\n"
-                  "\r\n");
-
-        mg_printf(conn,"<meta http-equiv=\"refresh\" content=\"1\">");
-        mg_printf(conn,
-        "<script type=\"text/javascript\">\r\n"
-            "function myrefresh() {\r\n"
-                "window.location.reload();\r\n"
-            "}\r\n"
-            "setTimeout('myrefresh()', 1000);\r\n"
-        "</script>\r\n");
-
-        QByteArray ba;
-
-
-        iv::vision::rawpic xrawpic;
-        if(ggrpc->GetRawPic(1,xrawpic) == 1)
-        {
-            ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-        }
-        else
-        {
-            ba = gbasnow;
-        }
-//        if(gLastUpdate > 0)
-//        {
-//        iv::vision::rawpic xrawpic;
-//        gMutex.lock();
-//        xrawpic.CopyFrom(mRawPic);
-//        gMutex.unlock();
-//        ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-
-//        }
-        mg_printf(conn,
-                  "--BoundaryString\r\n"
-                  "Content-type: image/jpeg\r\n"
-                  "Content-Length: %zu\r\n"
-                  "\r\n",
-                  ba.size());
-
-
-        mg_write(conn, ba.data(), ba.size());
-
-
-        mg_printf(conn, "\r\n\r\n");
-
-
-        ncount++;
-        printf("send pic. %d\n",ncount);
-
-        return true;
-    }
-};
-
-
-
-class PicLeftHandler : public CivetHandler
-{
-  public:
-    bool
-    handleGet(CivetServer *server, struct mg_connection *conn)
-    {
-
-        static int ncount;
-
-        mg_printf(conn,
-                  "HTTP/1.1 200 OK\r\n"
-                  "Connection: close\r\n"
-                  "Max-Age: 0\r\n"
-                  "Expires: 0\r\n"
-                  "Cache-Control: no-cache, no-store, must-revalidate, private\r\n"
-                  "Pragma: no-cache\r\n"
-                  "Content-Type: multipart/x-mixed-replace; "
-                  "boundary=--BoundaryString\r\n"
-                  "\r\n");
-
-        mg_printf(conn,"<meta http-equiv=\"refresh\" content=\"1\">");
-        mg_printf(conn,
-        "<script type=\"text/javascript\">\r\n"
-            "function myrefresh() {\r\n"
-                "window.location.reload();\r\n"
-            "}\r\n"
-            "setTimeout('myrefresh()', 1000);\r\n"
-        "</script>\r\n");
-
-        QByteArray ba;
-
 
-        iv::vision::rawpic xrawpic;
-        if(ggrpc->GetRawPic(2,xrawpic) == 1)
-        {
-            ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-        }
-        else
-        {
-            ba = gbasnow;
-        }
-//        if(gLastUpdate > 0)
-//        {
-//        iv::vision::rawpic xrawpic;
-//        gMutex.lock();
-//        xrawpic.CopyFrom(mRawPic);
-//        gMutex.unlock();
-//        ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-
-//        }
-        mg_printf(conn,
-                  "--BoundaryString\r\n"
-                  "Content-type: image/jpeg\r\n"
-                  "Content-Length: %zu\r\n"
-                  "\r\n",
-                  ba.size());
-
-
-        mg_write(conn, ba.data(), ba.size());
-
-
-        mg_printf(conn, "\r\n\r\n");
-
-
-        ncount++;
-        printf("send pic. %d\n",ncount);
-
-        return true;
-    }
-};
-
-
-
-class PicRightHandler : public CivetHandler
-{
-  public:
-    bool
-    handleGet(CivetServer *server, struct mg_connection *conn)
-    {
-
-        static int ncount;
-
-        mg_printf(conn,
-                  "HTTP/1.1 200 OK\r\n"
-                  "Connection: close\r\n"
-                  "Max-Age: 0\r\n"
-                  "Expires: 0\r\n"
-                  "Cache-Control: no-cache, no-store, must-revalidate, private\r\n"
-                  "Pragma: no-cache\r\n"
-                  "Content-Type: multipart/x-mixed-replace; "
-                  "boundary=--BoundaryString\r\n"
-                  "\r\n");
-
-        mg_printf(conn,"<meta http-equiv=\"refresh\" content=\"1\">");
-        mg_printf(conn,
-        "<script type=\"text/javascript\">\r\n"
-            "function myrefresh() {\r\n"
-                "window.location.reload();\r\n"
-            "}\r\n"
-            "setTimeout('myrefresh()', 1000);\r\n"
-        "</script>\r\n");
-
-        QByteArray ba;
-
-
-        iv::vision::rawpic xrawpic;
-        if(ggrpc->GetRawPic(3,xrawpic) == 1)
-        {
-            ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-        }
-        else
-        {
-            ba = gbasnow;
-        }
-//        if(gLastUpdate > 0)
-//        {
-//        iv::vision::rawpic xrawpic;
-//        gMutex.lock();
-//        xrawpic.CopyFrom(mRawPic);
-//        gMutex.unlock();
-//        ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
-
-//        }
-        mg_printf(conn,
-                  "--BoundaryString\r\n"
-                  "Content-type: image/jpeg\r\n"
-                  "Content-Length: %zu\r\n"
-                  "\r\n",
-                  ba.size());
-
-
-        mg_write(conn, ba.data(), ba.size());
-
-
-        mg_printf(conn, "\r\n\r\n");
-
-
-        ncount++;
-        printf("send pic. %d\n",ncount);
-
-        return true;
-    }
-};
-
-
-#ifdef USE_WEBSOCKET
-class WebSocketHandler : public CivetWebSocketHandler {
-
-    int a = 1;
-    virtual bool handleConnection(CivetServer *server,
-                                  const struct mg_connection *conn) {
-        printf("WS connected\n");
-        return true;
-    }
-
-    virtual void handleReadyState(CivetServer *server,
-                                  struct mg_connection *conn) {
-        printf("WS ready\n");
-
-        const char *text = "Hello from the websocket ready handler";
-        mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT, text, strlen(text));
-    }
-
-    virtual bool handleData(CivetServer *server,
-                            struct mg_connection *conn,
-                            int bits,
-                            char *data,
-                            size_t data_len) {
-        printf("WS got %lu bytes: ", (long unsigned)data_len);
-        QJsonDocument xup;
-        QByteArray bajson;
-        bajson.push_back(data);
-        xup = QJsonDocument::fromJson(bajson);
-
-        QJsonObject xupo = xup.object();
-        QString strctrl = "AA";
-        if(xupo.contains("Ctrl"))
-        {
-            strctrl = xupo.value("Ctrl").toString();
-        }
-        QString strshift = "D";
-        if(xupo.contains("Shift"))
-        {
-            strshift = xupo.value("Shift").toString();
-        }
-        QString strvel = "0.13";
-        if(xupo.contains("Vel"))
-        {
-            strvel = xupo.value("Vel").toString();
-        }
-        QString strwheel = "0.03";
-        if(xupo.contains("Wheel"))
-        {
-            strwheel = xupo.value("Wheel").toString();
-        }
-        fwrite(data, 1, data_len, stdout);
-//        printf("\n");
-
-
-        QJsonObject obj1;
-
-     //   obj1.insert("Class", "sindata");
-
-        obj1.insert("test","test");
-
-        QString strsend = QString(QJsonDocument(obj1).toJson(QJsonDocument::Compact));
-
-
-
-                mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT,strsend.toLatin1().data(),strsend.length());
-                return (data_len<5);
-
-        char strout[100];
-  //      snprintf(strout,100,"hello %s <img src =\"/Pic?%d\"> ",data,a);a++;
-
-        snprintf(strout,100,"<p><h3>hello %s </h3> acc:%f <br> brake:%f <br> wheel:%f</p> ",data,1.0,0.0,300.0);a++;
-
-
-
-
- //       mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT, data, data_len);
-        mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT,strout,strnlen(strout,255));
-  //      mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT,"hello",5);
-        return true;
-//        return (data_len<5);
-    }
-
-    virtual void handleClose(CivetServer *server,
-                             const struct mg_connection *conn) {
-        printf("WS closed\n");
-    }
-};
-#endif
 
 
 int main(int argc, char *argv[])
@@ -451,14 +26,6 @@ int main(int argc, char *argv[])
     std::string strport = xp.GetParam("Port","6101");
 //    gpa = iv::modulecomm::RegisterRecv(strmsgname.data(),Listenpic);
 
-    QFile xFile;
-    xFile.setFileName("./frontend/tvsnow.jpg");
-    if(xFile.open(QIODevice::ReadOnly))
-    {
-        gbasnow = xFile.readAll();
-
-    }
-    xFile.close();
 
     mg_init_library(0);
 
@@ -474,27 +41,22 @@ int main(int argc, char *argv[])
     // CivetServer server(options); // <-- C style start
     CivetServer server(cpp_options); // <-- C++ style start
 
-    WsStartHandler h_ws;
-    server.addHandler("/", h_ws);
+    starthandler h_ws;
+//    server.addHandler("/",h_ws);
 
-    PicFrontHandler h_picfront;
-    server.addHandler("/picfront", h_picfront);
+    PicHandler h_pic;
 
-    PicRearHandler h_picrear;
-    server.addHandler("/picrear", h_picrear);
-    PicLeftHandler h_picleft;
-    server.addHandler("/picleft", h_picleft);
-    PicRightHandler h_picright;
-    server.addHandler("/picright", h_picright);
+    server.addHandler("/picfront", h_pic);
+    server.addHandler("/picrear", h_pic);
+    server.addHandler("/picleft", h_pic);
+    server.addHandler("/picright", h_pic);
 
 #ifdef USE_WEBSOCKET
-    WebSocketHandler h_websocket;
-    server.addWebSocketHandler("/remoteservice", h_websocket);
+    RemoteHandler h_remotesocket;
+    server.addWebSocketHandler("/remoteservice", h_remotesocket);
 //    printf("Run websocket example at http://localhost:%s/\n", PORT);
 #endif
 
-    ggrpc = new grpcpc("test");
-    ggrpc->start();
-
     return a.exec();
+
 }

src/driver/driver_cloud_grpc_civetweb/pichandler.cpp

@@ -0,0 +1,105 @@
+#include "pichandler.h"
+
+#include "grpccivet.h"
+
+#include "rawpic.pb.h"
+
+PicHandler::PicHandler()
+{
+
+}
+
+bool PicHandler::handleGet(CivetServer *server, mg_connection *conn)
+{
+    static int ncount;
+
+    mg_printf(conn,
+              "HTTP/1.1 200 OK\r\n"
+              "Connection: close\r\n"
+              "Max-Age: 0\r\n"
+              "Expires: 0\r\n"
+              "Cache-Control: no-cache, no-store, must-revalidate, private\r\n"
+              "Pragma: no-cache\r\n"
+              "Content-Type: multipart/x-mixed-replace; "
+              "boundary=--BoundaryString\r\n"
+              "\r\n");
+
+    mg_printf(conn,"<meta http-equiv=\"refresh\" content=\"1\">");
+    mg_printf(conn,
+    "<script type=\"text/javascript\">\r\n"
+        "function myrefresh() {\r\n"
+            "window.location.reload();\r\n"
+        "}\r\n"
+        "setTimeout('myrefresh()', 1000);\r\n"
+    "</script>\r\n");
+
+
+
+
+    grpcpc * pgrpc = ServiceGRPCCIVET.Getgrpcpc();
+    if(pgrpc == NULL)return false;
+
+    unsigned int ncam = 10000;
+    const struct mg_request_info * preq = mg_get_request_info(conn);
+    if(preq->local_uri != NULL)
+    {
+        if(strncmp(preq->local_uri,"/picfront",256) == 0)
+        {
+            ncam = 0;
+        }
+        if(strncmp(preq->local_uri,"/picrear",256) == 0)
+        {
+            ncam = 1;
+        }
+        if(strncmp(preq->local_uri,"/picleft",256) == 0)
+        {
+            ncam = 2;
+        }
+        if(strncmp(preq->local_uri,"/picright",256) == 0)
+        {
+            ncam = 3;
+        }
+    }
+
+    QByteArray ba;
+
+
+    iv::vision::rawpic xrawpic;
+
+    if(pgrpc->GetRawPic((unsigned int)ncam,xrawpic) == 1)
+    {
+        ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
+    }
+    else
+    {
+        ba = ServiceGRPCCIVET.GetSNOW();
+    }
+
+//        if(gLastUpdate > 0)
+//        {
+//        iv::vision::rawpic xrawpic;
+//        gMutex.lock();
+//        xrawpic.CopyFrom(mRawPic);
+//        gMutex.unlock();
+//        ba.append(xrawpic.picdata().data(),xrawpic.picdata().size());
+
+//        }
+    mg_printf(conn,
+              "--BoundaryString\r\n"
+              "Content-type: image/jpeg\r\n"
+              "Content-Length: %zu\r\n"
+              "\r\n",
+              ba.size());
+
+
+    mg_write(conn, ba.data(), ba.size());
+
+
+    mg_printf(conn, "\r\n\r\n");
+
+
+    ncount++;
+    printf("send pic. %d\n",ncount);
+
+    return true;
+}

src/driver/driver_cloud_grpc_civetweb/pichandler.h

@@ -0,0 +1,22 @@
+#ifndef PICHANDLER_H
+#define PICHANDLER_H
+
+#include "grpcpc.h"
+
+#include "CivetServer.h"
+#include <cstring>
+
+#ifdef _WIN32
+#include <windows.h>
+#else
+#include <unistd.h>
+#endif
+
+class PicHandler : public CivetHandler
+{
+public:
+    PicHandler();
+    bool handleGet(CivetServer *server, struct mg_connection *conn);
+};
+
+#endif // PICHANDLER_H

src/driver/driver_cloud_grpc_civetweb/remotehandler.cpp

@@ -0,0 +1,223 @@
+#include "remotehandler.h"
+
+#include <QMutex>
+#include <iostream>
+#include <QFile>
+#include <QJsonObject>
+#include <QJsonDocument>
+#include <QJsonArray>
+
+#include "grpccivet.h"
+
+#include "remotectrl.pb.h"
+
+#ifdef USE_WEBSOCKET
+
+RemoteHandler::RemoteHandler()
+{
+
+}
+
+bool RemoteHandler::handleConnection(CivetServer *server, const mg_connection *conn)
+{
+    printf("WS connected\n");
+    return true;
+}
+
+void RemoteHandler::handleReadyState(CivetServer *server, mg_connection *conn)
+{
+    printf("WS ready\n");
+
+    const char *text = "Hello from the websocket ready handler";
+    mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT, text, strlen(text));
+}
+
+bool RemoteHandler::handleData(CivetServer *server, mg_connection *conn, int bits, char *data, size_t data_len)
+{
+    printf("WS got %lu bytes: ", (long unsigned)data_len);
+    QJsonDocument xup;
+    QByteArray bajson;
+    bajson.push_back(data);
+    xup = QJsonDocument::fromJson(bajson);
+
+    QJsonObject xupo = xup.object();
+    QString strctrl = "AA";
+    if(xupo.contains("Ctrl"))
+    {
+        strctrl = xupo.value("Ctrl").toString();
+    }
+    QString strshift = "D";
+    if(xupo.contains("Shift"))
+    {
+        strshift = xupo.value("Shift").toString();
+    }
+    QString strvel = "0.00";
+    if(xupo.contains("Vel"))
+    {
+        strvel = xupo.value("Vel").toString();
+    }
+    QString strwheel = "0.50";
+    if(xupo.contains("Wheel"))
+    {
+        strwheel = xupo.value("Wheel").toString();
+    }
+    fwrite(data, 1, data_len, stdout);
+//        printf("\n");
+
+    iv::cloud::cloudmsg xmsg;
+    iv::gps::gpsimu xgpsimu;
+    bool bHaveGPS = false;
+    CalcRemote(xmsg,strctrl,strshift,strvel,strwheel,xgpsimu,bHaveGPS);
+    ServiceGRPCCIVET.Getgrpcpc()->SetSendMsg(xmsg);
+
+    QJsonObject obj1;
+
+ //   obj1.insert("Class", "sindata");
+
+    obj1.insert("test","test");
+
+    if(bHaveGPS)
+    {
+        obj1.insert("Lat",QString::number(xgpsimu.lat(),'f',7));
+        obj1.insert("Lon",QString::number(xgpsimu.lon(),'f',7));
+        obj1.insert("Heading",QString::number(xgpsimu.heading()));
+    }
+
+    QString strsend = QString(QJsonDocument(obj1).toJson(QJsonDocument::Compact));
+
+
+
+    mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT,strsend.toLatin1().data(),strsend.length());
+    return true;
+    return (data_len<5);
+
+    char strout[100];
+//      snprintf(strout,100,"hello %s <img src =\"/Pic?%d\"> ",data,a);a++;
+
+    snprintf(strout,100,"<p><h3>hello %s </h3> acc:%f <br> brake:%f <br> wheel:%f</p> ",data,1.0,0.0,300.0);
+
+
+
+
+//       mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT, data, data_len);
+    mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT,strout,strnlen(strout,255));
+//      mg_websocket_write(conn, MG_WEBSOCKET_OPCODE_TEXT,"hello",5);
+    return true;
+//        return (data_len<5);
+}
+
+int RemoteHandler::CalcRemote(iv::cloud::cloudmsg &xmsg,QString strctrl,QString strshift,QString strvel,QString strwheel,iv::gps::gpsimu & nowgpsimu,bool & bhavegps)
+{
+    grpcpc * pgrpc = ServiceGRPCCIVET.Getgrpcpc();
+    iv::cloud::cloudmsg xrecv;
+    int nrecv = pgrpc->GetRecvMsg(xrecv);
+    iv::gps::gpsimu xgpsimu;
+    bhavegps = false;
+    if(nrecv == 1)
+    {
+        int nsize = xrecv.xclouddata_size();
+        int i;
+        for(i=0;i<nsize;i++)
+        {
+            if(xrecv.xclouddata(i).msgname() == "hcp2_gpsimu")
+            {
+                bhavegps = xgpsimu.ParseFromArray(xrecv.xclouddata(i).data().data(),xrecv.xclouddata(i).data().size());
+                if(bhavegps)
+                {
+                    nowgpsimu.CopyFrom(xgpsimu);
+                }
+                break;
+            }
+        }
+    }
+    iv::remotectrl xremote;
+    if(bhavegps == false)
+    {
+
+        xremote.set_ntype(iv::remotectrl_CtrlType_AUTO);
+
+    }
+    else
+    {
+        if(strctrl == "Auto")
+        {
+            xremote.set_ntype(iv::remotectrl_CtrlType_AUTO);
+        }
+        else
+        {
+            xremote.set_ntype(iv::remotectrl_CtrlType_REMOTE);
+            if(strshift == "Drive")
+            {
+                xremote.set_shift(1);
+            }
+            else
+            {
+                if(strshift == "Rear")
+                {
+                    xremote.set_shift(-1);
+                }
+                else
+                {
+                    xremote.set_shift(0);
+                }
+            }
+            double fwheel = strwheel.toDouble();
+            fwheel = (0.5 - fwheel)*200.0;
+            xremote.set_wheel(fwheel);
+            double fVel = strvel.toDouble();
+            double fAcc,fBrake;
+            CalcMidCarWheelBrake(xgpsimu,fVel,fAcc,fBrake);
+            xremote.set_acc(fAcc);
+            xremote.set_brake(fBrake);
+
+        }
+    }
+
+    iv::cloud::cloudunit xunit;
+    int nunitsize = xremote.ByteSize();
+    std::shared_ptr<char> pstr_ptr = std::shared_ptr<char>(new char[nunitsize]);
+    if(xremote.SerializeToArray(pstr_ptr.get(),nunitsize))
+    {
+        xunit.set_msgname("remotectrl");
+        xunit.set_data(pstr_ptr.get(),nunitsize);
+        iv::cloud::cloudunit * punit = xmsg.add_xclouddata();
+        punit->CopyFrom(xunit);
+    }
+
+    return 0;
+}
+
+void  RemoteHandler::CalcMidCarWheelBrake(iv::gps::gpsimu &xgpsimu, double fVel, double &fAcc, double &fBrake)
+{
+    const double fMaxVel = 5.0;
+    double fV = sqrt(pow(xgpsimu.ve(),2)+pow(xgpsimu.vn(),2))*3.6;
+    double fNeedV = fVel * fMaxVel/10.0;
+    if(fNeedV > fV)
+    {
+        fBrake = 0.0;
+        fAcc = 50.0;
+    }
+    else
+    {
+        if(fNeedV>0.5)
+        {
+            if(fNeedV>(0.9*fV))
+            {
+                fAcc = 0.0;
+                fBrake = 0.0;
+            }
+            else
+            {
+                fAcc =0.0;
+                fBrake = 30.0;
+            }
+        }
+        else
+        {
+            fAcc = 0.0;
+            fBrake = 0.0;
+        }
+    }
+}
+
+#endif

src/driver/driver_cloud_grpc_civetweb/remotehandler.h

@@ -0,0 +1,44 @@
+#ifndef REMOTEHANDLER_H
+#define REMOTEHANDLER_H
+
+#include "CivetServer.h"
+#include <cstring>
+
+#ifdef _WIN32
+#include <windows.h>
+#else
+#include <unistd.h>
+#endif
+
+#include "gpsimu.pb.h"
+#include "cloud.pb.h"
+
+#ifdef USE_WEBSOCKET
+
+#include <QString>
+
+class RemoteHandler : public CivetWebSocketHandler
+{
+public:
+    RemoteHandler();
+
+public:
+    virtual bool handleConnection(CivetServer *server,
+                                  const struct mg_connection *conn);
+    virtual void handleReadyState(CivetServer *server,
+                                  struct mg_connection *conn);
+    virtual bool handleData(CivetServer *server,
+                            struct mg_connection *conn,
+                            int bits,
+                            char *data,
+                            size_t data_len);
+
+
+private:
+    int CalcRemote(iv::cloud::cloudmsg & xmsg,QString strctrl,QString strshift,QString strvel,QString strwheel,iv::gps::gpsimu & nowgpsimu,bool & bhavegps);
+    void  CalcMidCarWheelBrake(iv::gps::gpsimu & xgpsimu,double fVel,double & fAcc,double & fBrake);
+};
+
+#endif
+
+#endif // REMOTEHANDLER_H

src/driver/driver_cloud_grpc_civetweb/starthandler.cpp

@@ -0,0 +1,32 @@
+#include "starthandler.h"
+
+#include <QFile>
+
+starthandler::starthandler()
+{
+
+}
+
+bool starthandler::handleGet(CivetServer *server, mg_connection *conn)
+{
+    (void )server;
+    mg_printf(conn,
+              "HTTP/1.1 200 OK\r\nContent-Type: text/html\r\nConnection: "
+              "close\r\n\r\n");
+
+    mg_printf(conn, "<!DOCTYPE html>\n");
+    mg_printf(conn, "<html>\n<head>\n");
+    mg_printf(conn, "<meta charset=\"UTF-8\">\n");
+    mg_printf(conn, "<title>ADC IV RemoteCtrl UI</title>\n");
+
+    QFile xFile;
+    xFile.setFileName("./frontend/dist/index.html");
+    if(xFile.open(QIODevice::ReadOnly))
+    {
+        QByteArray ba = xFile.readAll();
+        mg_printf(conn,"%s",ba.data());
+
+    }
+
+    return true;
+}

src/driver/driver_cloud_grpc_civetweb/starthandler.h

@@ -0,0 +1,23 @@
+#ifndef STARTHANDLER_H
+#define STARTHANDLER_H
+
+#include "CivetServer.h"
+#include <cstring>
+
+#ifdef _WIN32
+#include <windows.h>
+#else
+#include <unistd.h>
+#endif
+
+
+class starthandler : public CivetHandler
+{
+public:
+    starthandler();
+
+public:
+  bool handleGet(CivetServer *server, struct mg_connection *conn);
+};
+
+#endif // STARTHANDLER_H

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.pro

@@ -39,7 +39,9 @@ SOURCES += \
         ../../include/msgtype/chassis.pb.cc \
         ../../include/msgtype/gpsimu.pb.cc \
         ../../include/msgtype/platform_feedback.pb.cc \
-        ../../include/msgtype/rawpic.pb.cc
+        ../../include/msgtype/rawpic.pb.cc \
+        ../../include/msgtype/startturnstile.pb.cc \
+        ../../include/msgtype/turnstile.pb.cc
 
 
 # Default rules for deployment.
@@ -88,6 +90,8 @@ HEADERS += \
         ../../include/msgtype/chassis.pb.h \
         ../../include/msgtype/gpsimu.pb.h \
         ../../include/msgtype/platform_feedback.pb.h \
-        ../../include/msgtype/rawpic.pb.h
+        ../../include/msgtype/rawpic.pb.h \
+        ../../include/msgtype/startturnstile.pb.h \
+        ../../include/msgtype/turnstile.pb.h
 
 #before compile in ubuntu20.04 : sudo apt install libyaml-cpp-dev libgrpc++-dev

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.yaml

@@ -43,3 +43,12 @@ GPS_IMU:
   msgname: hcp2_gpsimu
   buffersize: 10000
   buffercount: 1
+start_turnstile:
+  msgname: startturnstile
+  buffersize: 10000
+  buffercount: 1
+turnstile:
+  msgname: turnstile
+  buffersize: 10000000
+  buffercount: 1
+

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.cpp

@@ -5,6 +5,9 @@
 
 #include "modulecomm.h"
 #include "gpsimu.pb.h"
+#include "licenseplate.pb.h"
+#include "startturnstile.pb.h"
+#include "turnstile.pb.h"
 
 extern std::string gstrserverip;
 extern std::string gstrpatrolPort;
@@ -25,6 +28,10 @@ VehiclePatrolExceptionClient::VehiclePatrolExceptionClient(std::shared_ptr<Chann
 
     ModuleFun funupdate = std::bind(&VehiclePatrolExceptionClient::ListenGPSIMUMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
     shmGPSIMU.mpa = iv::modulecomm::RegisterRecvPlus(shmGPSIMU.mstrmsgname,funupdate);
+    funupdate = std::bind(&VehiclePatrolExceptionClient::ListenTurnstileMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmTurnstile.mpa = iv::modulecomm::RegisterRecvPlus(shmTurnstile.mstrmsgname,funupdate);
+
+    shmStartTurnstile.mpa = iv::modulecomm::RegisterSend(shmStartTurnstile.mstrmsgname,shmStartTurnstile.mnBufferSize,shmStartTurnstile.mnBufferCount);
 }
 
 VehiclePatrolExceptionClient::~VehiclePatrolExceptionClient(void)
@@ -75,6 +82,120 @@ void VehiclePatrolExceptionClient::dec_yaml(const char *stryamlpath)
         shmGPSIMU.mnBufferCount = 1;
     }
 
+    if(config["pic_front"])
+    {
+        if(config["pic_front"]["msgname"]&&config["pic_front"]["buffersize"]&&config["pic_front"]["buffercount"])
+        {
+            strmsgname = config["pic_front"]["msgname"].as<std::string>();
+            strncpy(shmPicFront.mstrmsgname,strmsgname.data(),255);
+            shmPicFront.mnBufferSize = config["pic_front"]["buffersize"].as<int>();
+            shmPicFront.mnBufferCount = config["pic_front"]["buffercount"].as<int>();
+            std::cout << "pic_front:" << shmPicFront.mstrmsgname << "," << shmPicFront.mnBufferSize << "," << shmPicFront.mnBufferCount << std::endl;
+        }
+    }
+    else
+    {
+        strmsgname = "picfront";
+        strncpy(shmPicFront.mstrmsgname,strmsgname.data(),255);
+        shmPicFront.mnBufferSize = 10000000;
+        shmPicFront.mnBufferCount = 1;
+    }
+
+    if(config["pic_rear"])
+    {
+        if(config["pic_rear"]["msgname"]&&config["pic_rear"]["buffersize"]&&config["pic_rear"]["buffercount"])
+        {
+            strmsgname = config["pic_rear"]["msgname"].as<std::string>();
+            strncpy(shmPicRear.mstrmsgname,strmsgname.data(),255);
+            shmPicRear.mnBufferSize = config["pic_rear"]["buffersize"].as<int>();
+            shmPicRear.mnBufferCount = config["pic_rear"]["buffercount"].as<int>();
+            std::cout << "pic_rear:" << shmPicRear.mstrmsgname << "," << shmPicRear.mnBufferSize << "," << shmPicRear.mnBufferCount << std::endl;
+        }
+    }
+    else
+    {
+        strmsgname = "picrear";
+        strncpy(shmPicRear.mstrmsgname,strmsgname.data(),255);
+        shmPicRear.mnBufferSize = 10000000;
+        shmPicRear.mnBufferCount = 1;
+    }
+
+    if(config["pic_left"])
+    {
+        if(config["pic_left"]["msgname"]&&config["pic_left"]["buffersize"]&&config["pic_left"]["buffercount"])
+        {
+            strmsgname = config["pic_left"]["msgname"].as<std::string>();
+            strncpy(shmPicLeft.mstrmsgname,strmsgname.data(),255);
+            shmPicLeft.mnBufferSize = config["pic_left"]["buffersize"].as<int>();
+            shmPicLeft.mnBufferCount = config["pic_left"]["buffercount"].as<int>();
+            std::cout << "pic_left:" << shmPicLeft.mstrmsgname << "," << shmPicLeft.mnBufferSize << "," << shmPicLeft.mnBufferCount << std::endl;
+        }
+    }
+    else
+    {
+        strmsgname = "picleft";
+        strncpy(shmPicLeft.mstrmsgname,strmsgname.data(),255);
+        shmPicLeft.mnBufferSize = 10000000;
+        shmPicLeft.mnBufferCount = 1;
+    }
+
+    if(config["pic_right"])
+    {
+        if(config["pic_right"]["msgname"]&&config["pic_right"]["buffersize"]&&config["pic_right"]["buffercount"])
+        {
+            strmsgname = config["pic_right"]["msgname"].as<std::string>();
+            strncpy(shmPicRight.mstrmsgname,strmsgname.data(),255);
+            shmPicRight.mnBufferSize = config["pic_right"]["buffersize"].as<int>();
+            shmPicRight.mnBufferCount = config["pic_right"]["buffercount"].as<int>();
+            std::cout << "pic_right:" << shmPicRight.mstrmsgname << "," << shmPicRight.mnBufferSize << "," << shmPicRight.mnBufferCount << std::endl;
+        }
+    }
+    else
+    {
+        strmsgname = "picright";
+        strncpy(shmPicRight.mstrmsgname,strmsgname.data(),255);
+        shmPicRight.mnBufferSize = 10000000;
+        shmPicRight.mnBufferCount = 1;
+    }
+
+    if(config["start_turnstile"])
+    {
+        if(config["start_turnstile"]["msgname"]&&config["start_turnstile"]["buffersize"]&&config["start_turnstile"]["buffercount"])
+        {
+            strmsgname = config["start_turnstile"]["msgname"].as<std::string>();
+            strncpy(shmStartTurnstile.mstrmsgname,strmsgname.data(),255);
+            shmStartTurnstile.mnBufferSize = config["start_turnstile"]["buffersize"].as<int>();
+            shmStartTurnstile.mnBufferCount = config["start_turnstile"]["buffercount"].as<int>();
+            std::cout << "start_turnstile:" << shmStartTurnstile.mstrmsgname << "," << shmStartTurnstile.mnBufferSize << "," << shmStartTurnstile.mnBufferCount << std::endl;
+        }
+    }
+    else
+    {
+        strmsgname = "startturnstile";
+        strncpy(shmStartTurnstile.mstrmsgname,strmsgname.data(),255);
+        shmStartTurnstile.mnBufferSize = 10000;
+        shmStartTurnstile.mnBufferCount = 1;
+    }
+
+    if(config["turnstile"])
+    {
+        if(config["turnstile"]["msgname"]&&config["turnstile"]["buffersize"]&&config["turnstile"]["buffercount"])
+        {
+            strmsgname = config["turnstile"]["msgname"].as<std::string>();
+            strncpy(shmTurnstile.mstrmsgname,strmsgname.data(),255);
+            shmTurnstile.mnBufferSize = config["turnstile"]["buffersize"].as<int>();
+            shmTurnstile.mnBufferCount = config["turnstile"]["buffercount"].as<int>();
+            std::cout << "turnstile:" << shmTurnstile.mstrmsgname << "," << shmTurnstile.mnBufferSize << "," << shmTurnstile.mnBufferCount << std::endl;
+        }
+    }
+    else
+    {
+        strmsgname = "turnstile";
+        strncpy(shmTurnstile.mstrmsgname,strmsgname.data(),255);
+        shmTurnstile.mnBufferSize = 10000000;
+        shmTurnstile.mnBufferCount = 1;
+    }
+
     return;
 }
 
@@ -87,14 +208,45 @@ void VehiclePatrolExceptionClient::ListenGPSIMUMsg(const char * strdata,const un
         return;
     }
 
-    gMutex_GPSIMU.lock();
+    double speed = sqrt(xdata.ve()*xdata.ve() + xdata.vn()*xdata.vn() + xdata.vd()*xdata.vd());
+
+    mutex_GPSIMU.lock();
     currentPosition.set_latitude(xdata.lat());
     currentPosition.set_longitude(xdata.lon());
     currentPosition.set_height(xdata.height());
-    gMutex_GPSIMU.unlock();
+    currentSpeed = speed;
+    mutex_GPSIMU.unlock();
 
 }
 
+void VehiclePatrolExceptionClient::ListenTurnstileMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
+{
+    iv::vision::turnstile xdata;
+    if(!xdata.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" ListenTurnstileMsg parese error."<<std::endl;
+        return;
+    }
+
+    if(statusTSGM == 1)
+    {
+        //set TSGM resul
+        isTSGM = true;
+        if(xdata.state() == false)
+            gateStatus = 1; //0 no gate 1 gate close 2 gate open
+        else
+            gateStatus = 2;
+        gateImage.clear();
+        gateImage.append(xdata.pic().picdata().data(),xdata.pic().picdata().size());
+        gateTime = QDateTime::currentMSecsSinceEpoch(); //time when get gateImage
+        mutex_GPSIMU.lock();
+        gatePosition.CopyFrom(currentPosition); //positon when get gateImage
+        mutex_GPSIMU.unlock();
+
+        timerTSGM.restart();
+        statusTSGM = 2;
+    }
+}
 
 std::string VehiclePatrolExceptionClient::uploadVehiclePatrolInfo(void)
 {
@@ -212,10 +364,101 @@ void VehiclePatrolExceptionClient::run()
     xTime.start();
     int lastTime = xTime.elapsed();
     uint64_t interval = std::atoi(gstrpatrolInterval.c_str());
+    //set isNeedTSGM
+    //set gateDestination
+    timerTSGM.start();
     while (!QThread::isInterruptionRequested())
     {
         if(abs(xTime.elapsed() - lastTime)>=interval)
         {
+            // do something
+            mutex_GPSIMU.lock();
+            org::jeecg::defsPatrol::grpc::GPSPoint tempPosition;
+            tempPosition.CopyFrom(currentPosition);
+            double tempSpeed = currentSpeed;
+            mutex_GPSIMU.unlock();
+
+            if(fabs(tempPosition.latitude()-gateDestination.latitude()) < 0.0001 && fabs(tempPosition.longitude()-gateDestination.longitude()) < 0.0001)
+            {
+                if(tempSpeed < 0.1 && statusTSGM == 0 && isNeedTSGM == true)
+                {
+                    iv::vision::startturnstile xmsg;
+
+                    xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
+                    xmsg.set_cameraname(shmPicLeft.mstrmsgname);
+                    xmsg.set_start(true);
+
+                    int ndatasize = xmsg.ByteSize();
+                    char * str = new char[ndatasize];
+                    std::shared_ptr<char> pstr;pstr.reset(str);
+                    if(!xmsg.SerializeToArray(str,ndatasize))
+                    {
+                        std::cout<<"StartTurnstile serialize error."<<std::endl;
+                        return;
+                    }
+                    iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);
+                    statusTSGM = 1;
+                    timerTSGM.restart();
+                }
+            }
+
+            if(statusTSGM == 1 && timerTSGM.elapsed() > 60000)
+            {
+                //timeout
+                iv::vision::startturnstile xmsg;
+
+                xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
+                xmsg.set_cameraname(shmPicLeft.mstrmsgname);
+                xmsg.set_start(false);
+
+                int ndatasize = xmsg.ByteSize();
+                char * str = new char[ndatasize];
+                std::shared_ptr<char> pstr;pstr.reset(str);
+                if(!xmsg.SerializeToArray(str,ndatasize))
+                {
+                    std::cout<<"StartTurnstile serialize error."<<std::endl;
+                    return;
+                }
+                iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);
+
+                isTSGM = false;
+                gateStatus = 0;
+                gateImage.clear();
+                gateTime = QDateTime::currentMSecsSinceEpoch();
+                mutex_GPSIMU.lock();
+                gatePosition.CopyFrom(currentPosition);
+                mutex_GPSIMU.unlock();
+
+                timerTSGM.restart();
+                statusTSGM = 3;
+            }
+
+            if(statusTSGM == 2 || statusTSGM == 3)
+            {
+                if(fabs(tempPosition.latitude()-gateDestination.latitude()) > 0.0001 && fabs(tempPosition.longitude()-gateDestination.longitude()) > 0.0001)
+                {
+                    iv::vision::startturnstile xmsg;
+
+                    xmsg.set_time(QDateTime::currentMSecsSinceEpoch());
+                    xmsg.set_cameraname(shmPicLeft.mstrmsgname);
+                    xmsg.set_start(false);
+
+                    int ndatasize = xmsg.ByteSize();
+                    char * str = new char[ndatasize];
+                    std::shared_ptr<char> pstr;pstr.reset(str);
+                    if(!xmsg.SerializeToArray(str,ndatasize))
+                    {
+                        std::cout<<"StartTurnstile serialize error."<<std::endl;
+                        return;
+                    }
+                    iv::modulecomm::ModuleSendMsg(shmStartTurnstile.mpa,str,ndatasize);
+
+                    timerTSGM.restart();
+                    statusTSGM = 0;
+                }
+            }
+
+
             updatePatrolData();
             std::string reply = uploadVehiclePatrolInfo();
             std::cout<< reply <<std::endl;

src/driver/driver_cloud_grpc_client_BS/vehicle_patrol.h

@@ -60,6 +60,7 @@ public:
     void dec_yaml(const char * stryamlpath);
 
     void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenTurnstileMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
 protected:
     void run();
@@ -68,31 +69,44 @@ private:
     std::unique_ptr<VehiclePatrolException::Stub> stub_;
 
     iv::msgunit shmGPSIMU;
-
-    QMutex gMutex_GPSIMU;
+    iv::msgunit shmPicFront;
+    iv::msgunit shmPicRear;
+    iv::msgunit shmPicLeft;
+    iv::msgunit shmPicRight;
+    iv::msgunit shmStartTurnstile;
+    iv::msgunit shmTurnstile;
+
+    QMutex mutex_GPSIMU;
     org::jeecg::defsPatrol::grpc::GPSPoint currentPosition;
+    double currentSpeed;
 
     std::string id;
 
-    bool isTVR; //Traffic Violation Recognition
+    bool isTVR = false; //Traffic Violation Recognition
     int32_t violationStatus; //0 no violation 1 overspeed 2 illegal parking 3 direction wrong 4 run the red light
     std::string vehicleLicenseNumber;
     QByteArray violationImage;
     uint64_t violationTime; //time when get violationImage
     org::jeecg::defsPatrol::grpc::GPSPoint violationPosition; //positon when get violationImage
 
-    bool isFSM; //Fire and Smoke Monitor
+    bool isFSM = false; //Fire and Smoke Monitor
     int32_t fireStatus; //0 no fire 1 has fire
     QByteArray fireImage;
     uint64_t fireTime; //time when get fireImage
     org::jeecg::defsPatrol::grpc::GPSPoint firePosition; //positon when get fireImage
 
-    bool isTSGM; //Turn Stile Gate Monitor
+    bool isTSGM = false; //Turn Stile Gate Monitor
     int32_t gateStatus; //0 no gate 1 gate close 2 gate open
     QByteArray gateImage;
     uint64_t gateTime; //time when get gateImage
     org::jeecg::defsPatrol::grpc::GPSPoint gatePosition; //positon when get gateImage
 
+    bool isNeedTSGM = false;
+    QTime timerTSGM;
+    uint8_t statusTSGM = 0; //0 ready 1 wait for result 2 has result 3 timeout but no result
+    org::jeecg::defsPatrol::grpc::GPSPoint gateDestination;
+
+
     std::string plateNumber;
 };
 

src/driver/driver_radar_continental_ARS408_SRR308/can_producer_consumer.cpp

@@ -66,7 +66,7 @@ uint64_t CAN_Producer_Consumer::Produce_Elements_From_CANMsg(const iv::can::canm
     return tempPtr;
 }
 
-iv::can::canraw CAN_Producer_Consumer::Consume_Element()
+iv::can::canraw CAN_Producer_Consumer::Consume_Element(void)
 {
     iv::can::canraw xraw;
     usedSpace.acquire(); //this function is block mode

src/driver/driver_radar_continental_ARS408_SRR308/can_producer_consumer.h

@@ -16,7 +16,7 @@ public:
     uint64_t Produce_Element(const iv::can::canraw &xraw); //return the position of first element int this storage
     uint64_t Produce_Elements(const iv::can::canmsg &xmsg,const int &size); //return the position of first element int this storage
     uint64_t Produce_Elements_From_CANMsg(const iv::can::canmsg &xmsg); //return the position of first element int this storage
-    iv::can::canraw Consume_Element();
+    iv::can::canraw Consume_Element(void);
     iv::can::canmsg Consume_Elements(const int &size);
     uint64_t Consume_Element_To_CANMsg(iv::can::canmsg &xmsg); //return the position of the consumed element
 

src/driver/driver_radar_continental_ARS408_SRR308/canrecv_consumer.cpp

@@ -83,9 +83,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch0(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -109,9 +109,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch0(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -208,9 +208,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch1(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -234,9 +234,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch1(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -333,9 +333,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch2(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -359,9 +359,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch2(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -458,9 +458,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch3(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -484,9 +484,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch3(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -583,9 +583,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch4(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -609,9 +609,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch4(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -708,9 +708,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch5(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -734,9 +734,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch5(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -833,9 +833,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch6(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -859,9 +859,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch6(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -958,9 +958,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch7(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
             emit RadarObjectArray_Ready();
         }
         else
@@ -984,9 +984,9 @@ void CANRecv_Consumer::Pack_Info_To_SHM_ch7(uint32_t can_id)
             xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
             xradarObjArray.set_radar_version(radar_Version);
 
-            radarObjArraySend_lock.lock();
-            radarObjArray_send.CopyFrom(xradarObjArray);
-            radarObjArraySend_lock.unlock();
+            radarObjArraySend_Lock.lock();
+            radarObjArray_Send.CopyFrom(xradarObjArray);
+            radarObjArraySend_Lock.unlock();
 //            std::cout<<radarObjArray_send.obj_size()<<std::endl;
             emit RadarObjectArray_Ready();
         }
@@ -1236,14 +1236,14 @@ void CANRecv_Consumer::run()
     }
 }
 
-void CANRecv_Consumer::RadarObjectArray_Ready_Slot()
+void CANRecv_Consumer::RadarObjectArray_Ready_Slot(void)
 {
-    radarObjArraySend_lock.lock();
-    int ndatasize = radarObjArray_send.ByteSize();
+    radarObjArraySend_Lock.lock();
+    int ndatasize = radarObjArray_Send.ByteSize();
     char * strser = new char[ndatasize];
     std::shared_ptr<char> pstrser;
     pstrser.reset(strser);
-    if(radarObjArray_send.SerializePartialToArray(strser,ndatasize))
+    if(radarObjArray_Send.SerializePartialToArray(strser,ndatasize))
     {
         iv::modulecomm::ModuleSendMsg(shmRadar.mpa,strser,ndatasize);
     }
@@ -1251,5 +1251,5 @@ void CANRecv_Consumer::RadarObjectArray_Ready_Slot()
     {
         std::cout<<"RadarObjectArray serialize error."<<std::endl;
     }
-    radarObjArraySend_lock.unlock();
+    radarObjArraySend_Lock.unlock();
 }

src/driver/driver_radar_continental_ARS408_SRR308/canrecv_consumer.h

@@ -42,10 +42,10 @@ protected:
     void Rotate_Translate(double &x,double &y,double theta,double offset_x,double offset_y); //RFU,deg
 
 signals:
-    void RadarObjectArray_Ready();
+    void RadarObjectArray_Ready(void);
 
 private slots:
-    void RadarObjectArray_Ready_Slot();
+    void RadarObjectArray_Ready_Slot(void);
 
 private:
     CAN_Producer_Consumer *pBuffer;
@@ -78,8 +78,8 @@ private:
     bool yaw_Rate_Loss = false; //
     std::string radar_Version = "4.30.1"; //
 
-    iv::radar::radarobjectarray radarObjArray_send;
-    QMutex radarObjArraySend_lock;
+    iv::radar::radarobjectarray radarObjArray_Send;
+    QMutex radarObjArraySend_Lock;
 };
 
 #endif // CANRECV_CONSUMER_H

src/driver/driver_ultrasonic_dauxi_KS136A/can_producer_consumer.cpp

@@ -66,7 +66,7 @@ uint64_t CAN_Producer_Consumer::Produce_Elements_From_CANMsg(const iv::can::canm
     return tempPtr;
 }
 
-iv::can::canraw CAN_Producer_Consumer::Consume_Element()
+iv::can::canraw CAN_Producer_Consumer::Consume_Element(void)
 {
     iv::can::canraw xraw;
     usedSpace.acquire(); //this function is block mode
@@ -78,6 +78,21 @@ iv::can::canraw CAN_Producer_Consumer::Consume_Element()
     return xraw;
 }
 
+iv::can::canraw CAN_Producer_Consumer::Consume_Element(int timeout)
+{
+    iv::can::canraw xraw;
+    bool tempResult = usedSpace.tryAcquire(1,timeout);
+    if(tempResult)
+    {
+        consumerLock.lock();
+        xraw = buffer.at(consumerPtr++ % bufferSize);
+        consumerLock.unlock();
+        freeSpace.release();
+    }
+
+    return xraw;
+}
+
 iv::can::canmsg CAN_Producer_Consumer::Consume_Elements(const int &size)
 {
     iv::can::canmsg xmsg;
@@ -105,3 +120,19 @@ uint64_t CAN_Producer_Consumer::Consume_Element_To_CANMsg(iv::can::canmsg &xmsg)
 
     return tempPtr;
 }
+
+uint64_t CAN_Producer_Consumer::Consume_Element_To_CANMsg(iv::can::canmsg &xmsg,int timeout)
+{
+    uint64_t tempPtr = consumerPtr % bufferSize;
+    bool tempResult = usedSpace.tryAcquire(1,timeout); //this function is block mode
+    if(tempResult)
+    {
+        iv::can::canraw *praw = xmsg.add_rawmsg();
+        consumerLock.lock();
+        praw->CopyFrom(buffer.at(consumerPtr++ % bufferSize));
+        consumerLock.unlock();
+        freeSpace.release();
+    }
+
+    return tempPtr;
+}

src/driver/driver_ultrasonic_dauxi_KS136A/can_producer_consumer.h

@@ -16,9 +16,11 @@ public:
     uint64_t Produce_Element(const iv::can::canraw &xraw); //return the position of first element int this storage
     uint64_t Produce_Elements(const iv::can::canmsg &xmsg,const int &size); //return the position of first element int this storage
     uint64_t Produce_Elements_From_CANMsg(const iv::can::canmsg &xmsg); //return the position of first element int this storage
-    iv::can::canraw Consume_Element();
+    iv::can::canraw Consume_Element(void);
+    iv::can::canraw Consume_Element(int timeout); //not block mode
     iv::can::canmsg Consume_Elements(const int &size);
     uint64_t Consume_Element_To_CANMsg(iv::can::canmsg &xmsg); //return the position of the consumed element
+    uint64_t Consume_Element_To_CANMsg(iv::can::canmsg &xmsg,int timeout); //not block mode
 
     QVector<iv::can::canraw> buffer;
 

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.cpp

@@ -1,1255 +1,169 @@
-//#include "canrecv_consumer.h"
-
-//#include "math.h"
-
-//extern setupConfig_t setupConfig;
-//extern iv::msgunit shmSonar;
-
-//CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
-//{
-//    pBuffer = pBuf;
-
-//    QObject::connect(this,&CANRecv_Consumer::RadarObjectArray_Ready,this,&CANRecv_Consumer::RadarObjectArray_Ready_Slot);
-
-//    iv::radar::radarobject xradarObj;
-//    vradarObj.fill(xradarObj,RADAR_OBJ_MAX_NUM);
-//    radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//}
-
-//CANRecv_Consumer::~CANRecv_Consumer()
-//{
-//    requestInterruption();
-//    while(this->isFinished() == false);
-//}
-
-//void CANRecv_Consumer::Clear_CAN_PrivateTempVariable(void)
-//{
-//    CAN_ID = 0x000u;
-//    CAN_DLC = 0;
-//    for(int i=0;i<8;i++) CAN_data[i] = 0x00;
-//}
-
-//uint32_t CANRecv_Consumer::Trans_From_CANRaw(const iv::can::canraw &xraw) //only for standard data frame, return can id
-//{
-//    this->Clear_CAN_PrivateTempVariable();
-//    CAN_ID = xraw.id();
-//    CAN_DLC = xraw.len();
-//    char tempData[8];
-//    int tempLen = (xraw.data().size() > 8) ? 8 : xraw.data().size();
-//    strncpy(tempData,xraw.data().data(),tempLen);
-//    for(int i=0;i<tempLen;i++)
-//    {
-//        CAN_data[i] = (uint8_t)tempData[i];
-//    }
-
-//    return CAN_ID;
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch0(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch0_rx.RadarState_ch0.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch0_rx.RadarState_ch0.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch0_rx.RadarState_ch0.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch0_rx.RadarState_ch0.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch0_rx.RadarState_ch0.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch0_rx.RadarState_ch0.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch0_rx.RadarState_ch0.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch0_rx.VersionID_ch0.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch0_rx.VersionID_ch0.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch0_rx.VersionID_ch0.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch0_rx.Obj_0_Status_ch0.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch0_rx.Obj_1_General_ch0.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch0_rx.Obj_2_Quality_ch0.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ID;
-//        tempAX = -ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch0_rx.Obj_3_Extended_ch0.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch1(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id - 0x010) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch1_rx.RadarState_ch1.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch1_rx.RadarState_ch1.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch1_rx.VersionID_ch1.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch1_rx.VersionID_ch1.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch1_rx.VersionID_ch1.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch1_rx.Obj_0_Status_ch1.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch1_rx.Obj_1_General_ch1.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch1_rx.Obj_2_Quality_ch1.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ID;
-//        tempAX = -ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch1_rx.Obj_3_Extended_ch1.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch2(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id - 0x020) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch2_rx.RadarState_ch2.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch2_rx.RadarState_ch2.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch2_rx.VersionID_ch2.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch2_rx.VersionID_ch2.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch2_rx.VersionID_ch2.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch2_rx.Obj_0_Status_ch2.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch2_rx.Obj_1_General_ch2.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch2_rx.Obj_2_Quality_ch2.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ID;
-//        tempAX = -ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch2_rx.Obj_3_Extended_ch2.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch3(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id - 0x030) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch3_rx.RadarState_ch3.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch3_rx.RadarState_ch3.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch3_rx.VersionID_ch3.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch3_rx.VersionID_ch3.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch3_rx.VersionID_ch3.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch3_rx.Obj_0_Status_ch3.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch3_rx.Obj_1_General_ch3.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch3_rx.Obj_2_Quality_ch3.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ID;
-//        tempAX = -ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch3_rx.Obj_3_Extended_ch3.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch4(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id - 0x040) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch4_rx.RadarState_ch4.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch4_rx.RadarState_ch4.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch4_rx.VersionID_ch4.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch4_rx.VersionID_ch4.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch4_rx.VersionID_ch4.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch4_rx.Obj_0_Status_ch4.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch4_rx.Obj_1_General_ch4.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch4_rx.Obj_2_Quality_ch4.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ID;
-//        tempAX = -ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch4_rx.Obj_3_Extended_ch4.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch5(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id - 0x050) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch5_rx.RadarState_ch5.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch5_rx.RadarState_ch5.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch5_rx.VersionID_ch5.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch5_rx.VersionID_ch5.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch5_rx.VersionID_ch5.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch5_rx.Obj_0_Status_ch5.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch5_rx.Obj_1_General_ch5.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch5_rx.Obj_2_Quality_ch5.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ID;
-//        tempAX = -ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch5_rx.Obj_3_Extended_ch5.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch6(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id - 0x060) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch6_rx.RadarState_ch6.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch6_rx.RadarState_ch6.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch6_rx.VersionID_ch6.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch6_rx.VersionID_ch6.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch6_rx.VersionID_ch6.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch6_rx.Obj_0_Status_ch6.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch6_rx.Obj_1_General_ch6.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch6_rx.Obj_2_Quality_ch6.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ID;
-//        tempAX = -ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch6_rx.Obj_3_Extended_ch6.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//void CANRecv_Consumer::Pack_Info_To_SHM_ch7(uint32_t can_id)
-//{
-//    std::string major,minor,patch;
-//    iv::radar::radarobject *pobj;
-//    iv::radar::radarobject xradarObj_null;
-//    double tempDX,tempDY,tempVX,tempVY,tempAX,tempAY,tempOri;
-//    uint8_t tempObjID;
-
-//    switch (can_id - 0x070) {
-//    case 0x201:
-//        persistent_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Persistent_Error;
-//        interference_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Interference;
-//        temperature_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Temperature_Error;
-//        temporary_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Temporary_Error;
-//        voltage_Error = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_Voltage_Error;
-//        speed_Loss = ars408_can_database_ch7_rx.RadarState_ch7.RadarState_MotionRxState & 0x01;
-//        yaw_Rate_Loss = (ars408_can_database_ch7_rx.RadarState_ch7.RadarState_MotionRxState >> 1) & 0x01;
-//        break;
-//    case 0x700:
-//        major = std::to_string((unsigned int)ars408_can_database_ch7_rx.VersionID_ch7.Version_MajorRelease);
-//        minor = std::to_string((unsigned int)ars408_can_database_ch7_rx.VersionID_ch7.Version_MinorRelease);
-//        patch = std::to_string((unsigned int)ars408_can_database_ch7_rx.VersionID_ch7.Version_PatchLevel);
-//        radar_Version = major + "." + minor + "." + patch;
-//        break;
-//    case 0x60A:
-//        if(ars408_can_database_ch7_rx.Obj_0_Status_ch7.Obj_NofObjects == 0)
-//        {
-//            xradarObjArray.Clear();
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-//            emit RadarObjectArray_Ready();
-//        }
-//        else
-//        {
-//            xradarObjArray.Clear();
-//            for(int i=0;i<RADAR_OBJ_MAX_NUM;i++)
-//            {
-//                if(radarObjFreshFlag[i] == true)
-//                {
-//                    pobj = xradarObjArray.add_obj();
-//                    pobj->CopyFrom(vradarObj[i]);
-//                }
-//            }
-//            xradarObjArray.set_mstime(QDateTime::currentMSecsSinceEpoch());
-//            xradarObjArray.set_persistent_error(persistent_Error);
-//            xradarObjArray.set_interference_error(interference_Error);
-//            xradarObjArray.set_temperature_error(temperature_Error);
-//            xradarObjArray.set_temporary_error(temporary_Error);
-//            xradarObjArray.set_voltage_error(voltage_Error);
-//            xradarObjArray.set_speed_loss(speed_Loss);
-//            xradarObjArray.set_yaw_rate_loss(yaw_Rate_Loss);
-//            xradarObjArray.set_radar_version(radar_Version);
-
-//            radarObjArraySend_lock.lock();
-//            radarObjArray_send.CopyFrom(xradarObjArray);
-//            radarObjArraySend_lock.unlock();
-////            std::cout<<radarObjArray_send.obj_size()<<std::endl;
-//            emit RadarObjectArray_Ready();
-//        }
-//        //clear the obj vector and flag vector
-//        vradarObj.fill(xradarObj_null,RADAR_OBJ_MAX_NUM);
-//        radarObjFreshFlag.fill(false,RADAR_OBJ_MAX_NUM);
-//        break;
-//    case 0x60B:
-//        tempObjID = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_ID;
-//        radarObjFreshFlag[tempObjID] = true;
-//        tempDX = -ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DistLat_phys;
-//        tempDY = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DistLong_phys;
-//        this->Rotate_Translate(tempDX,tempDY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_x(tempDX);
-//        vradarObj[tempObjID].set_y(tempDY);
-//        tempVX = -ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_VrelLat_phys;
-//        tempVY = ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_VrelLong_phys;
-//        this->Rotate_Translate(tempVX,tempVY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_vx(tempVX);
-//        vradarObj[tempObjID].set_vy(tempVY);
-//        vradarObj[tempObjID].set_vel(sqrt(tempVX*tempVX+tempVY*tempVY));
-//        vradarObj[tempObjID].set_bvalid(true);
-//        vradarObj[tempObjID].set_id(tempObjID);
-//        vradarObj[tempObjID].set_dynamic_property(ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_DynProp);
-//        vradarObj[tempObjID].set_rcs_radarcrosssection(ars408_can_database_ch7_rx.Obj_1_General_ch7.Obj_RCS_phys);
-//        break;
-//    case 0x60C:
-//        tempObjID = ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_ID;
-//        vradarObj[tempObjID].set_distlong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_DistLong_rms));
-//        vradarObj[tempObjID].set_distlat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_DistLat_rms));
-//        vradarObj[tempObjID].set_vrellong_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_VrelLong_rms));
-//        vradarObj[tempObjID].set_vrellat_rms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_VrelLat_rms));
-//        vradarObj[tempObjID].set_object_arellatrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_ArelLat_rms));
-//        vradarObj[tempObjID].set_object_arellongrms(this->Trans_DVA_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_ArelLong_rms));
-//        vradarObj[tempObjID].set_object_orientationrms(this->Trans_Ori_RMS_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_Orientation_rms));
-//        vradarObj[tempObjID].set_object_measurestate(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_MeasState);
-//        vradarObj[tempObjID].set_object_probofexist(this->Trans_ProbOfExist_Ro_To_Phy(ars408_can_database_ch7_rx.Obj_2_Quality_ch7.Obj_ProbOfExist));
-//        break;
-//    case 0x60D:
-//        tempObjID = ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ID;
-//        tempAX = -ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ArelLat_phys;
-//        tempAY = ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_ArelLong_phys;
-//        this->Rotate_Translate(tempAX,tempAY,setupConfig.radarAzimuth,setupConfig.radarOffsetX,setupConfig.radarOffsetY);
-//        vradarObj[tempObjID].set_object_arellong(tempAY);
-//        vradarObj[tempObjID].set_object_arellat(tempAX);
-//        tempOri = ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_OrientationAngle_phys + setupConfig.radarAzimuth;
-//        if(tempOri > 180.0)tempOri = tempOri - 360.0;
-//        if(tempOri < -180.0)tempOri = tempOri + 360.0;
-//        vradarObj[tempObjID].set_object_orientationangle(tempOri);
-//        vradarObj[tempObjID].set_object_class(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_Class);
-//        vradarObj[tempObjID].set_object_length(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_Length_phys);
-//        vradarObj[tempObjID].set_object_width(ars408_can_database_ch7_rx.Obj_3_Extended_ch7.Obj_Width_phys);
-//        break;
-//    default:
-//        break;
-//    }
-//}
-
-//double CANRecv_Consumer::Trans_DVA_RMS_Ro_To_Phy(uint8_t data)
-//{
-//    double phy;
-//    switch (data) {
-//    case 0x0: phy = 0.005;break;
-//    case 0x1: phy = 0.006;break;
-//    case 0x2: phy = 0.008;break;
-//    case 0x3: phy = 0.011;break;
-//    case 0x4: phy = 0.014;break;
-//    case 0x5: phy = 0.018;break;
-//    case 0x6: phy = 0.023;break;
-//    case 0x7: phy = 0.029;break;
-//    case 0x8: phy = 0.038;break;
-//    case 0x9: phy = 0.049;break;
-//    case 0xA: phy = 0.063;break;
-//    case 0xB: phy = 0.081;break;
-//    case 0xC: phy = 0.105;break;
-//    case 0xD: phy = 0.135;break;
-//    case 0xE: phy = 0.174;break;
-//    case 0xF: phy = 0.224;break;
-//    case 0x10: phy = 0.288;break;
-//    case 0x11: phy = 0.371;break;
-//    case 0x12: phy = 0.478;break;
-//    case 0x13: phy = 0.616;break;
-//    case 0x14: phy = 0.794;break;
-//    case 0x15: phy = 1.023;break;
-//    case 0x16: phy = 1.317;break;
-//    case 0x17: phy = 1.697;break;
-//    case 0x18: phy = 2.187;break;
-//    case 0x19: phy = 2.817;break;
-//    case 0x1A: phy = 3.630;break;
-//    case 0x1B: phy = 4.676;break;
-//    case 0x1C: phy = 6.025;break;
-//    case 0x1D: phy = 7.762;break;
-//    case 0x1E: phy = 10.000;break;
-//    case 0x1F: phy = -1.0;break;
-//    default:
-//        phy =  -1.0;
-//        break;
-//    }
-
-//    return phy;
-//}
-
-//double CANRecv_Consumer::Trans_Ori_RMS_Ro_To_Phy(uint8_t data)
-//{
-//    double phy;
-//    switch (data) {
-//    case 0x0: phy = 0.005;break;
-//    case 0x1: phy = 0.007;break;
-//    case 0x2: phy = 0.010;break;
-//    case 0x3: phy = 0.014;break;
-//    case 0x4: phy = 0.020;break;
-//    case 0x5: phy = 0.029;break;
-//    case 0x6: phy = 0.041;break;
-//    case 0x7: phy = 0.058;break;
-//    case 0x8: phy = 0.082;break;
-//    case 0x9: phy = 0.116;break;
-//    case 0xA: phy = 0.165;break;
-//    case 0xB: phy = 0.234;break;
-//    case 0xC: phy = 0.332;break;
-//    case 0xD: phy = 0.471;break;
-//    case 0xE: phy = 0.669;break;
-//    case 0xF: phy = 0.949;break;
-//    case 0x10: phy = 1.346;break;
-//    case 0x11: phy = 1.909;break;
-//    case 0x12: phy = 2.709;break;
-//    case 0x13: phy = 3.843;break;
-//    case 0x14: phy = 5.451;break;
-//    case 0x15: phy = 7.734;break;
-//    case 0x16: phy = 10.971;break;
-//    case 0x17: phy = 15.565;break;
-//    case 0x18: phy = 22.081;break;
-//    case 0x19: phy = 31.325;break;
-//    case 0x1A: phy = 44.439;break;
-//    case 0x1B: phy = 63.044;break;
-//    case 0x1C: phy = 89.437;break;
-//    case 0x1D: phy = 126.881;break;
-//    case 0x1E: phy = 180.000;break;
-//    case 0x1F: phy = -1.0;break;
-//    default:
-//        phy =  -1.0;
-//        break;
-//    }
-
-//    return phy;
-//}
-
-//double CANRecv_Consumer::Trans_ProbOfExist_Ro_To_Phy(uint8_t data)
-//{
-//    double phy;
-//    switch (data) {
-//    case 0x0:
-//        phy = -1.0;
-//        break;
-//    case 0x1:
-//        phy = 25.0;
-//        break;
-//    case 0x2:
-//        phy = 50.0;
-//        break;
-//    case 0x3:
-//        phy = 75.0;
-//        break;
-//    case 0x4:
-//        phy = 90.0;
-//        break;
-//    case 0x5:
-//        phy = 99.0;
-//        break;
-//    case 0x6:
-//        phy = 99.9;
-//        break;
-//    case 0x7:
-//        phy = 100.0;
-//        break;
-//    default:
-//        phy = 0.0;
-//        break;
-//    }
-
-//    return phy;
-//}
-
-//void CANRecv_Consumer::Rotate_Translate(double &x,double &y,double theta,double offset_x,double offset_y) //RFU,deg
-//{
-//    double tempX = x;
-//    double tempY = y;
-//    double tempTheta = theta / 180.0 * M_PI;
-//    double tempSinTheta = sin(tempTheta);
-//    double tempCosTheta = cos(tempTheta);
-
-//    x = (tempX * tempCosTheta + tempY * tempSinTheta) + offset_x;
-//    y = (tempY * tempCosTheta - tempX * tempSinTheta) + offset_y;
-//}
-
-//void CANRecv_Consumer::run()
-//{
-//    radarType = setupConfig.radarType;
-
-//    uint32_t tempCANID = 0x000u;
-
-//    while (!QThread::isInterruptionRequested())
-//    {
-//        switch (setupConfig.radarID) {
-//        case 0:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch0_Receive(&ars408_can_database_ch0_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch0(tempCANID);
-//            break;
-//        case 1:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch1_Receive(&ars408_can_database_ch1_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch1(tempCANID);
-//            break;
-//        case 2:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch2_Receive(&ars408_can_database_ch2_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch2(tempCANID);
-//            break;
-//        case 3:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch3_Receive(&ars408_can_database_ch3_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch3(tempCANID);
-//            break;
-//        case 4:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch4_Receive(&ars408_can_database_ch4_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch4(tempCANID);
-//            break;
-//        case 5:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch5_Receive(&ars408_can_database_ch5_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch5(tempCANID);
-//            break;
-//        case 6:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch6_Receive(&ars408_can_database_ch6_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch6(tempCANID);
-//            break;
-//        case 7:
-//            this->Trans_From_CANRaw(pBuffer->Consume_Element());
-//            tempCANID = ars408_can_database_ch7_Receive(&ars408_can_database_ch7_rx,CAN_data,CAN_ID,CAN_DLC);
-//            Pack_Info_To_SHM_ch7(tempCANID);
-//            break;
-//        default:
-//            break;
-//        }
-//    }
-//}
-
-//void CANRecv_Consumer::RadarObjectArray_Ready_Slot()
-//{
-//    radarObjArraySend_lock.lock();
-//    int ndatasize = radarObjArray_send.ByteSize();
-//    char * strser = new char[ndatasize];
-//    std::shared_ptr<char> pstrser;
-//    pstrser.reset(strser);
-//    if(radarObjArray_send.SerializePartialToArray(strser,ndatasize))
-//    {
-//        iv::modulecomm::ModuleSendMsg(shmSonar.mpa,strser,ndatasize);
-//    }
-//    else
-//    {
-//        std::cout<<"RadarObjectArray serialize error."<<std::endl;
-//    }
-//    radarObjArraySend_lock.unlock();
-//}
+#include "canrecv_consumer.h"
+
+#include "math.h"
+
+extern setupConfig_t setupConfig;
+extern iv::msgunit shmSonar;
+
+CANRecv_Consumer::CANRecv_Consumer(CAN_Producer_Consumer *pBuf)
+{
+    pBuffer = pBuf;
+
+    QObject::connect(this,&CANRecv_Consumer::UltrasonicData_Ready,this,&CANRecv_Consumer::UltrasonicData_Ready_Slot);
+
+    objDist.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
+    sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
+}
+
+CANRecv_Consumer::~CANRecv_Consumer()
+{
+    requestInterruption();
+    while(this->isFinished() == false);
+}
+
+void CANRecv_Consumer::Clear_CAN_PrivateTempVariable(void)
+{
+    CAN_ID = 0x000u;
+    CAN_DLC = 0;
+    for(int i=0;i<8;i++) CAN_data[i] = 0x00;
+}
+
+uint32_t CANRecv_Consumer::Trans_From_CANRaw(const iv::can::canraw &xraw) //only for standard data frame, return can id
+{
+    this->Clear_CAN_PrivateTempVariable();
+    CAN_ID = xraw.id();
+    CAN_DLC = xraw.len();
+    char tempData[8];
+    int tempLen = (xraw.data().size() > 8) ? 8 : xraw.data().size();
+    strncpy(tempData,xraw.data().data(),tempLen);
+    for(int i=0;i<tempLen;i++)
+    {
+        CAN_data[i] = (uint8_t)tempData[i];
+    }
+
+    return CAN_ID;
+}
+
+void CANRecv_Consumer::run()
+{
+    iv::can::canraw tempCANRaw;
+    uint8_t tempSensorID = 0;
+    decodeTimer.start();
+
+    while (!QThread::isInterruptionRequested())
+    {
+        tempCANRaw.CopyFrom(pBuffer->Consume_Element(1));
+        if(decodeEnableFlag == true)
+        {
+            if(decodeTimer.elapsed() >= 25)
+            {
+                decodeEnableFlag = false;
+                objDist[decodeSensorID] = DIST_ERROR;
+                sensorStatus[decodeSensorID] = false;
+                tempSensorID = decodeSensorID + 1;
+                if(tempSensorID >= NUM_OF_SENSOR_MAX)
+                {
+                    //copy and send
+                    sonarSend_Lock.lock();
+                    objDist_Send.clear();
+                    sensorStatus_Send.clear();
+                    objDist_Send.swap(objDist);
+                    sensorStatus_Send.swap(sensorStatus);
+                    sonarSend_Lock.unlock();
+                    emit UltrasonicData_Ready();
+                    //clear
+                    objDist.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
+                    sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
+                    tempSensorID = 0;
+                }
+                emit Enable_Ask(true,tempSensorID);
+                decodeTimer.restart();
+            }
+            else
+            {
+                this->Trans_From_CANRaw(tempCANRaw);
+                if(CAN_ID == setupConfig.sonarCAN_ID && CAN_DLC == 2)
+                {
+                    uint16_t tempDist = (CAN_data[0] << 8) | CAN_data[1];
+                    if(tempDist <= 0x1493 && tempDist >= 0x88)
+                    {
+                        decodeEnableFlag = false;
+                        objDist[decodeSensorID] = tempDist;
+                        sensorStatus[decodeSensorID] = true;
+                        tempSensorID = decodeSensorID + 1;
+                        if(tempSensorID >= NUM_OF_SENSOR_MAX)
+                        {
+                            //copy and send
+                            sonarSend_Lock.lock();
+                            objDist_Send.clear();
+                            sensorStatus_Send.clear();
+                            objDist_Send.swap(objDist);
+                            sensorStatus_Send.swap(sensorStatus);
+                            sonarSend_Lock.unlock();
+                            emit UltrasonicData_Ready();
+                            //clear
+                            objDist.fill(DIST_ERROR,NUM_OF_SENSOR_MAX);
+                            sensorStatus.fill(false,NUM_OF_SENSOR_MAX);
+                            tempSensorID = 0;
+                        }
+                        emit Enable_Ask(true,tempSensorID);
+                        decodeTimer.restart();
+                    }
+                }
+            }
+        }
+    }
+}
+
+void CANRecv_Consumer::Enable_DecodeResult_Slot(bool enableFlag,uint8_t sensorID)
+{
+    decodeEnableFlag = enableFlag;
+    decodeSensorID = sensorID;
+    decodeTimer.restart();
+}
+
+void CANRecv_Consumer::UltrasonicData_Ready_Slot(void)
+{
+    iv::ultrasonic::ultrasonic xmsg;
+    sonarSend_Lock.lock();
+    xmsg.set_sigobjdist_flside(objDist_Send[0]);
+    xmsg.set_sigobjdist_flcorner(objDist_Send[1]);
+    xmsg.set_sigobjdist_flmiddle(objDist_Send[2]);
+    xmsg.set_sigobjdist_frmiddle(objDist_Send[3]);
+    xmsg.set_sigobjdist_frcorner(objDist_Send[4]);
+    xmsg.set_sigobjdist_frside(objDist_Send[5]);
+    xmsg.set_sigobjdist_rrside(objDist_Send[6]);
+    xmsg.set_sigobjdist_rrcorner(objDist_Send[7]);
+    xmsg.set_sigobjdist_rrmiddle(objDist_Send[8]);
+    xmsg.set_sigobjdist_rlmiddle(objDist_Send[9]);
+    xmsg.set_sigobjdist_rlcorner(objDist_Send[10]);
+    xmsg.set_sigobjdist_rlside(objDist_Send[11]);
+
+    xmsg.set_sigsensor_front_ls(sensorStatus_Send[0]);
+    xmsg.set_sigsensor_front_l(sensorStatus_Send[1]);
+    xmsg.set_sigsensor_front_lm(sensorStatus_Send[2]);
+    xmsg.set_sigsensor_front_rm(sensorStatus_Send[3]);
+    xmsg.set_sigsensor_front_r(sensorStatus_Send[4]);
+    xmsg.set_sigsensor_front_rs(sensorStatus_Send[5]);
+    xmsg.set_sigsensor_rear_rs(sensorStatus_Send[6]);
+    xmsg.set_sigsensor_rear_r(sensorStatus_Send[7]);
+    xmsg.set_sigsensor_rear_rm(sensorStatus_Send[8]);
+    xmsg.set_sigsensor_rear_lm(sensorStatus_Send[9]);
+    xmsg.set_sigsensor_rear_l(sensorStatus_Send[10]);
+    xmsg.set_sigsensor_rear_ls(sensorStatus_Send[11]);
+    sonarSend_Lock.unlock();
+
+    xmsg.set_timestamp(QDateTime::currentMSecsSinceEpoch());
+    int ndatasize = xmsg.ByteSize();
+    char * strser = new char[ndatasize];
+    std::shared_ptr<char> pstrser;
+    pstrser.reset(strser);
+    if(xmsg.SerializePartialToArray(strser,ndatasize))
+    {
+        iv::modulecomm::ModuleSendMsg(shmSonar.mpa,strser,ndatasize);
+    }
+    else
+    {
+        std::cout<<"ultrasonic data serialize error."<<std::endl;
+    }
+}

src/driver/driver_ultrasonic_dauxi_KS136A/canrecv_consumer.h

@@ -1,85 +1,65 @@
 #ifndef CANRECV_CONSUMER_H
 #define CANRECV_CONSUMER_H
 
-//#include <QObject>
-//#include <QTimer>
-//#include <QDateTime>
-//#include <QThread>
-
-//#include "modulecomm.h"
-//#include "radarobjectarray.pb.h"
-
-//#include "can_producer_consumer.h"
-//#include "decode_cfg.h"
-//#include "iv_msgunit.h"
-
-//#define RADAR_OBJ_MAX_NUM 128
-
-//class CANRecv_Consumer : public QThread
-//{
-//    Q_OBJECT
-//public:
-//    CANRecv_Consumer(CAN_Producer_Consumer *pBuf);
-//    ~CANRecv_Consumer();
-//    void Clear_CAN_PrivateTempVariable(void);
-
-//protected:
-//    void run();
-//    uint32_t Trans_From_CANRaw(const iv::can::canraw &xraw); //only for standard data frame, return can id
-//    void Pack_Info_To_SHM_ch0(uint32_t can_id);
-//    void Pack_Info_To_SHM_ch1(uint32_t can_id);
-//    void Pack_Info_To_SHM_ch2(uint32_t can_id);
-//    void Pack_Info_To_SHM_ch3(uint32_t can_id);
-//    void Pack_Info_To_SHM_ch4(uint32_t can_id);
-//    void Pack_Info_To_SHM_ch5(uint32_t can_id);
-//    void Pack_Info_To_SHM_ch6(uint32_t can_id);
-//    void Pack_Info_To_SHM_ch7(uint32_t can_id);
-//    //ro trans phy 3
-//    double Trans_DVA_RMS_Ro_To_Phy(uint8_t data);
-//    double Trans_Ori_RMS_Ro_To_Phy(uint8_t data);
-//    double Trans_ProbOfExist_Ro_To_Phy(uint8_t data);
-//    //rotation and translation
-//    void Rotate_Translate(double &x,double &y,double theta,double offset_x,double offset_y); //RFU,deg
-
-//signals:
-//    void RadarObjectArray_Ready();
-
-//private slots:
-//    void RadarObjectArray_Ready_Slot();
-
-//private:
-//    CAN_Producer_Consumer *pBuffer;
-
-//    uint8_t radarType = 0; //0 ARS408-21xx 1 ARS408-21sc3 2 SRR308
-
-//    uint32_t CAN_ID = 0x000u;
-//    uint8_t CAN_DLC = 0;
-//    uint8_t CAN_data[8] = {0};
-
-//    ars408_can_database_ch0_rx_t ars408_can_database_ch0_rx;
-//    ars408_can_database_ch1_rx_t ars408_can_database_ch1_rx;
-//    ars408_can_database_ch2_rx_t ars408_can_database_ch2_rx;
-//    ars408_can_database_ch3_rx_t ars408_can_database_ch3_rx;
-//    ars408_can_database_ch4_rx_t ars408_can_database_ch4_rx;
-//    ars408_can_database_ch5_rx_t ars408_can_database_ch5_rx;
-//    ars408_can_database_ch6_rx_t ars408_can_database_ch6_rx;
-//    ars408_can_database_ch7_rx_t ars408_can_database_ch7_rx;
-
-//    iv::radar::radarobjectarray xradarObjArray;
-//    QVector<iv::radar::radarobject> vradarObj;
-//    QVector<bool> radarObjFreshFlag;
-
-//    bool persistent_Error = false; //notice if multi thread, these param need a mutex lock
-//    bool interference_Error = false; //
-//    bool temperature_Error = false; //
-//    bool temporary_Error = false; //
-//    bool voltage_Error = false; //
-//    bool speed_Loss = false; //
-//    bool yaw_Rate_Loss = false; //
-//    std::string radar_Version = "4.30.1"; //
-
-//    iv::radar::radarobjectarray radarObjArray_send;
-//    QMutex radarObjArraySend_lock;
-//};
+#include <QObject>
+#include <QTimer>
+#include <QDateTime>
+#include <QThread>
+
+#include "modulecomm.h"
+#include "ultrasonic.pb.h"
+
+#include "can_producer_consumer.h"
+#include "decode_cfg.h"
+#include "iv_msgunit.h"
+
+#ifndef NUM_OF_SENSOR_MAX
+#define NUM_OF_SENSOR_MAX 12
+#endif
+
+#ifndef DIST_ERROR
+#define DIST_ERROR 50000 //50000mm
+#endif
+
+class CANRecv_Consumer : public QThread
+{
+    Q_OBJECT
+public:
+    CANRecv_Consumer(CAN_Producer_Consumer *pBuf);
+    ~CANRecv_Consumer();
+    void Clear_CAN_PrivateTempVariable(void);
+
+public slots:
+    void Enable_DecodeResult_Slot(bool enableFlag,uint8_t sensorID);
+
+protected:
+    void run();
+    uint32_t Trans_From_CANRaw(const iv::can::canraw &xraw); //only for standard data frame, return can id
+
+signals:
+    void Enable_Ask(bool enableFlag,uint8_t sensorID);
+    void UltrasonicData_Ready(void);
+
+private slots:
+    void UltrasonicData_Ready_Slot(void);
+
+private:
+    CAN_Producer_Consumer *pBuffer;
+
+    uint32_t CAN_ID = 0x000u;
+    uint8_t CAN_DLC = 0;
+    uint8_t CAN_data[8] = {0};
+
+    bool decodeEnableFlag = false;
+    uint8_t decodeSensorID = 0;
+    QTime decodeTimer;
+
+    QVector<uint32_t> objDist;
+    QVector<bool> sensorStatus;
+
+    QVector<uint32_t> objDist_Send;
+    QVector<bool> sensorStatus_Send;
+    QMutex sonarSend_Lock;
+};
 
 #endif // CANRECV_CONSUMER_H

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_consumer.cpp

@@ -18,12 +18,6 @@ CANSend_Consumer::~CANSend_Consumer()
 
 void CANSend_Consumer::run()
 {
-    QTime xTime;
-    xTime.start();
-
-    int lastTime = xTime.elapsed();
-    uint64_t interval = 20; //interval time should more than 10 ms
-
     uint64_t CANPackageIndex = 0;
     uint8_t CANPackageChannel;
     iv::can::canmsg xmsg;
@@ -35,37 +29,29 @@ void CANSend_Consumer::run()
 
     while (!QThread::isInterruptionRequested())
     {
-        pBuffer->Consume_Element_To_CANMsg(xmsg); //this function is block mode
-
-        if(abs(xTime.elapsed() - lastTime)>=interval || xmsg.rawmsg_size() > 3500)
+        pBuffer->Consume_Element_To_CANMsg(xmsg);
+        if(xmsg.rawmsg_size()>0)
         {
-            //update timer
-            lastTime = xTime.elapsed();
-            if(xmsg.rawmsg_size()>0) //actrully, because the fucntion before this is block mode, this determine is allways true, just for safety
+            //pack
+            xmsg.set_index(CANPackageIndex);
+            xmsg.set_channel(CANPackageChannel);
+            xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
+            //send
+            int ndatasize = xmsg.ByteSize();
+            char * strser = new char[ndatasize];
+            std::shared_ptr<char> pstrser;
+            pstrser.reset(strser);
+            if(xmsg.SerializePartialToArray(strser,ndatasize))
             {
-                //pack
-                xmsg.set_index(CANPackageIndex);
-                xmsg.set_channel(CANPackageChannel);
-                xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
-                //send
-                int ndatasize = xmsg.ByteSize();
-                char * strser = new char[ndatasize];
-                std::shared_ptr<char> pstrser;
-                pstrser.reset(strser);
-                if(xmsg.SerializePartialToArray(strser,ndatasize))
-                {
-                    iv::modulecomm::ModuleSendMsg(shmCANSend.mpa,strser,ndatasize);
-                }
-                else
-                {
-                    std::cout<<"CANSend_Consumer serialize error."<<std::endl;
-                }
-                //clear and update index
-                xmsg.clear_rawmsg();
-                CANPackageIndex++;
+                iv::modulecomm::ModuleSendMsg(shmCANSend.mpa,strser,ndatasize);
             }
             else
-                std::cout << "rawmsg size is zero, no data need to be send." << std::endl;
+            {
+                std::cout<<"CANSend_Consumer serialize error."<<std::endl;
+            }
+            //clear and update index
+            xmsg.clear_rawmsg();
+            CANPackageIndex++;
         }
     }
 }

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_producer.cpp

@@ -41,26 +41,25 @@ iv::can::canraw CANSend_Producer::Trans_to_CANRaw(void)
 
 void CANSend_Producer::run()
 {
-    QTime xTime;
-    xTime.start();
-
-    int lastTime = xTime.elapsed();
-    uint64_t interval = 25; //interval time should more than 10 ms
-
     while (!QThread::isInterruptionRequested())
     {
-        if(abs(xTime.elapsed() - lastTime)>=interval)
+        if(askEnableFlag == true)
         {
-            //update timer
-            lastTime = xTime.elapsed();
+            askEnableFlag = false;
 
             this->Clear_CAN_PrivateTempVariable();
             CAN_ID = setupConfig.sonarCAN_ID;
             CAN_data[0] = 0xE8;
             CAN_data[1] = 0x02;
-            CAN_data[2] = 0x16;
             CAN_DLC = 3;
-            pBuffer->Produce_Element(this->Trans_to_CANRaw());
+            if(askSensorID < NUM_OF_SENSOR_MAX)
+            {
+                CAN_data[2] =  askSensorID * 8 + 0x16;
+                pBuffer->Produce_Element(this->Trans_to_CANRaw());
+                emit Enable_DecodeResult(true,askSensorID);
+            }
+            else
+                std::cout<<"askSensorID is error"<<std::endl;
         }
         else
         {
@@ -68,3 +67,9 @@ void CANSend_Producer::run()
         }
     }
 }
+
+void CANSend_Producer::Enable_Ask_Slot(bool enableFlag , uint8_t sensorID)
+{
+    askEnableFlag = enableFlag;
+    askSensorID = sensorID;
+}

src/driver/driver_ultrasonic_dauxi_KS136A/cansend_producer.h

@@ -11,6 +11,10 @@
 #include "can_producer_consumer.h"
 #include "decode_cfg.h"
 
+#ifndef NUM_OF_SENSOR_MAX
+#define NUM_OF_SENSOR_MAX 12
+#endif
+
 class CANSend_Producer : public QThread
 {
     Q_OBJECT
@@ -19,6 +23,9 @@ public:
     ~CANSend_Producer();
     void Clear_CAN_PrivateTempVariable(void);
 
+public slots:
+    void Enable_Ask_Slot(bool enableFlag,uint8_t sensorID);
+
 protected:
     void run();
     iv::can::canraw Trans_to_CANRaw(void);
@@ -31,6 +38,12 @@ private:
     uint8_t CAN_RTR = 0;
     uint8_t CAN_DLC = 0;
     uint8_t CAN_data[8] = {0};
+
+    bool askEnableFlag = true;
+    uint8_t askSensorID = 0;
+
+signals:
+    void Enable_DecodeResult(bool enableFlag,uint8_t sensorID);
 };
 
 #endif // CANSEND_PRODUCER_H

src/driver/driver_ultrasonic_dauxi_KS136A/decode_cfg.cpp

@@ -7,7 +7,7 @@ int DecodeSonarCfgFromXML(setupConfig_t &setupConfig , const std::string &xmlFil
     setupConfig.strMemCANRecv = xp.GetParam("canrecv_shm","canrecv0");
     setupConfig.strMemCANSend = xp.GetParam("cansend_shm","cansend0");
     setupConfig.strMemSonar = xp.GetParam("sonar_shm","sonar");
-    setupConfig.sonarCAN_ID = std::strtoul(xp.GetParam("sonarCAN_ID","0x301").c_str(),0,0);
+    setupConfig.sonarCAN_ID = std::strtoul(xp.GetParam("sonarCAN_ID","0x101").c_str(),0,0);
 
 //    std::cout<<"canrecv_shm : "<<setupConfig.strMemCANRecv<<std::endl;
 //    std::cout<<"cansend_shm : "<<setupConfig.strMemCANSend<<std::endl;

src/driver/driver_ultrasonic_dauxi_KS136A/main.cpp

@@ -12,6 +12,10 @@
 
 #include "canmsg.pb.h"
 
+#ifndef NUM_OF_SENSOR_MAX
+#define NUM_OF_SENSOR_MAX 12
+#endif
+
 #include "decode_cfg.h"
 #include "iv_msgunit.h"
 #include "can_producer_consumer.h"
@@ -35,7 +39,7 @@ CAN_Producer_Consumer CANSend(4096);
 
 CANSend_Producer* can_send_producer;
 CANSend_Consumer* can_send_consumer;
-//CANRecv_Consumer* can_recv_consumer;
+CANRecv_Consumer* can_recv_consumer;
 
 void ListenCANMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
@@ -106,18 +110,23 @@ int main(int argc, char *argv[])
     shmCANRecv.mnBufferCount = 3;
     shmCANRecv.mpa = iv::modulecomm::RegisterRecv(shmCANRecv.mstrmsgname,ListenCANMsg);
 
-    can_send_producer = new CANSend_Producer(&CANSend);
-    can_send_producer->start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
     can_send_consumer = new CANSend_Consumer(&CANSend);
     can_send_consumer->start();
-//    can_recv_consumer = new CANRecv_Consumer(&CANRecv);
-//    can_recv_consumer->start();
+    can_send_producer = new CANSend_Producer(&CANSend);
+    can_recv_consumer = new CANRecv_Consumer(&CANRecv);
+    qRegisterMetaType<uint8_t>("uint8_t");
+    QObject::connect(can_send_producer,&CANSend_Producer::Enable_DecodeResult,can_recv_consumer,&CANRecv_Consumer::Enable_DecodeResult_Slot);
+    QObject::connect(can_recv_consumer,&CANRecv_Consumer::Enable_Ask,can_send_producer,&CANSend_Producer::Enable_Ask_Slot);
+    can_send_producer->start();
+    can_recv_consumer->start();
 
     int rtn = a.exec();
 
     can_send_producer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
     can_send_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
-//    can_recv_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
+    can_recv_consumer->terminate(); //function "terminate" can not be used in other position. it is dangerous.
 
     if(gfault != nullptr)delete gfault;
     if(givlog != nullptr)delete givlog;

src/include/proto/ultrasonic.proto

@@ -19,7 +19,7 @@ package iv.ultrasonic;
 
 message ultrasonic
 {
-	optional uint32 sigObjDist_FLSide   = 1; //前左区域距离显示
+	optional uint32 sigObjDist_FLSide   = 1; //前左区域距离显示 mm
 	optional uint32 sigObjDist_FLCorner = 2; //前左角
 	optional uint32 sigObjDist_FLMiddle = 3; //前中左
 	optional uint32 sigObjDist_FRMiddle = 4; //前中右