solve region dead lock in decontruction.

update cloud, dispite any capture failure.
chutian computer crash waiting to be solved

velodyne_lidar/chassis_ceres_solver.cpp

@@ -192,6 +192,8 @@ Error_manager chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Pt
 {
     // std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
     // create_mat(m_model);
+    // cv::imshow("model", m_model);
+    // cv::waitKey(30);
 
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     // std::chrono::duration<double> time_used_mat = std::chrono::duration_cast<std::chrono::duration<double>>(t1 - t0);
@@ -235,9 +237,9 @@ Error_manager chassis_ceres_solver::solve_mat(pcl::PointCloud<pcl::PointXYZ>::Pt
     if(update_debug_img)
     {
         cost_func->generate_mat(m_projected_mat, cloud, variable);
-        // cv::namedWindow("win", cv::WINDOW_FREERATIO);
-        // cv::imshow("win", m_projected_mat);
-        // cv::waitKey(30);
+        cv::namedWindow("win", cv::WINDOW_FREERATIO);
+        cv::imshow("win", m_projected_mat);
+        cv::waitKey(30);
     }
 
 

velodyne_lidar/ground_region.cpp

@@ -282,7 +282,9 @@ Ground_region::Ground_region()
 // deconstructor
 Ground_region::~Ground_region()
 {
-    if(m_measure_thread){
+    // LOG(WARNING) << "start deconstruct ground region";
+    if (m_measure_thread)
+    {
         m_measure_condition.kill_all();
         // Close Capturte Thread
         if (m_measure_thread->joinable())
@@ -292,6 +294,14 @@ Ground_region::~Ground_region()
             m_measure_thread = nullptr;
         }
     }
+    // LOG(WARNING) << "thread released";
+    // 将创建的检测器析构
+    if(m_detector)
+    {
+        delete m_detector;
+        m_detector = nullptr;
+    }
+    // LOG(WARNING) << "detector released";
 }
 
 Error_manager Ground_region::init(velodyne::Region region, pcl::PointCloud<pcl::PointXYZ>::Ptr left_model, pcl::PointCloud<pcl::PointXYZ>::Ptr right_model)
@@ -327,11 +337,11 @@ bool computer_var(std::vector<double> data, double &var)
 
 Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, detect_wheel_ceres3d::Detect_result &last_result)
 {
-    // 1.点云合法性检验
+    // 1.*********点云合法性检验
     if (cloud->size() == 0)
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "no point");
 
-    // 2.点云预处理
+    // 2.*********点云预处理
     std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
     for (int i = 0; i < cloud->size(); ++i)
@@ -342,7 +352,8 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             cloud_filtered->push_back(pt);
         }
     }
-        //离群点过滤
+
+    //离群点过滤
     pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
     sor.setInputCloud(cloud_filtered);
     sor.setMeanK(15);            //K近邻搜索点个数
@@ -353,22 +364,22 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     if (cloud_filtered->size() == 0)
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");
 
-        //下采样
+    //下采样
     pcl::ApproximateVoxelGrid<pcl::PointXYZ> vox;   //创建滤波对象
     vox.setInputCloud(cloud_filtered);   //设置需要过滤的点云给滤波对象
-    vox.setLeafSize(0.02f, 0.02f, 0.02f); //设置滤波时创建的体素体积为1cm的立方体
+    vox.setLeafSize(0.01f, 0.01f, 0.01f); //设置滤波时创建的体素体积为1cm的立方体
     vox.filter(*cloud_filtered);         //执行滤波处理，存储输出
 
     if (cloud_filtered->size() == 0)
         return Error_manager(VELODYNE_REGION_EMPTY_CLOUD, NORMAL, "filtered no point");
 
-        // 更新过滤点
+    // 更新过滤点
     m_filtered_cloud_mutex.lock();
     mp_cloud_filtered->clear();
     mp_cloud_filtered->operator+=(*cloud_filtered);
     m_filtered_cloud_mutex.unlock();
 
-    // 3.位姿优化，获取中心xy与角度
+    // 3.*********位姿优化，获取中心xy与角度
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_detect_z(new pcl::PointCloud<pcl::PointXYZ>);
     double x, y, theta, width, z_value=0.2;
     if(!Car_pose_detector::get_instance_references().detect_pose(cloud_filtered, cloud_detect_z, x, y, theta, width, z_value, false))
@@ -380,7 +391,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
     std::chrono::duration<double> time_used_bowl = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
 
-    // ***************** 测试xoz优化底盘检测算法 *****************
+    // 4.*********xoz优化获取底盘高度
     chassis_ceres_solver t_solver;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_z_solver(new pcl::PointCloud<pcl::PointXYZ>);
     for (int i = 0; i < cloud->size(); ++i)
@@ -391,7 +402,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
             cloud_z_solver->push_back(pt);
         }
     }
-    double mid_z = 0.05, height = 0.08;
+    double mid_z = 0.06, height = 0.08;
     Car_pose_detector::get_instance_references().inv_trans_cloud(cloud_z_solver, x, y, theta);
     // //下采样
     // vox.setInputCloud(cloud_z_solver);   //设置需要过滤的点云给滤波对象
@@ -438,7 +449,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         return Error_manager(VELODYNE_REGION_CERES_SOLVE_ERROR, NEGLIGIBLE_ERROR, (std::string("failed to find chassis z value: ")+std::to_string(chassis_z)).c_str());
     }
     bool ret = false;
-    while(chassis_z > mid_z)
+    while(chassis_z > (mid_z-0.02))
     {
         ret = classify_ceres_detect(cloud_filtered, chassis_z, result);
         // changed by yct, 暂时直接识别，调试用，之后将条件恢复
@@ -485,14 +496,20 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
 
     if (results.size() == 0)
     {
-        // changed by yct, save 3d wheel detect result.
-        static int save_debug = 0;
-        if (save_debug++ == 2)
-            m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
+        // 20201010, may lead to problem in chutian, uncomment in debug only
+        // // changed by yct, save 3d wheel detect result. 
+        // static int save_debug = 0;
+        // if (save_debug++ == 2)
+        //     m_detector->save_debug_data("/home/youchen/extra_space/chutian/measure/chutian_velo_ws/log/debug");
 
         // std::cout << "\n-------- no result: " << std::endl;
+        LOG(INFO) << "failed with midz, currz: " << mid_z << ", " << chassis_z;
         return Error_manager(FAILED, NORMAL, "no car detected");
     }
+    else
+    {
+        LOG(INFO) << "detected with suitable z value: " << chassis_z;
+    }
     ///  to be
     float min_mean_loss = 1.0;
     for (int i = 0; i < results.size(); ++i)
@@ -616,11 +633,13 @@ Error_manager Ground_region::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
     //    cloud->points[i].y = t_point.y();
     //    cloud->points[i].z = t_point.z();
     //}
-
-    std::lock_guard<std::mutex> lck(m_cloud_collection_mutex);
-    mp_cloud_collection = cloud;
-    // LOG(WARNING) << "update region cloud size: " << mp_cloud_collection->size() << ",,, input size: " << cloud->size();
-    m_cloud_collection_time = std::chrono::system_clock::now();
+    // 限定锁区域，解决因条件变量在析构时无法获得内部mutex导致此互斥锁卡住，造成死锁无法结束程序的问题。
+    {
+        std::lock_guard<std::mutex> lck(m_cloud_collection_mutex);
+        mp_cloud_collection = cloud;
+        // LOG(WARNING) << "update region cloud size: " << mp_cloud_collection->size() << ",,, input size: " << cloud->size();
+        m_cloud_collection_time = std::chrono::system_clock::now();
+    }
     m_measure_condition.notify_one(false, true);
 
     // std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
@@ -631,6 +650,7 @@ Error_manager Ground_region::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
 
 void Ground_region::thread_measure_func()
 {
+    LOG(INFO) << " Ground_region::thread_measure_func() start " << this;
     while (m_measure_condition.is_alive())
     {
         m_measure_condition.wait();
@@ -640,7 +660,7 @@ void Ground_region::thread_measure_func()
             detect_wheel_ceres3d::Detect_result t_result;
 
             Error_manager ec = detect(mp_cloud_collection, t_result);
-            
+      
             std::lock_guard<std::mutex> lck(m_car_result_mutex);
             if (ec == SUCCESS)
             {
@@ -670,10 +690,11 @@ void Ground_region::thread_measure_func()
             else
             {
                 m_car_wheel_information.correctness = false;
-                // LOG(ERROR) << ec.to_string();
+                LOG(ERROR) << ec.to_string();
             }
             m_detect_update_time = std::chrono::system_clock::now();
         }
         m_region_status = E_READY;
     }
+    LOG(INFO) << " Ground_region::thread_measure_func() end " << this;
 }

velodyne_lidar/velodyne_manager.cpp

@@ -2,7 +2,7 @@
  * @Description: 
  * @Author: yct
  * @Date: 2021-07-23 16:39:04
- * @LastEditTime: 2021-09-26 11:49:46
+ * @LastEditTime: 2021-10-10 21:46:59
  * @LastEditors: yct
  */
 #include "velodyne_manager.h"
@@ -421,20 +421,22 @@ void Velodyne_manager::collect_cloud_thread_fun()
 						}
 					}
 				}
-				if ( t_result == SUCCESS)
-				{
-					// m_cloud_update_time = std::chrono::system_clock::now();
-					//成功则唤醒预测算法
-					update_region_cloud();
-				}
-				else
-				{
-					// mp_cloud_collection->clear();
-					// LOG(WARNING) << t_result.to_string();
-					// // 未连接雷达时，可读入点云用于测试
-					// read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/comb_calib.txt", mp_cloud_collection);
-					// update_region_cloud();
-				}
+				// 20211010 changed by yct,无论是否存在获取失败的情况，都更新点云到区域，保证其他区域功能正常
+				update_region_cloud();
+				// if ( t_result == SUCCESS)
+				// {
+				// 	// m_cloud_update_time = std::chrono::system_clock::now();
+				// 	//成功则唤醒预测算法
+				// 	update_region_cloud();
+				// }
+				// else
+				// {
+				// 	// mp_cloud_collection->clear();
+				// 	// LOG(WARNING) << t_result.to_string();
+				// 	// // 未连接雷达时，可读入点云用于测试
+				// 	// read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/comb_calib.txt", mp_cloud_collection);
+				// 	// update_region_cloud();
+				// }
 			}
 		}
         t_result.error_manager_clear_all();
@@ -471,13 +473,13 @@ Error_manager Velodyne_manager::update_region_cloud()
 				}
 				else
 				{
-					LOG(WARNING) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中未获得点云。";
+					LOG_EVERY_N(WARNING, 100) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中未获得点云。";
 					continue;
 				}
 			}
 			else
 			{
-				LOG(WARNING) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中未初始化完全。";
+				LOG_EVERY_N(WARNING, 100) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中未初始化完全。";
 				continue;
 			}
 		}