20211019chutian

main.cpp

@@ -20,7 +20,7 @@
 #include "./velodyne_lidar/velodyne_manager.h"
 
 #define LIVOX_NUMBER	     2
-#define POINT_DEBUG
+// #define POINT_DEBUG
 
 #ifdef POINT_DEBUG
 std::shared_ptr<pcl::visualization::PCLVisualizer> gp_visualizer;

setting/velodyne_manager.prototxt

@@ -93,7 +93,7 @@ region
     border_minx:-1.3
     border_maxx:1.3
     plc_offsetx:1.913
-    plc_offsety:5.998
+    plc_offsety:-6.078
     lidar_exts
     {
         lidar_id:6
@@ -134,8 +134,8 @@ region
     turnplate_cy:0.0
     border_minx:-1.3
     border_maxx:1.3
-    plc_offsetx:-1.8983
-    plc_offsety:6.03489
+    plc_offsetx:-1.8783
+    plc_offsety:-6.11489
     # 4-->5: -3.81895 0.038776
     lidar_exts
     {
@@ -155,4 +155,4 @@ region
             cy:0.019388
         }
     }
-}
+}

velodyne_lidar/car_pose_detector.cpp

@@ -109,7 +109,7 @@ bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pt
     theta = t_vars[2];
     if(fabs(theta)>30*M_PI/180.0)
     {
-        std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
+        //std::cout << "wrong angle, detect failed" << theta*180.0/M_PI << std::endl;
         return false;
     }
 
@@ -186,4 +186,4 @@ bool Car_pose_detector::detect_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pt
     // std::cout << "estimated x,y,theta = " << x << ", " << y << ", " << theta*180.0/M_PI << std::endl;
     // std::cout << "----------------------------------" << std::endl;
     return true;
-}
+}

velodyne_lidar/ground_region.cpp

@@ -339,7 +339,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 {
     // 1.*********点云合法性检验*********
     if (cloud->size() == 0)
-        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "no point");
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "no input point");
 
     // 2.*********点云预处理*********
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
@@ -354,6 +354,10 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             cloud_cut->push_back(pt);
         }
     }
+    if(cloud_cut->size() <= 0)
+    {
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "cut no point");
+    }
 
     //离群点过滤
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
@@ -364,7 +368,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     sor.filter(*cloud_clean); //保存滤波结果到cloud_filter
 
     if (cloud_clean->size() == 0)
-        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "StatisticalOutlierRemoval no point");
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "StatisticalOutlierRemoval no point");
 
     //下采样
     pcl::VoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
@@ -373,7 +377,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
     // cloud_filtered = cloud_clean;
     if (cloud_filtered->size() == 0)
-        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "ApproximateVoxelGrid no point");
+        return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NEGLIGIBLE_ERROR, "ApproximateVoxelGrid no point");
 
     // 更新过滤点
     m_filtered_cloud_mutex.lock();
@@ -491,7 +495,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         //     m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
 
         // std::cout << "\n-------- no result: " << std::endl;
-        LOG_IF(INFO, m_region.region_id() == 4) << "failed with midz, currz: " << mid_z << ", " << chassis_z;
+        //LOG_IF(INFO, m_region.region_id() == 4) << "failed with midz, currz: " << mid_z << ", " << chassis_z;
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, "3d wheel detect failed.");
     }
     else
@@ -685,12 +689,12 @@ void Ground_region::thread_measure_func()
 				t_wheel_info_stamped.measure_time = m_detect_update_time;
 				Measure_filter::get_instance_references().update_data(m_region.region_id(), t_wheel_info_stamped);
 
-                // LOG_IF(INFO, m_region.region_id() == 4) << m_car_wheel_information.to_string();
+                 //LOG_IF(INFO, m_region.region_id() == 4) << m_car_wheel_information.to_string();
             }
             else
             {
                 m_car_wheel_information.correctness = false;
-                LOG_IF(ERROR, m_region.region_id() == 4) << ec.to_string();
+                //LOG_IF(ERROR, m_region.region_id() == 4) << ec.to_string();
             }
             m_detect_update_time = std::chrono::system_clock::now();
         }