add c2 vlp16 lidar ,change groud cloud cut by classis

测量节点/main.cpp

@@ -195,12 +195,11 @@ int main(int argc,char* argv[])
 	System_communication::get_instance_references().communication_uninit();
 	System_communication_mq::get_instance_references().rabbitmq_uninit();
 
-
+	if(WJ_VELO == 1 || WJ_VELO == 2)
+		Velodyne_manager::get_instance_references().Velodyne_manager_uninit();
 	System_executor::get_instance_references().system_executor_uninit();
 	if(WJ_VELO == 0 || WJ_VELO == 2)
 		Wanji_manager::get_instance_references().wanji_manager_uninit();
-	if(WJ_VELO == 1 || WJ_VELO == 2)
-		Velodyne_manager::get_instance_references().Velodyne_manager_uninit();
 
 	return 0;
 }

测量节点/rslidar/rslidar_driver.cpp

@@ -73,7 +73,6 @@ Error_manager RS_lidar_device::init(velodyne::velodyneLidarParams params)
 Error_manager RS_lidar_device::uninit()
 {
     mb_exit_process = true;
-    m_lidar_driver.stop();
 
     if(mp_cloud_handle_thread != nullptr)
     {
@@ -82,6 +81,7 @@ Error_manager RS_lidar_device::uninit()
         delete(mp_cloud_handle_thread);
         mp_cloud_handle_thread = nullptr;
     }
+    m_lidar_driver.stop();
 
 	if (m_rs_lidar_device_status != E_FAULT)
 	{

测量节点/rslidar/rslidar_driver.h

@@ -118,6 +118,7 @@ public:
 	{
 		while (!mb_exit_process)
 		{
+			usleep(1);
 			std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();
 			if (msg.get() == NULL)
 			{

测量节点/setting/velodyne_manager.prototxt

@@ -10,10 +10,9 @@ distribution_mode:false
 velodyne_lidars
 {
     ip:""
-    port:2368
-    model:"VLP16"
-    # port:2367
-    # model:"RSHELIOS16"
+    # # rshelios16 ---- 192.168.1.208
+    port:2367
+    model:"RSHELIOS16"
     calibrationFile:"../setting/VLP16db.yaml"
     lidar_id:6
     max_range:8.0
@@ -23,14 +22,15 @@ velodyne_lidars
     rpm:600
     calib
     {
-         r:-0.276397
-         p:-0.11717
-         y:91.572  # 88.703 #89.6003
+         r:179.276397
+         p:-0.11717  #p:-0.11717
+         y:-1.572
          cz:0.05467
-        #r:-0.623165
-        #p:0.601821
-        #y:87.0198
-        #cz:0.101527
+        # # helios
+        # r: -1.920474
+        # p: 2.0245645
+        # y: 87.24734
+        # cz:0.105
     }
     difop:7788
 }
@@ -179,10 +179,10 @@ velodyne_lidars
 region
 {
     minx:-1.6
-	maxx:1.6
+	maxx:1.4
 	miny:-2.6
 	maxy:2.6
-	minz:0.025 # 0.025
+	minz:0.03 # 0.025
 	maxz:0.5
     region_id:5
     turnplate_cx:0.0
@@ -206,8 +206,10 @@ region
         lidar_id:6
         calib
         {
-            cx:1.92004
-            cy:-0.03
+            r:1.0
+            p:0.5
+            cx:1.86504
+            cy:-0.027
         }
     }
     lidar_exts
@@ -215,10 +217,12 @@ region
         lidar_id:5
         calib
         {
+            r:1.5
+            p:-1.0
             #cx:-4.021775
             #cy:-0.039429
             cx:-1.89004
-            cy:0
+            cy:0.0
         }
     }
 }

测量节点/system/system_executor.cpp

@@ -543,7 +543,7 @@ Error_manager System_executor::encapsulate_send_mq_status()
 		// 获取区域点云填入信息
 		pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
 		iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
-		if (cloud_count == 5)
+		if (cloud_count == 10)
 		{
 			std::string t_filename = std::string("vlp16_region_") + std::to_string(iter->first) + "_cloud.txt";
 			save_cloud_txt(t_region_cloud, t_filename);

测量节点/tests/lidar_calib_tools.cpp

@@ -196,9 +196,10 @@ void matrix_trans()
     //     -0.888444364071, -0.143861815333, 0.435855954885,
     //     -0.440983742476, 0.004216216505, -0.897505164146;
     //621
-    rot_matrix0 << -0.154493823647, 0.987829446793, 0.018017675728,
-            0.905054211617, 0.148814871907, -0.398410588503,
-            -0.396243005991, -0.045244999230, -0.91703021526;
+    rot_matrix0 << -0.108142159879, -0.990688860416, 0.082709200680, 3.672443628311,
+                    0.993864834309, -0.109678961337, -0.014255203307, -0.117867439985,
+                    0.023193931207, 0.080660179257, 0.996471762657, 0.054298080504,
+                    0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000;
     Eigen::Matrix3d rot_matrix1;
     rot_matrix1.setIdentity();
     // rot_matrix1 << 0.9993, 0.03618, 0.0,

测量节点/tests/rslidar_test.cpp

@@ -37,7 +37,8 @@ void bridge_test()
 {
     velodyne::velodyneLidarParams params;
     params.set_ip("");
-    params.set_port(6699);
+    params.set_port(2367);
+	params.set_difop(7788);
     params.set_model("HELIOS16P");
     params.set_calibrationfile("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/VLP16db.yaml");
 

测量节点/velodyne_lidar/ground_region.cpp

@@ -604,21 +604,32 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         t_width_extract_cloud->operator+=(m_detector->m_right_rear_cloud);
 
         pcl::PointXYZ t_rough_min_p, t_rough_max_p;
+        //add by zzw
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_for_width_upwheel(new pcl::PointCloud<pcl::PointXYZ>);
+        for (int i = 0; i < t_width_rough_cloud->size(); ++i)
+        {
+            pcl::PointXYZ pt = t_width_rough_cloud->points[i];
+            if (pt.z > chassis_z)
+            {
+                cloud_for_width_upwheel->push_back(pt);
+            }
+        }
+
         // rough width for human detect
         {
             //离群点过滤
-            sor.setInputCloud(t_width_rough_cloud);
+            sor.setInputCloud(cloud_for_width_upwheel);
             sor.setMeanK(5);                    //K近邻搜索点个数
             sor.setStddevMulThresh(1.0);        //标准差倍数
             sor.setNegative(false);             //保留未滤波点（内点）
-            sor.filter(*t_width_rough_cloud); //保存滤波结果到cloud_filter
+            sor.filter(*cloud_for_width_upwheel); //保存滤波结果到cloud_filter
 
             Eigen::Affine3d t_affine = Eigen::Affine3d::Identity();
             t_affine.prerotate(Eigen::AngleAxisd((90 - last_result.theta) * M_PI / 180.0, Eigen::Vector3d::UnitZ()));
-            pcl::transformPointCloud(*t_width_rough_cloud, *t_width_rough_cloud, t_affine.matrix());
+            pcl::transformPointCloud(*cloud_for_width_upwheel, *cloud_for_width_upwheel, t_affine.matrix());
 
             // 车宽重计算，并赋值到当前车宽
-            pcl::getMinMax3D(*t_width_rough_cloud, t_rough_min_p, t_rough_max_p);
+            pcl::getMinMax3D(*cloud_for_width_upwheel, t_rough_min_p, t_rough_max_p);
         }
         double rough_width = t_rough_max_p.x - t_rough_min_p.x;
 
@@ -640,7 +651,7 @@ Error_manager Ground_region::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, d
         double accurate_width = t_max_p.x - t_min_p.x;
         last_result.width = accurate_width;
 
-        if(fabs(rough_width - accurate_width) > 0.08)
+        if(fabs(rough_width - accurate_width) > 0.15)
         {
             std::string disp_str = std::string("width difference too large, assume noise situation, ")+std::to_string(rough_width)+", "+std::to_string(accurate_width);
             LOG(WARNING) << disp_str;

测量节点/velodyne_lidar/velodyne_manager.cpp

@@ -389,7 +389,7 @@ void Velodyne_manager::collect_cloud_thread_fun()
 						t_result.compare_and_cover_error(t_error);
 						LOG_EVERY_N(WARNING, 9000) << "cloud timeout lidar id: "<<iter->first;
 						// 将该雷达对应所有区域点云清空
-					}else if(cloud_count==5){
+					}else if(cloud_count==10){
 						std::string t_filename = std::string("./intrin_cloud_")+std::to_string(iter->first)+".txt";
 						save_cloud_txt(t_cloud, t_filename);
 						LOG(INFO) << "lidar intrin cloud has been saved in " + t_filename;