增加测量状态消息发送

build/sys.prototxt

@@ -1,9 +1,23 @@
 
+id:0
+
+communication_cfg
+{
+	ip:"192.168.2.254"
+	port:30005
+}
 
 lidar
 {
 	ip:"192.168.2.201"
 	port:2367
+	calib
+	{
+		y:-3.0
+		cx:3.36
+		cy:-0.02
+	}
+	
 }
 
 lidar
@@ -15,16 +29,10 @@ lidar
 box
 {
 	minx:0
-	maxx:50
-	miny:-100
-	maxy:100
-	minz:-1
-	maxz:100
+	maxx:3.1
+	miny:-2.75
+	maxy:3.08
+	minz:0.01
+	maxz:0.2
 }
-id:0
 
-communication_cfg
-{
-	ip:"192.168.2.179"
-	port:30005
-}

src/ground_region.cpp

@@ -9,6 +9,7 @@
 #include <pcl/segmentation/extract_clusters.h>
 #include <fcntl.h>
 #include "publisher.h"
+#include "point_tool.h"
 //欧式聚类*******************************************************
 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Ground_region::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr sor_cloud)
 {
@@ -330,13 +331,21 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr Ground_region::scans2cloud(const velodyne::F
     for(int i=0;i<frame.scans.size();++i)
     {
         pcl::PointXYZ pt;
+        float ath=frame.scans[i].azimuth*M_PI/180.0 ;
+        float vth=frame.scans[i].vertical*M_PI/180.0;
+        float distance=frame.scans[i].distance/100.0;
+        //std::cout<<frame.scans[i].distance<<std::endl;
+        pt.z=sin(vth)*distance;
+        pt.x=sin(ath)*cos(vth)*distance;
+        pt.y=cos(ath)*cos(vth)*distance;
+        cloud->push_back(pt);
 
     }
     // 变换
     Eigen::Matrix3d rotation_matrix3 ;
-    rotation_matrix3= Eigen::AngleAxisd(calib.r(), Eigen::Vector3d::UnitZ()) *
-            Eigen::AngleAxisd(calib.p(), Eigen::Vector3d::UnitY()) *
-            Eigen::AngleAxisd(calib.y(), Eigen::Vector3d::UnitX());
+    rotation_matrix3= Eigen::AngleAxisd(calib.r()*M_PI/180.0, Eigen::Vector3d::UnitX()) *
+            Eigen::AngleAxisd(calib.p()*M_PI/180.0, Eigen::Vector3d::UnitY()) *
+            Eigen::AngleAxisd(calib.y()*M_PI/180.0, Eigen::Vector3d::UnitZ());
 
     Eigen::Affine3d transform_2 = Eigen::Affine3d::Identity();
     transform_2.translation() << calib.cx(),calib.cy(),calib.cz();
@@ -355,6 +364,7 @@ Error_manager Ground_region::sync_capture(std::vector<pcl::PointCloud<pcl::Point
     std::vector<velodyne::Frame>  sync_frames;
     while((false==m_exit_condition.wait_for_ex(std::chrono::microseconds(1))) && sync==false)
     {
+        sync_frames.clear();
         bool sync_once=true;
         for(int i=0;i<m_lidars.size();++i)
         {
@@ -395,7 +405,7 @@ Error_manager Ground_region::prehandle_cloud(std::vector<pcl::PointCloud<pcl::Po
                                              message::Ground_measure_statu_msg& msg)
 {
     if(msg.laser_statu_vector_size()!=m_lidars.size())
-        return ERROR;
+        return Error_manager(ERROR,NORMAL,"lsaer statu size != cloud size");
     //直通滤波
     ground_region::Box box = m_configure.box();
     std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> filtered_cloud_vector;
@@ -516,16 +526,14 @@ void Ground_region::thread_measure_func(Ground_region* p)
     base_info.set_receiver(message::eEmpty);
     msg.mutable_base_info()->CopyFrom(base_info);
     msg.set_ground_statu(message::Nothing);
+    for(int i=0;i<p->m_lidars.size();++i)
+    {
+        msg.add_laser_statu_vector(message::LASER_DISCONNECT);
+    }
     const float fps=10.;
     //保持 10 fps
     while(false==p->m_exit_condition.wait_for_millisecond(int(1000./fps)))
     {
-        for(int i=0;i<p->m_lidars.size();++i)
-        {
-            msg.add_laser_statu_vector(
-                    p->m_lidars[i]->isRun()?message::LASER_READY:message::LASER_DISCONNECT);
-        }
-
         std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
         Error_manager code=p->sync_capture(clouds,0.5);
         if(code!=SUCCESS)
@@ -534,11 +542,27 @@ void Ground_region::thread_measure_func(Ground_region* p)
             continue;
         }
 
+
+        static int save=0;
+        save++;
+        if(save==10)
+        {
+            for(int i=0;i<clouds.size();++i)
+            {
+                std::cout<<"-------------------save  size:"<<clouds[i]->size()<<std::endl;
+                char file[255]={0};
+                sprintf(file,"./%d.txt",i);
+                save_cloud_txt(clouds[i],file);
+            }
+            printf("  saved \n");
+        }
+
+
         Tick tick;
         code=p->prehandle_cloud(clouds,msg);
         if(code!=SUCCESS)
         {
-            LOG(WARNING)<<code.get_error_description();
+            LOG(WARNING)<<code.get_error_description()<< msg.laser_statu_vector_size()<<", "<<clouds.size();
             continue;
         }
         detect_wheel_ceres::Detect_result result;
@@ -572,10 +596,13 @@ void Ground_region::thread_measure_func(Ground_region* p)
         locate_information.set_locate_front_theta(result.front_theta);
         locate_information.set_locate_correct(true);
 
+        msg.mutable_locate_information_realtime()->CopyFrom(locate_information);
+
         Communication_message c_msg;
         c_msg.reset(msg.base_info(),msg.SerializeAsString());
         Publisher::get_instance_pointer()->publish_msg(&c_msg);
 
+        LOG(INFO)<<locate_information.DebugString()<<std::endl;
 
     }
 }

src/tool/point_tool.cpp

@@ -0,0 +1,288 @@
+//
+// 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);
+}
+
+//欧式聚类*******************************************************
+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;
+
+}
+

src/tool/point_tool.h

@@ -0,0 +1,30 @@
+//
+// 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);
+
+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();
+#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_

src/tool/proto_tool.h

@@ -9,7 +9,7 @@
 #include "../tool/singleton.h"
 #include <istream>
 #include <google/protobuf/message.h>
-
+#include "point_tool.h"
 class proto_tool:public Singleton<proto_tool>
 {
 	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。

src/vlp16.hpp

@@ -358,7 +358,8 @@ class VelodyneCapture
 
                 // Complete Retrieve Capture One Rotation Data
 #ifndef PUSH_SINGLE_PACKETS
-                if( last_azimuth > azimuth ){
+                if( last_azimuth > azimuth )
+                {
                     // Push One Rotation Data to Queue
                     mutex.lock();
                     m_frame.scans=lasers;
@@ -435,6 +436,7 @@ class VelodyneCapture
                     // Sensor Type 0x21 is HDL-32E, 0x22 is VLP-16
                     const DataPacket* packet = reinterpret_cast<const DataPacket*>( data );
                     parseDataPacket(  packet, lasers, last_azimuth );
+                    //std::cout<<" lasers size :"<<m_frame.scans.size()<<std::endl;
 
                 }
                 run = false;