cloud update debug. filtered cloud output

velodyne_lidar/ground_region.cpp

@@ -265,6 +265,7 @@ bool Ground_region::classify_ceres_detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
         }
         else
         {
+            LOG(WARNING) << error_str;
             return false;
         }
     }
@@ -299,6 +300,7 @@ Error_manager Ground_region::init(velodyne::Region region, pcl::PointCloud<pcl::
     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>);
+    mp_cloud_filtered = 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;
@@ -340,6 +342,11 @@ 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");
 
+    m_filtered_cloud_mutex.lock();
+    mp_cloud_filtered->clear();
+    mp_cloud_filtered->operator+=(*cloud_filtered);
+    m_filtered_cloud_mutex.unlock();
+
     float start_z = m_region.minz();
     float max_z = 0.2;
     float center_z = (start_z + max_z) / 2.0;
@@ -352,7 +359,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
     {
         detect_wheel_ceres3d::Detect_result result;
         bool ret = classify_ceres_detect(cloud_filtered, center_z, result);
-        // std::cout << "z: " << center_z <<", "<<start_z<<"," << max_z << std::endl;
+        // std::cout << "z: " << center_z <<", "<<start_z<<"," << max_z <<(ret?"clustered":"clustering failed")<< std::endl;
         if (ret)
         {
             results.push_back(result);
@@ -453,8 +460,20 @@ Error_manager Ground_region::get_last_wheel_information(Common_data::Car_wheel_i
 
 Error_manager Ground_region::update_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
 {
+    // // 点云z转90度，调试用
+    //Eigen::AngleAxisd rot_z = Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ());
+    //for (size_t i = 0; i < cloud->size(); i++)
+    //{
+    //    Eigen::Vector3d t_point(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
+    //    t_point = rot_z.toRotationMatrix() * t_point;
+    //    cloud->points[i].x = t_point.x();
+    //    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();
     m_measure_condition.notify_one(false, true);
     return SUCCESS;
@@ -490,6 +509,7 @@ void Ground_region::thread_measure_func()
             else
             {
                 m_car_wheel_information.correctness = false;
+                // LOG(ERROR) << ec.to_string();
             }
         }
         m_region_status = E_READY;

velodyne_lidar/ground_region.h

@@ -31,6 +31,7 @@ public:
       left_back = 2,
       right_front = 3,
       right_back = 4,
+      filtered = 5,
    };
    enum Ground_region_status
    {
@@ -67,6 +68,7 @@ public:
       {
       case Region_cloud_type::total:
          out_cloud->operator+=(*mp_cloud_collection);
+         // LOG(WARNING) << "region cloud size: " << mp_cloud_collection->size();
          break;
       case Region_cloud_type::left_front:
          out_cloud->operator+=(m_detector->m_left_front_cloud);
@@ -80,6 +82,9 @@ public:
       case Region_cloud_type::right_back:
          out_cloud->operator+=(m_detector->m_right_rear_cloud);
          break;
+      case Region_cloud_type::filtered:
+         out_cloud->operator+=(*mp_cloud_filtered);
+         break;
       }
    }
    // 获取区域终端号, 注意！未初始化调用将返回-1
@@ -134,6 +139,9 @@ private:
 	pcl::PointCloud<pcl::PointXYZ>::Ptr 		mp_cloud_collection;			//扫描点的点云集合, 内存由上级管理
    std::chrono::system_clock::time_point     m_cloud_collection_time; // 点云更新时间
 
+   std::mutex	 								      m_filtered_cloud_mutex;       // 点云更新互斥锁, 内存由上级管理
+   pcl::PointCloud<pcl::PointXYZ>::Ptr       mp_cloud_filtered;      // 区域切除后的点
+
    Common_data::Car_wheel_information			m_car_wheel_information;		//车轮的定位结果,
 	std::chrono::system_clock::time_point		m_detect_update_time;			//定位结果的更新时间.
 };

velodyne_lidar/velodyne_manager.cpp

@@ -2,7 +2,7 @@
  * @Description: 
  * @Author: yct
  * @Date: 2021-07-23 16:39:04
- * @LastEditTime: 2021-09-01 09:14:32
+ * @LastEditTime: 2021-09-03 18:05:56
  * @LastEditors: yct
  */
 #include "velodyne_manager.h"
@@ -20,7 +20,7 @@ Error_manager Velodyne_manager::velodyne_manager_init_from_protobuf(std::string
     velodyne::velodyneManagerParams t_velo_params;
     if (!proto_tool::read_proto_param(prototxt_path, t_velo_params))
     {
-        return Error_manager(WJ_MANAGER_READ_PROTOBUF_ERROR, MINOR_ERROR, "velodyne_manager read_proto_param  failed");
+        return Error_manager(VELODYNE_MANAGER_READ_PROTOBUF_ERROR, MINOR_ERROR, "velodyne_manager read_proto_param  failed");
     }
 
     return velodyne_manager_init_from_protobuf(t_velo_params, terminal);
@@ -366,7 +366,7 @@ void Velodyne_manager::collect_cloud_thread_fun()
 					if (t_error != Error_code::SUCCESS)
 					{
 						t_result.compare_and_cover_error(t_error);
-						//						LOG(WARNING) << "cloud timeout: "<<get_cloud_count;
+						LOG_EVERY_N(WARNING, 9000) << "cloud timeout lidar id: "<<iter->first;
 						// 将该雷达对应所有区域点云清空
 					}
 					//                    get_cloud_count++;
@@ -397,10 +397,12 @@ void Velodyne_manager::collect_cloud_thread_fun()
 							if (t_error != SUCCESS)
 							{
 								m_region_laser_to_cloud_collection_map[t_region_lidar_key]->clearCloud();
+								// LOG(WARNING) << "编号" << iter->first << "雷达在区域" << iter_region->first << "中点云清除 " << t_error.to_string();
 							}
 							else
 							{
 								m_region_laser_to_cloud_collection_map[t_region_lidar_key]->setCloud(t_cloud);
+								// LOG(WARNING) << "编号" << iter->first << "雷达在区域" << iter_region->first << "中点云 " << t_cloud->size();
 							}
 						}
 					}
@@ -414,7 +416,7 @@ void Velodyne_manager::collect_cloud_thread_fun()
 				else
 				{
 					// mp_cloud_collection->clear();
-//					LOG(WARNING) << t_result.to_string();
+					// 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();
@@ -447,6 +449,7 @@ Error_manager Velodyne_manager::update_region_cloud()
 				if (t_timed_cloud->m_cloud->size() > 0)
 				{
 					t_region_cloud->operator+=(*t_timed_cloud->transCloud());
+					// LOG(WARNING) << "编号" << iter_lidar->first << "雷达在区域" << iter->first << "中点云已添加" << t_timed_cloud->m_cloud->size();
 				}
 				else
 				{
@@ -462,6 +465,7 @@ Error_manager Velodyne_manager::update_region_cloud()
 		}
 
 		iter->second->update_cloud(t_region_cloud);
+		// LOG(WARNING) << "send to region" << iter->first << "with cloud size: " << t_region_cloud->size();
 	}
 	return Error_code::SUCCESS;
 }

velodyne_lidar/velodyne_manager.h

@@ -4,7 +4,7 @@
  * 分布式模式下，程序各自独立，分别只实例化一个设定的区域，以及相应雷达
  * @Author: yct
  * @Date: 2021-07-23 16:37:33
- * @LastEditTime: 2021-09-02 17:27:03
+ * @LastEditTime: 2021-09-03 16:20:26
  * @LastEditors: yct
  */
 
@@ -85,6 +85,7 @@ struct Timestamped_cloud
 					m_cloud->points[i].z = t_point.z() / t_point.w();
 				}
 			}
+			t_cloud->operator+=(*m_cloud);
 
 			return t_cloud;			
 		}