采集区域点云，已测试区域功能(带滤波器)

CMakeLists.txt

@@ -3,6 +3,7 @@ project(measure_wj_proj)
 cmake_minimum_required(VERSION 3.5)
 
 set (CMAKE_CXX_STANDARD 11)
+set(CMAKE_BUILD_TYPE "Release")
 
 #set(PCL_DIR "/home/youchen/pcl-1.8/share/pcl-1.8")
 #nanomsg
@@ -92,6 +93,10 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY_SRC )
 #         -lpthread
 #         )
 
+aux_source_directory(./velodyne_lidar/match3d/common VELODYNE_LIDAR_COMMON)
+aux_source_directory(./velodyne_lidar/match3d/ VELODYNE_LIDAR_MATCH)
+
+
 # vlp16 driver test
 add_executable(vlp16 tests/vlp16_driver_test.cpp 
 velodyne_lidar/velodyne_driver/input.cc 
@@ -104,6 +109,9 @@ ${ERROR_SRC}
 ${TOOL_SRC}
 ${TASK_MANAGER_SRC}
 ${WANJI_LIDAR_SRC}
+velodyne_lidar/ground_region.cpp
+${VELODYNE_LIDAR_COMMON}
+${VELODYNE_LIDAR_MATCH}
 )
 target_link_libraries(vlp16 
 ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ${YAML_CPP_LIBRARIES} ${OpenCV_LIBS}

tests/vlp16_driver_test.cpp

@@ -2,7 +2,7 @@
  * @Description: vlp16驱动测试
  * @Author: yct
  * @Date: 2021-07-26 14:11:26
- * @LastEditTime: 2021-07-28 15:42:50
+ * @LastEditTime: 2021-07-29 18:46:48
  * @LastEditors: yct
  */
 
@@ -105,12 +105,94 @@ void device_test()
     }else{
         std::cout << "get cloud failed" << std::endl;
     }
+    t_device.uninit();
+}
+
+#include "../velodyne_lidar/ground_region.h"
+#include "../wanji_lidar/measure_filter.h"
+#include "../tool/point_tool.h"
+void region_detect()
+{
+    std::ifstream in("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/comb_calib.txt");
+    if(!in.is_open())
+    {
+        std::cout << "cannot open file to read cloud." << std::endl;
+        return;
+    }
+    else
+    {
+        // 准备点云
+        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+        while(!in.eof())
+        {
+            std::string t_linestr;
+            if (getline(in, t_linestr))
+            {
+                std::vector<std::string> str_vector;
+                std::stringstream ss(t_linestr);
+                std::string num_str;
+                while (getline(ss, num_str, ' '))
+                {
+                    str_vector.push_back(num_str);
+                }
+                if (str_vector.size() != 3)
+                {
+                    std::cout << "unsupported point cloud / cannot find point x y z value " << std::endl;
+                    return;
+                }
+                pcl::PointXYZ t_point;
+                t_point.x = stod(str_vector[0]);
+                t_point.y = stod(str_vector[1]);
+                t_point.z = stod(str_vector[2]);
+                t_cloud->push_back(t_point);
+            }
+        }
+        // 初始化region
+        velodyne::Region param;
+        param.set_minx(-3.10);
+        param.set_maxx(0);
+        param.set_miny(-2.75);
+        param.set_maxy(3.08);
+        param.set_minz(0.01);
+        param.set_maxz(0.2);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr left = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::PointCloud<pcl::PointXYZ>::Ptr right = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
+        if (!read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/left_model.txt", left) || !read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/right_model.txt", right))
+        {
+            std::cout << "cannot read left/right model " << std::endl;
+            return;
+        }
+        Ground_region t_region;
+        std::cout << "before init" << std::endl;
+        t_region.init(0, param, left, right);
+        std::cout << "after init" << std::endl;
+        // 测试region功能
+        detect_wheel_ceres3d::Detect_result t_result;
+        Error_manager ec = t_region.detect(t_cloud, t_result);
+        std::cout << ec.to_string() << std::endl;
+        std::cout << "---------------" << std::endl;
+        for (size_t i = 0; i < 12; i++)
+        {
+            t_region.update_cloud(t_cloud);
+            usleep(1000*100);
+        }
+
+        Common_data::Car_wheel_information t_wheel_info;
+
+        ec = Measure_filter::get_instance_references().get_filtered_wheel_information(0, t_wheel_info);
+        if (ec == SUCCESS)
+        {
+            std::cout << t_wheel_info.to_string() << std::endl;
+        }
+        else { std::cout << ec.to_string() << std::endl; }
+    }
 }
 
 int main()
 {
     // velo_driver_test();
-    device_test();
+    // device_test();
+    region_detect();
 
     return 0;
 }

tool/common_data.h

@@ -7,6 +7,8 @@
 #include <chrono>
 #include <cmath>
 #include <string>
+#include <string.h>
+
 class Common_data
 {
 public:
@@ -82,7 +84,7 @@ public:
 		// wx=1 wy=1 wa=0.5 wb=1 ww=0.5 wft=0.5
 		float calc_score()
 		{
-			float weights[] = {1.0f, 1.0f, 0.5f, 1.0f, 0.5f, 0.5f};
+			float weights[] = {1.0f, 1.0f, 0.5f, 0.1f, 0.05f, 0.05f};
 			float final_score = 0.0f;
 			final_score += fabs(weights[0] * this->center_x);
 			final_score += fabs(weights[1] * this->center_y);
@@ -106,7 +108,7 @@ public:
 	struct Car_wheel_information_stamped
 	{
 		Car_wheel_information wheel_data;
-		std::chrono::system_clock::time_point measure_time;
+		std::chrono::steady_clock::time_point measure_time;
 				public:
 		Car_wheel_information_stamped& operator+=(const Car_wheel_information_stamped& other)
 		{

tool/point2D_tool.h

@@ -2,8 +2,8 @@
 // Created by huli on 2020/9/1.
 //
 
-#ifndef NNXX_TESTS_POINT2D_H
-#define NNXX_TESTS_POINT2D_H
+#ifndef POINT2D_TOOL_H
+#define POINT2D_TOOL_H
 
 #include <pcl/point_types.h>
 #include <pcl/PCLPointCloud2.h>
@@ -78,4 +78,4 @@ public:
 };
 
 
-#endif //NNXX_TESTS_POINT2D_H
+#endif //POINT2D_TOOL_H

tool/point_tool.cpp

@@ -0,0 +1,337 @@
+//
+// Created by zx on 2021/5/11.
+//
+
+#include "point_tool.h"
+
+void rotation_matrix_to_rpy()
+{
+    //2#  92.0084   112.664  179.263
+    //3#  85.2597  73.8993  -1.70915
+    //4#  91.93   112.146   -179.65
+    //5#  32.9435  67.8446  -52.9897
+    //6#  124.098  119.42  -150.147
+    //8#  37.3443  111.887  130.948
+
+    /*
+    mat_r00:0.0229183
+    mat_r01:-0.9985044
+    mat_r02:0.0496393
+    mat_r03:5287.829
+    mat_r10:0.2763783
+    mat_r11:0.0540452
+    mat_r12:0.9595283
+    mat_r13:-2399.535
+    mat_r20:-0.9607757
+    mat_r21:-0.0082715
+    mat_r22:0.2772034
+    mat_r23:6674.29500*/
+    Eigen::Matrix3d mat;
+    mat<<0.0229183,
+            -0.9985044,
+    0.0496393,
+            0.2763783,
+    0.0540452,
+    0.9595283,
+            -0.9607757,
+                    -0.0082715,
+    0.2772034;
+    Eigen::Vector3d euler_angles = mat.eulerAngles(2, 1, 0);
+    std::cout<<euler_angles*180.0/M_PI<<std::endl;
+}
+
+void rpy_to_rotation_matrix()
+{
+    //
+    float ea[3]={-172,105.42,99.38};
+    Eigen::Matrix3d rotation_matrix3;
+    rotation_matrix3 = Eigen::AngleAxisd(ea[2]*M_PI/180.0, Eigen::Vector3d::UnitZ()) *
+            Eigen::AngleAxisd(ea[1]*M_PI/180.0, Eigen::Vector3d::UnitY()) *
+            Eigen::AngleAxisd(ea[0]*M_PI/180.0, Eigen::Vector3d::UnitX());
+    std::cout << "rotation matrix3 =\n" << rotation_matrix3 << std::endl;
+}
+
+void reverse_test(){
+    Eigen::Matrix3d mat;
+    mat<<-0.2963634,0.9547403,-0.0252988,-0.2261306,-0.0958803,-0.9693665,-0.9279189,-0.2815638,0.2443113;
+    Eigen::Vector3d euler_angles = mat.eulerAngles(2, 1, 0);
+    std::cout<<euler_angles*180.0/M_PI<<std::endl;
+    Eigen::Matrix3d rotation_matrix3;
+    rotation_matrix3 = Eigen::AngleAxisd(euler_angles[0], Eigen::Vector3d::UnitZ()) *
+            Eigen::AngleAxisd(euler_angles[1], Eigen::Vector3d::UnitY()) *
+            Eigen::AngleAxisd(euler_angles[2], Eigen::Vector3d::UnitX());
+    std::cout << "origin matrix3 =\n" << mat << std::endl;
+    std::cout << "rotation matrix3 =\n" << rotation_matrix3 << std::endl;
+}
+
+bool string2point(std::string str,pcl::PointXYZ& point)
+{
+    std::istringstream iss;
+    iss.str(str);
+    float value;
+    float data[3]={0};
+    for(int i=0;i<3;++i)
+    {
+        if(iss>>value)
+        {
+            data[i]=value;
+        }
+        else
+        {
+            return false;
+        }
+    }
+    point.x=data[0];
+    point.y=data[1];
+    point.z=data[2];
+    return true;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
+{
+    std::ifstream fin(file.c_str());
+    const int line_length=64;
+    char str[line_length]={0};
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    while(fin.getline(str,line_length))
+    {
+        pcl::PointXYZ point;
+        if(string2point(str,point))
+        {
+            cloud->push_back(point);
+        }
+    }
+    return cloud;
+}
+
+
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string file)
+{
+    FILE* pf=fopen(file.c_str(),"w");
+    for(int i=0;i<cloud->size();++i)
+    {
+        pcl::PointXYZ point=cloud->points[i];
+        fprintf(pf,"%.3f %.3f %.3f\n",point.x,point.y,point.z);
+    }
+    fclose(pf);
+}
+
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,std::string file)
+{
+    FILE* pf=fopen(file.c_str(),"w");
+    for(int i=0;i<cloud->size();++i)
+    {
+        pcl::PointXYZRGB point=cloud->points[i];
+        fprintf(pf,"%.3f %.3f %.3f %d %d %d\n",point.x,point.y,point.z,point.r,point.g,point.b);
+    }
+    fclose(pf);
+}
+
+//欧式聚类*******************************************************
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
+{
+    std::vector<pcl::PointIndices> ece_inlier;
+    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
+    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ece;
+    ece.setInputCloud(sor_cloud);
+    ece.setClusterTolerance(0.2);
+    ece.setMinClusterSize(20);
+    ece.setMaxClusterSize(20000);
+    ece.setSearchMethod(tree);
+    ece.extract(ece_inlier);
+
+    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>  segmentation_clouds;
+    for (int i = 0; i < ece_inlier.size(); i++)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_copy(new pcl::PointCloud<pcl::PointXYZ>);
+        std::vector<int> ece_inlier_ext = ece_inlier[i].indices;
+        copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy);//按照索引提取点云数据
+        segmentation_clouds.push_back(cloud_copy);
+    }
+    return segmentation_clouds;
+}
+
+double distance(cv::Point2f p1, cv::Point2f p2)
+{
+    return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
+}
+
+
+bool isRect(std::vector<cv::Point2f>& points)
+{
+    if (points.size() == 4)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        cv::Point2f ps = points[0], pt = points[1];
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l)
+            {
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+
+        //直角边与坐标轴的夹角 <20°
+        float thresh=20.0*M_PI/180.0;
+        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
+        float angle=atan2(vct.y,vct.x);
+        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
+        {
+            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            /*char description[255]={0};
+            sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+        float width=std::min(l1,l2);
+        float length=std::max(l1,l2);
+        if(width<1.400 || width >1.900 || length >3.300 ||length < 2.200)
+        {
+            /*char description[255]={0};
+            sprintf(description,"width<1400 || width >1900 || length >3300 ||length < 2200 l:%.1f,w:%.1f",length,width);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+        double d = distance(pc, points[3]);
+        cv::Point2f center1 = (ps + pt) * 0.5;
+        cv::Point2f center2 = (pc + points[3]) * 0.5;
+        if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150) {
+            /*std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
+            char description[255]={0};
+            sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 0.150 ");
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+        //std::cout << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2<<std::endl;
+
+
+        return true;
+    }
+    else if(points.size()==3)
+    {
+        double L[3] = {0.0};
+        L[0] = distance(points[0], points[1]);
+        L[1] = distance(points[1], points[2]);
+        L[2] = distance(points[0], points[2]);
+        double max_l = L[0];
+        double l1 = L[1];
+        double l2 = L[2];
+        int max_index = 0;
+        cv::Point2f ps = points[0], pt = points[1];
+        cv::Point2f pc = points[2];
+        for (int i = 1; i < 3; ++i) {
+            if (L[i] > max_l) {
+                max_index = i;
+                max_l = L[i];
+                l1 = L[abs(i + 1) % 3];
+                l2 = L[abs(i + 2) % 3];
+                ps = points[i % 3];
+                pt = points[(i + 1) % 3];
+                pc = points[(i + 2) % 3];
+            }
+        }
+
+        //直角边与坐标轴的夹角 <20°
+        float thresh=20.0*M_PI/180.0;
+        cv::Point2f vct(pt.x-pc.x,pt.y-pc.y);
+        float angle=atan2(vct.y,vct.x);
+        if(!(fabs(angle)<thresh || (M_PI_2-fabs(angle)<thresh)))
+        {
+            //std::cout<<" 4 wheel axis angle : "<<angle<<std::endl;
+            return false;
+        }
+
+        double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+        if (fabs(cosa) >= 0.15) {
+            /*char description[255]={0};
+            sprintf(description,"3 wheels angle cos value(%.2f) >0.13 ",cosa);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+        double l=std::max(l1,l2);
+        double w=std::min(l1,l2);
+        if(l>2.100 && l<3.300 && w>1.400 && w<2.100)
+        {
+            //生成第四个点
+            cv::Point2f vec1=ps-pc;
+            cv::Point2f vec2=pt-pc;
+            cv::Point2f point4=(vec1+vec2)+pc;
+            points.push_back(point4);
+
+            /*char description[255]={0};
+            sprintf(description,"3 wheels rectangle cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
+            std::cout<<description<<std::endl;*/
+            return true;
+        }
+        else
+        {
+
+            /*char description[255]={0};
+            sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
+            std::cout<<description<<std::endl;*/
+            return false;
+        }
+
+    }
+    //std::cout<<" default false"<<std::endl;
+    return false;
+
+}
+
+bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
+{
+    // std::lock_guard<std::mutex> lck(m_cloud_mutex);
+    pointcloud->clear();
+    std::ifstream in(filename);
+    if (!in.is_open())
+    {
+        std::cout << "failed to open file " << filename << std::endl;
+        return false;
+    }
+    while (!in.eof())
+    {
+        std::string t_linestr;
+        if (getline(in, t_linestr))
+        {
+            std::vector<std::string> str_vector;
+            std::stringstream ss(t_linestr);
+            std::string num_str;
+            while (getline(ss, num_str, ' '))
+            {
+                str_vector.push_back(num_str);
+            }
+            if (str_vector.size() != 3)
+            {
+                std::cout << "unsupported point cloud / cannot find point x y z value: " << filename << std::endl;
+                return false;
+            }
+            pcl::PointXYZ t_cloud;
+            t_cloud.x = stod(str_vector[0]);
+            t_cloud.y = stod(str_vector[1]);
+            t_cloud.z = stod(str_vector[2]);
+            pointcloud->push_back(t_cloud);
+        }
+    }
+    in.close();
+    std::cout << "file read finished with points " << pointcloud->size() << std::endl;
+    return true;
+}

tool/point_tool.h

@@ -0,0 +1,33 @@
+//
+// Created by zx on 2021/5/11.
+//
+
+#ifndef ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_
+#define ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_
+#include <iostream>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <opencv2/opencv.hpp>
+
+bool string2point(std::string str, pcl::PointXYZ &point);
+pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file);
+
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::string file);
+void save_cloud_txt(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string file);
+
+double distance(cv::Point2f p1, cv::Point2f p2);
+bool isRect(std::vector<cv::Point2f> &points);
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);
+
+void rpy_to_rotation_matrix();
+void rotation_matrix_to_rpy();
+void reverse_test();
+
+bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
+
+#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_

tool/proto_tool.h

@@ -3,8 +3,8 @@
 
 
 
-#ifndef __PROTO_TOOL_H
-#define __PROTO_TOOL_H
+#ifndef __POINT_TOOL_H
+#define __POINT_TOOL_H
 
 #include "../tool/singleton.h"
 #include <istream>
@@ -34,7 +34,7 @@ public:
 
 
 
-#endif //__PROTO_TOOL_H
+#endif //__POINT_TOOL_H
 
 
 

velodyne_lidar/ground_region.cpp

@@ -10,6 +10,9 @@
 #include <pcl/segmentation/extract_clusters.h>
 #include <fcntl.h>
 
+// 测量结果滤波,不影响现有结构
+#include "../wanji_lidar/measure_filter.h"
+
 //欧式聚类*******************************************************
 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
 {
@@ -271,6 +274,7 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
 // constructor
 Ground_region::Ground_region()
 {
+    m_region_status = E_UNKNOWN;
     m_detector = nullptr;
     m_measure_thread = nullptr;
 }
@@ -290,10 +294,15 @@ Ground_region::~Ground_region()
     }
 }
 
-Error_manager Ground_region::init(velodyne::Region region)
+Error_manager Ground_region::init(int terminal_id, velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model, pcl::PointCloud<pcl::PointXYZ>::Ptr right_model)
 {
+    m_terminal_id = terminal_id;
     m_region = region;
+    m_detector = new detect_wheel_ceres3d(left_model,right_model);
+    mp_cloud_collection = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
     m_measure_thread = new std::thread(&Ground_region::thread_measure_func, this);
+    m_measure_condition.reset();
+    m_region_status = E_READY;
     return SUCCESS;
 }
 
@@ -393,31 +402,45 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     return SUCCESS;
 }
 
-#include "message/message_base.pb.h"
+Error_manager Ground_region::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
+{
+    std::lock_guard<std::mutex> lck(m_cloud_collection_mutex);
+    mp_cloud_collection = cloud;
+    m_cloud_collection_time = std::chrono::steady_clock::now();
+    m_measure_condition.notify_one(false, true);
+    return SUCCESS;
+}
+
 void Ground_region::thread_measure_func()
 {
-    //保持 10 fps
     while (m_measure_condition.is_alive())
     {
         m_measure_condition.wait();
         if (m_measure_condition.is_alive())
         {
-            //  to be 发布结果
-            msg.set_ground_statu(message::Car_correct);
-            message::Locate_information locate_information;
-            locate_information.set_locate_x(result.cx);
-            locate_information.set_locate_y(result.cy);
-            locate_information.set_locate_angle(result.theta);
-            locate_information.set_locate_wheel_base(result.wheel_base);
-            locate_information.set_locate_length(result.wheel_base);
-            locate_information.set_locate_wheel_width(result.width);
-            locate_information.set_locate_width(result.body_width);
-            locate_information.set_locate_front_theta(result.front_theta);
-            locate_information.set_locate_correct(true);
-
-            Communication_message c_msg;
-            c_msg.reset(msg.base_info(), msg.SerializeAsString());
-            Publisher::get_instance_pointer()->publish_msg(&c_msg);
+            m_region_status = E_BUSY;
+            detect_wheel_ceres3d::Detect_result t_result;
+            Error_manager ec = detect(mp_cloud_collection, t_result);
+            m_detect_update_time = std::chrono::steady_clock::now();
+            m_car_wheel_information.center_x = t_result.cx;
+            m_car_wheel_information.center_y = t_result.cy;
+            m_car_wheel_information.car_angle = t_result.theta;
+            m_car_wheel_information.wheel_base = t_result.wheel_base;
+            m_car_wheel_information.wheel_width = t_result.width;
+            m_car_wheel_information.front_theta = t_result.front_theta;
+            if (ec == SUCCESS)
+            {
+                m_car_wheel_information.correctness = true;
+                Common_data::Car_wheel_information_stamped t_wheel_info_stamped;
+				t_wheel_info_stamped.wheel_data = m_car_wheel_information;
+				t_wheel_info_stamped.measure_time = m_detect_update_time;
+				Measure_filter::get_instance_references().update_data(m_terminal_id, t_wheel_info_stamped);
+            }
+            else
+            {
+                m_car_wheel_information.correctness = false;
+            }
         }
+        m_region_status = E_READY;
     }
 }

velodyne_lidar/ground_region.h

@@ -12,7 +12,7 @@
 #include "../error_code/error_code.h"
 #include "match3d/detect_wheel_ceres3d.h"
 #include "velodyne_config.pb.h"
-#include "message/measure_message.pb.h"
+#include "../message/measure_message.pb.h"
 
 #include <opencv2/opencv.hpp>
 #include <glog/logging.h>
@@ -23,20 +23,40 @@
 class Ground_region
 {
 public:
+
+	enum Ground_region_status
+	{
+		E_UNKNOWN = 0, //未知
+		E_READY = 1,   //准备，待机
+		E_BUSY = 2,	   //工作正忙
+
+		E_FAULT = 10, //故障
+	};
+
    Ground_region();
    ~Ground_region();
    // 区域类初始化
-   Error_manager init(velodyne::Region region);
+   Error_manager init(int terminal_id, velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model, pcl::PointCloud<pcl::PointXYZ>::Ptr right_model);
    // 公有检测函数,根据初始化设置的区域进行裁剪，并识别获得结果
    Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &result);
    // 公有点云更新函数，传入最新点云获得结果
    Error_manager update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
+   // 获取区域终端号, 注意！未初始化调用将返回-1
+   int get_terminal_id()
+   {
+      if(m_region_status == E_UNKNOWN)
+         return -1;
+      else
+         return m_terminal_id;
+   }
+   // 获取区域状态
+   Ground_region_status get_status() { return m_region_status; }
 
 private:
    // 点云分类，z切，3d优化
    bool classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double thresh_z,
                               detect_wheel_ceres3d::Detect_result &result);
-   // 自动检测线程函数
+   // 自动检测线程函数,频率由外部更新点云频率控制
    void thread_measure_func();
 
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud);
@@ -45,19 +65,23 @@ private:
 
    bool isRect(std::vector<cv::Point2f> &points);
 
-public:
-   velodyne::Region m_region;
-   detect_wheel_ceres3d *m_detector;
 
 private:
+   int m_terminal_id; // 当前区域对应id
+   velodyne::Region m_region; // 区域配置
+   detect_wheel_ceres3d *m_detector; // 检测器
+   Ground_region_status m_region_status; // 区域状态
+
    // 自动检测线程
    std::thread *m_measure_thread;
    Thread_condition m_measure_condition;
 
-   std::mutex*	 								mp_cloud_collection_mutex;       // 点云更新互斥锁, 内存由上级管理
+   std::mutex	 								      m_cloud_collection_mutex;       // 点云更新互斥锁, 内存由上级管理
 	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由上级管理
-	Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
-	std::chrono::system_clock::time_point		m_detect_updata_time;			//定位结果的更新时间.
+   std::chrono::steady_clock::time_point     m_cloud_collection_time; // 点云更新时间
+
+   Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
+	std::chrono::steady_clock::time_point		m_detect_update_time;			//定位结果的更新时间.
 };
 
 #endif //LIDARMEASURE__GROUD_REGION_HPP_

velodyne_lidar/match3d/detect_wheel_ceres3d.cpp

@@ -5,6 +5,7 @@
 #include "detect_wheel_ceres3d.h"
 #include "interpolated_grid.hpp"
 #include <pcl/common/transforms.h>
+#include "point_tool.h"
 
 class CostFunctor3d
 {
@@ -147,9 +148,9 @@ detect_wheel_ceres3d::detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr l
     float resolution=0.01;
     mp_left_grid=new HybridGrid(resolution);
     mp_right_grid=new HybridGrid(resolution);
-// std::cout << "2222" << std::endl;
     for(int i=0;i<left_grid_cloud->size();++i)
     {
+// std::cout << "2222 "<< i << std::endl;
         pcl::PointXYZ pt=left_grid_cloud->points[i];
         Eigen::Vector3f u(pt.x,pt.y,pt.z);
 
@@ -181,7 +182,7 @@ detect_wheel_ceres3d::detect_wheel_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr l
         }
 
     }
-std::cout << "3333" << std::endl;
+// std::cout << "3333" << std::endl;
     for(int i=0;i<right_grid_cloud->size();++i)
     {
         pcl::PointXYZ pt=right_grid_cloud->points[i];
@@ -207,12 +208,11 @@ std::cout << "3333" << std::endl;
         }
 
     }
-std::cout << "4444" << std::endl;
+// std::cout << "4444" << std::endl;
     // save_model("/home/zx/zzw/catkin_ws/src/feature_extra");
 
 }
 
-#include "point_tool.h"
 void detect_wheel_ceres3d::save_model(std::string dir)
 {
 
@@ -546,10 +546,10 @@ void detect_wheel_ceres3d::get_costs(double* variable,double &lf, double &rf, do
 
 void detect_wheel_ceres3d::save_debug_data(std::string dir )
 {
-    save_cloud_txt(m_left_front_transformed_cloud.makeShared(),dir+"/left_front_tranformed.txt");
-    save_cloud_txt(m_right_front_transformed_cloud.makeShared(),dir+"/right_front_tranformed.txt");
-    save_cloud_txt(m_left_rear_transformed_cloud.makeShared(),dir+"/left_rear_tranformed.txt");
-    save_cloud_txt(m_right_rear_transformed_cloud.makeShared(),dir+"/right_rear_tranformed.txt");
+    ::save_cloud_txt(m_left_front_transformed_cloud.makeShared(),dir+"/left_front_tranformed.txt");
+    ::save_cloud_txt(m_right_front_transformed_cloud.makeShared(),dir+"/right_front_tranformed.txt");
+    ::save_cloud_txt(m_left_rear_transformed_cloud.makeShared(),dir+"/left_rear_tranformed.txt");
+    ::save_cloud_txt(m_right_rear_transformed_cloud.makeShared(),dir+"/right_rear_tranformed.txt");
 }
 
 bool detect_wheel_ceres3d::Solve(Detect_result &detect_result, Loss_result &loss_result, std::string &error_info)

wanji_lidar/measure_filter.h

@@ -2,7 +2,7 @@
  * @Description: 测量结果滤波器
  * @Author: yct
  * @Date: 2021-03-15 14:41:46
- * @LastEditTime: 2021-03-16 16:30:08
+ * @LastEditTime: 2021-07-29 18:50:45
  * @LastEditors: yct
  */
 
@@ -50,7 +50,7 @@ public:
         { 
             m_measure_results_map[terminal_id].push_back(data);
             // 剔除超时数据
-            while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now()-m_measure_results_map[terminal_id].front().measure_time).count() > MAX_TIME_INTERVAL_MILLI)
+            while(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now()-m_measure_results_map[terminal_id].front().measure_time).count() > MAX_TIME_INTERVAL_MILLI)
             {
                 m_measure_results_map[terminal_id].pop_front();
             }
@@ -102,7 +102,7 @@ public:
         int t_result_count = 0;
         for (size_t i = 0; i < t_score_vec.size(); i++)
         {
-            if(t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-1] || t_score_vec[i] == t_sorted_score_vec[t_sorted_score_vec.size()-2])
+            if(t_score_vec[i] > t_sorted_score_vec[t_sorted_score_vec.size()-3])
             {
                 continue;
             }else{

wanji_lidar/region_detect.cpp

@@ -71,7 +71,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 /**
  * 返回二维点之间距离
  * */
-double distance(cv::Point2f p1, cv::Point2f p2)
+double Region_detector::distance(cv::Point2f p1, cv::Point2f p2)
 {
 	return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
 }

wanji_lidar/region_detect.h

@@ -77,6 +77,8 @@ protected:
     // 判断是否足够近似矩形
     Error_manager isRect(std::vector<cv::Point2f>& points, bool print=false);
 
+    double distance(cv::Point2f p1, cv::Point2f p2);
+
 protected:
 	Point2D_tool::Point2D_box		m_region_box;		// 区域范围参数
 

wanji_lidar/region_worker.cpp

@@ -14,7 +14,7 @@ Region_worker::Region_worker()
 	mp_cloud_collection_mutex = NULL;
 	mp_cloud_collection = NULL;
 
-	m_detect_updata_time = std::chrono::system_clock::now() - std::chrono::hours(1);
+	m_detect_updata_time = std::chrono::steady_clock::now() - std::chrono::hours(1);
 
 	mp_auto_detect_thread = NULL;
 
@@ -84,7 +84,7 @@ Error_manager Region_worker::detect_wheel_result_ex(std::mutex* p_cloud_mutex, p
 
 //外部调用获取新的车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会等待车轮定位信息刷新, 直到超时, (内置while)
 Error_manager Region_worker::get_new_wheel_information_and_wait(Common_data::Car_wheel_information* p_car_wheel_information,
-												 std::chrono::system_clock::time_point command_time, int timeout_ms)
+												 std::chrono::steady_clock::time_point command_time, int timeout_ms)
 {
 	if ( p_car_wheel_information == NULL )
 	{
@@ -92,7 +92,7 @@ Error_manager Region_worker::get_new_wheel_information_and_wait(Common_data::Car
 							 "  POINTER IS NULL ");
 	}
 	//判断是否超时
-	while (std::chrono::system_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
+	while (std::chrono::steady_clock::now() - command_time < std::chrono::milliseconds(timeout_ms))
 	{
 		//获取指令时间之后的信息, 如果没有就会循环
 		if(m_detect_updata_time > command_time  )
@@ -112,7 +112,7 @@ Error_manager Region_worker::get_new_wheel_information_and_wait(Common_data::Car
 
 //外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
 Error_manager Region_worker::get_current_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
-											std::chrono::system_clock::time_point command_time)
+											std::chrono::steady_clock::time_point command_time)
 {
 	if ( p_car_wheel_information == NULL )
 	{
@@ -133,7 +133,7 @@ Error_manager Region_worker::get_current_wheel_information(Common_data::Car_whee
 }
 //外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待
 Error_manager Region_worker::get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
-										 std::chrono::system_clock::time_point command_time)
+										 std::chrono::steady_clock::time_point command_time)
 {
 	if ( p_car_wheel_information == NULL )
 	{
@@ -159,7 +159,7 @@ Region_worker::Region_worker_status Region_worker::get_status()
 	return m_region_worker_status;
 }
 
-std::chrono::system_clock::time_point Region_worker::get_detect_updata_time()
+std::chrono::steady_clock::time_point Region_worker::get_detect_updata_time()
 {
 	return m_detect_updata_time;
 }
@@ -220,11 +220,11 @@ void Region_worker::auto_detect_thread_fun()
 //            if(m_index==5)
 //			    LOG(INFO) << " huli test :::: " << " mp_cloud_collection->size() = " << mp_cloud_collection->size();
 //
-//			auto start   = std::chrono::system_clock::now();
+//			auto start   = std::chrono::steady_clock::now();
 
 			t_error = detect_wheel_result_ex(mp_cloud_collection_mutex, mp_cloud_collection, m_car_wheel_information);
 
-//			auto end   = std::chrono::system_clock::now();
+//			auto end   = std::chrono::steady_clock::now();
 //			auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
 //			std::cout <<  "花费了"
 //				 << double(duration.count()*1000) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "毫秒" << std::endl;
@@ -255,7 +255,7 @@ void Region_worker::auto_detect_thread_fun()
 			}
 
 			//无论结果如何,都要刷新时间, 表示这次定位已经执行了.
-			m_detect_updata_time = std::chrono::system_clock::now();
+			m_detect_updata_time = std::chrono::steady_clock::now();
 //            if(m_index==5)
 //                LOG(INFO) << "before filter";
 			// 测量正确,更新到滤波器

wanji_lidar/region_worker.h

@@ -71,19 +71,19 @@ public:
 
 	//外部调用获取新的车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会等待车轮定位信息刷新, 直到超时, (内置while)
 	Error_manager get_new_wheel_information_and_wait(Common_data::Car_wheel_information* p_car_wheel_information,
-													 std::chrono::system_clock::time_point command_time, int timeout_ms=1500);
+													 std::chrono::steady_clock::time_point command_time, int timeout_ms=1500);
 
 	//外部调用获取当前车轮定位信息, 获取指令时间之后的车轮定位信息, 如果没有就会报错, 不会等待
 	Error_manager get_current_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
-												std::chrono::system_clock::time_point command_time);
+												std::chrono::steady_clock::time_point command_time);
 	//外部调用获取最新的车轮定位信息, 获取指令时间往前一个周期内的车轮定位信息, 如果没有就会报错, 不会等待
 	Error_manager get_last_wheel_information(Common_data::Car_wheel_information* p_car_wheel_information,
-											 std::chrono::system_clock::time_point command_time);
+											 std::chrono::steady_clock::time_point command_time);
 
 public:
 	Region_worker_status get_status();
 
-	std::chrono::system_clock::time_point get_detect_updata_time();
+	std::chrono::steady_clock::time_point get_detect_updata_time();
 
 protected:
 	static cv::RotatedRect create_rotate_rect(float length,float width,float angle,float x,float y);
@@ -104,7 +104,7 @@ protected:
 	std::mutex*	 								mp_cloud_collection_mutex;       // 点云更新互斥锁, 内存由上级管理
 	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由上级管理
 	Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
-	std::chrono::system_clock::time_point		m_detect_updata_time;			//定位结果的更新时间.
+	std::chrono::steady_clock::time_point		m_detect_updata_time;			//定位结果的更新时间.
 
 	std::thread*        						mp_auto_detect_thread;   		//自动预测的线程指针，内存由本类管理
 	Thread_condition							m_auto_detect_condition;		//自动预测的条件变量

wanji_lidar/wanji_manager.cpp

@@ -422,7 +422,7 @@ void Wanji_manager::execute_thread_fun()
 				else
 				{
 					//如果在指令时间1秒后都没有成功获取结果, 返回错误原因
-					if ( std::chrono::system_clock::now() - m_region_worker_map[mp_wanji_manager_task->get_terminal_id()]->get_detect_updata_time() < std::chrono::milliseconds(WANJI_MANAGER_EXECUTE_TIMEOUT_MS)  )
+					if ( std::chrono::steady_clock::now() - m_region_worker_map[mp_wanji_manager_task->get_terminal_id()]->get_detect_updata_time() < std::chrono::milliseconds(WANJI_MANAGER_EXECUTE_TIMEOUT_MS)  )
 					{
 						//没有超时, 那么就等1ms, 然后重新循环
 						std::this_thread::sleep_for(std::chrono::milliseconds(1));

wanji_lidar/wanji_manager_task.cpp

@@ -16,7 +16,7 @@ Wanji_manager_task::~Wanji_manager_task()
 }
 
 //初始化函数
-Error_manager Wanji_manager_task::task_init(int terminal_id, std::chrono::system_clock::time_point command_time	)
+Error_manager Wanji_manager_task::task_init(int terminal_id, std::chrono::steady_clock::time_point command_time	)
 {
 	m_task_statu = TASK_CREATED;
 	m_task_statu_information.clear();
@@ -32,7 +32,7 @@ Error_manager Wanji_manager_task::task_init(Task_statu task_statu,
 						void* p_tast_receiver,
 						std::chrono::milliseconds task_over_time,
 						int terminal_id,
-						std::chrono::system_clock::time_point command_time
+						std::chrono::steady_clock::time_point command_time
 )
 {
 	m_task_statu = task_statu;
@@ -51,7 +51,7 @@ int Wanji_manager_task::get_terminal_id()
 {
 	return m_terminal_id;
 }
-std::chrono::system_clock::time_point Wanji_manager_task::get_command_time()
+std::chrono::steady_clock::time_point Wanji_manager_task::get_command_time()
 {
 	return m_command_time;
 }

wanji_lidar/wanji_manager_task.h

@@ -19,17 +19,17 @@ public:
     // 析构
     ~Wanji_manager_task();
 	//初始化函数
-	Error_manager task_init(int terminal_id, std::chrono::system_clock::time_point command_time	);
+	Error_manager task_init(int terminal_id, std::chrono::steady_clock::time_point command_time	);
 	Error_manager task_init(Task_statu task_statu,
 							std::string task_statu_information,
 							void* p_tast_receiver,
 							std::chrono::milliseconds task_over_time,
 							int terminal_id,
-							std::chrono::system_clock::time_point command_time
+							std::chrono::steady_clock::time_point command_time
 	);
 public://get or set member variable
 	int get_terminal_id();
-	std::chrono::system_clock::time_point get_command_time();
+	std::chrono::steady_clock::time_point get_command_time();
 	Common_data::Car_wheel_information get_car_wheel_information();
 	void set_car_wheel_information(Common_data::Car_wheel_information car_wheel_information);
 
@@ -37,7 +37,7 @@ private:
 	//终端id
 	int 										m_terminal_id;
 	//下发指令的时间点. 从command_time提前一个扫描周期, 然后取最新的点
-	std::chrono::system_clock::time_point 		m_command_time;
+	std::chrono::steady_clock::time_point 		m_command_time;
     // 测量结果
     Common_data::Car_wheel_information			m_car_wheel_information;