wj 716n calib, fusion test,

remove wj statble check

CMakeLists.txt

@@ -2,7 +2,7 @@ project(measure_wj_proj)
 
 cmake_minimum_required(VERSION 3.5)
 
-set (CMAKE_CXX_STANDARD 14)
+set (CMAKE_CXX_STANDARD 11)
 set(CMAKE_BUILD_TYPE "Release")
 
 # set(PCL_DIR "/home/youchen/pcl-1.8/share/pcl-1.8")
@@ -27,7 +27,7 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
 
 #other libs
 FIND_PACKAGE(Protobuf REQUIRED)
-# FIND_PACKAGE(Glog REQUIRED)
+FIND_PACKAGE(Glog REQUIRED)
 FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL REQUIRED)
 FIND_PACKAGE(Ceres REQUIRED)
@@ -86,8 +86,8 @@ add_executable(measure_wj
 		)
 
 target_link_libraries(measure_wj
-        /usr/lib/x86_64-linux-gnu/libglog.so
-        /usr/lib/x86_64-linux-gnu/libgflags.so
+        /usr/local/lib/libglog.a
+        /usr/local/lib/libgflags.a
         nnxx
         nanomsg
 
@@ -122,8 +122,8 @@ ${VELODYNE_LIDAR_DRIVER}
 ${VELODYNE_LIDAR}
 )
 target_link_libraries(vlp16 
-/usr/lib/x86_64-linux-gnu/libglog.so
-/usr/lib/x86_64-linux-gnu/libgflags.so
+/usr/local/lib/libglog.a
+/usr/local/lib/libgflags.a
 nnxx
 nanomsg
 

setting/communication.prototxt

@@ -3,7 +3,8 @@
 communication_parameters
 {
 
-  bind_string:"tcp://127.0.0.1:30010"
+  bind_string:"tcp://192.168.1.233:30010"
+  connect_string_vector:"tcp://192.168.1.233:30000"
   # connect_string_vector:"tcp://127.0.0.1:30000"
  # connect_string_vector:"tcp://192.168.2.166:9002"
 

setting/velodyne_manager.prototxt

@@ -1,7 +1,7 @@
 fence_data_path:"/home/zx/data/ground_detect/"
 #fence_log_path:"/home/zx/yct/MainStructure/new_electronic_fence/log"
-left_model_path:"/home/yct/Documents/ierobot/measure/puai_wj_2021/setting/left_model.txt"
-right_model_path:"/home/yct/Documents/ierobot/measure/puai_wj_2021/setting/right_model.txt"
+left_model_path:"/home/zx/yct/chutian_measure_2021/setting/left_model.txt"
+right_model_path:"/home/zx/yct/chutian_measure_2021/setting/right_model.txt"
 distribution_mode:false
 
 #-----------------------------------lidars, id 0-6 from A1-C2

setting/wanji_manager.prototxt

@@ -14,24 +14,24 @@ wj_lidar
     net_config
     {
         ip_address:"192.168.1.219"
-        port:2112
+        port:2110
     }
     transform
     {
-        m00:-0.0773542
-		m01:0.9970038
-		m02:1.742
-		m10:0.9970038
-		m11:0.0773542
-		m12:-2.5350000
+        m00:0.9999
+		m01:-0.014
+		m02:0.0#-1.87
+		m10:0.014
+		m11:0.9999
+		m12:0.0#-2.03
     }
     scan_limit
     {
         dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
+        minx:-4.0
+        maxx:4.0
+        miny:-1.5
+        maxy:6.0
     }
     lidar_id:8
 }
@@ -48,24 +48,24 @@ wj_lidar
     net_config
     {
         ip_address:"192.168.1.220"
-        port:2112
+        port:2110
     }
     transform
     {
-        m00:0.0255674
-		m01:-0.9996730
-		m02:5.233
-		m10:-0.9996730
-		m11:-0.0255674
-		m12:3.44200000
+        m00:-0.99976
+		m01:-0.02166
+		m02:0.0#-1.8476
+		m10:0.02166
+		m11:-0.99976
+		m12:0.0#1.89293
     }
     scan_limit
     {
         dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
+        minx:-4.0
+        maxx:4.0
+        miny:-1.5
+        maxy:6.0
     }
     lidar_id:9
 }
@@ -82,24 +82,24 @@ wj_lidar
     net_config
     {
         ip_address:"192.168.1.221"
-        port:2112
+        port:2110
     }
     transform
     {
-        m00:0.1196470
-		m01:0.9928164
-		m02:8.617
-		m10:0.9928164
-		m11:-0.1196470
-		m12:-2.5510000
+        m00:0.999
+		m01:0.04442
+		m02:0.0#1.9111
+		m10:-0.04442
+		m11:0.999
+		m12:0.0#-2.0223
     }
     scan_limit
     {
         dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:3.2
+        minx:-3.0
+        maxx:6.0
+        miny:-14
+        maxy:14
     }
     lidar_id:10
 }
@@ -116,24 +116,24 @@ wj_lidar
     net_config
     {
         ip_address:"192.168.1.222"
-        port:2112
+        port:2110
     }
     transform
     {
-        m00:0.0158848
-		m01:-0.9998739
-		m02:12.162
-		m10:-0.9998739
-		m11:-0.0158848
-		m12:3.43600000
+        m00:-0.9999
+		m01:-0.00311
+		m02:0.0#1.9236
+		m10:0.00311
+		m11:-0.9999
+		m12:0.0#1.8811
     }
     scan_limit
     {
         dist_limit:8
-        minx:0.1
-        maxx:5.5
-        miny:-4
-        maxy:4
+        minx:-4.0
+        maxx:4.0
+        miny:-1.5
+        maxy:6.0
     }
     lidar_id:11
 }
@@ -232,20 +232,21 @@ wj_lidar
 #5
 regions
 {
-    minx:-1.6
-	maxx:1.6
+    minx:-1.42
+	maxx:1.5
 	miny:-2.6
-	maxy:2.6
-	minz:0.025
+	maxy:2.3
+	minz:0.03
 	maxz:0.5
-    region_id:5
+    region_id:4
     turnplate_cx:0.0
     turnplate_cy:0.0
     border_minx:-1.2
     border_maxx:1.2
-    plc_offsetx:1.913
-    plc_offsety:-6.078
-    plc_offset_degree:-89.5
+    # -1.8683
+    plc_offsetx:-1.865
+    plc_offsety:-6.11489
+    plc_offset_degree:-89.6
     plc_border_miny:-7.51
     car_min_width:1.55
     car_max_width:1.92
@@ -259,8 +260,8 @@ regions
         lidar_id:8
         calib
         {
-            cx:0.0
-            cy:0.0
+            cx:-1.87
+            cy:-2.03
         }
     }
     lidar_exts
@@ -268,8 +269,8 @@ regions
         lidar_id:9
         calib
         {
-            cx:0.0
-            cy:0.0
+            cx:-1.8476
+            cy:1.89293
         }
     }
     lidar_exts
@@ -277,8 +278,8 @@ regions
         lidar_id:10
         calib
         {
-            cx:0.0
-            cy:0.0
+            cx:1.9111
+            cy:-2.0223
         }
     }
     lidar_exts
@@ -286,8 +287,8 @@ regions
         lidar_id:11
         calib
         {
-            cx:0.0
-            cy:0.0
+            cx:1.9236
+            cy:1.8811
         }
     }
 }

system/system_executor.cpp

@@ -584,7 +584,7 @@ Error_manager System_executor::encapsulate_send_status()
 			// 获取区域点云填入信息
 			pcl::PointCloud<pcl::PointXYZ>::Ptr t_wj_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
 			t_wj_region_iter->second->get_region_cloud(t_wj_region_cloud);
-			if (cloud_count == 5)
+			if (cloud_count == 8)
 			{
 				std::string t_filename = std::string("wj_region_") + std::to_string(t_wj_region_iter->first) + "_cloud.txt";
 				save_cloud_txt(t_wj_region_cloud, t_filename);
@@ -598,14 +598,18 @@ Error_manager System_executor::encapsulate_send_status()
 			// 	tp_cloud->set_z(t_wj_region_cloud->points[j].z);
 			// }
 
-			// 融合测量结果，验证中心点差异x<0.03, y<0.03, 角度差异ang<0.6, 轴距差异wb<0.055
+			// LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<"\n"<<t_car_wheel_information.to_string()<<"\n"
+			// 								<<t_wj_car_wheel_information.to_string()<<"\n"
+			// 								<<t_car_wheel_information.range_status<<", "<<t_wj_car_wheel_information.range_status;
+			// 融合测量结果，验证中心点差异x<0.03, y<0.06, 角度差异ang<0.6, 轴距差异wb<0.055
 			double dx = t_car_wheel_information.car_center_x - t_wj_car_wheel_information.car_center_x;
 			double dy = t_car_wheel_information.car_center_y - t_wj_car_wheel_information.car_center_y;
 			double dang = t_car_wheel_information.car_angle - t_wj_car_wheel_information.car_angle;
 			double dwb = t_car_wheel_information.car_wheel_base - t_wj_car_wheel_information.car_wheel_base;
-			if(fabs(dx) < 0.03 && fabs(dy) < 0.03 && fabs(dang) < 0.6 && fabs(dwb) < 0.055)
+
+			if(fabs(dx) < 0.03 && fabs(dy) < 0.06 && fabs(dang) < 0.6 && fabs(dwb) < 0.055)
 			{
-				t_car_wheel_information.car_wheel_base = t_car_wheel_information.car_wheel_base*0.2 + t_car_wheel_information.car_wheel_base*0.8;
+				t_car_wheel_information.car_wheel_base = t_car_wheel_information.car_wheel_base*0.3 + t_car_wheel_information.car_wheel_base*0.7;
 			}else
 			{
 				LOG_IF(WARNING, iter->second->get_terminal_id() == DISP_TERM_ID)<<

system/system_executor.h

@@ -18,7 +18,7 @@
 #include "../wanji_lidar/wanji_manager.h"
 #include "../velodyne_lidar/velodyne_manager.h"
 
-#define DISP_TERM_ID 5
+#define DISP_TERM_ID -1
 // 0wj  1velo	2both
 #define WJ_VELO 2
 

velodyne_lidar/ground_region.cpp

@@ -865,11 +865,7 @@ void Ground_region::thread_measure_func()
                 // else{
                 //     std::cout<<ec.to_string()<<std::endl;
                 // }
-<<<<<<< HEAD
                 //  LOG_IF(INFO, m_region.region_id() == 1) << m_car_wheel_information.to_string();
-=======
-                 //LOG_IF(INFO, m_region.region_id() == 4 || m_region.region_id() == 1) << m_car_wheel_information.to_string();
->>>>>>> 00fb9584746fa2f0fc86b7596cdc4711435586f0
             }
             else
             {

wanji_lidar/region_detect.cpp

@@ -424,7 +424,13 @@ Error_manager Region_detector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud
     detect_wheel_ceres::Detect_result detect_result{x,y,c,wheelbase,width,front_wheel_theta};
     std::string error_info;
     if(!m_detector_ceres.detect(seg_clouds, detect_result, error_info))
+    {
+        if(print)
+        {
+            LOG(WARNING) << error_info;
+        }
         return WJ_REGION_CERES_SOLVE_ERROR;
+    }
     else {
         x = detect_result.cx;
         y = detect_result.cy;

wanji_lidar/region_worker.cpp

@@ -416,19 +416,20 @@ void Region_worker::auto_detect_thread_fun()
                 t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_x -= m_region_param.plc_offsetx();
                 t_wheel_info_stamped_for_car_move.wheel_data.uniform_car_y -= m_region_param.plc_offsety();
                 t_wheel_info_stamped_for_car_move.wheel_data.car_angle -= m_region_param.plc_offset_degree();
-                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
-                // success means car stable
-                if(car_status_res == SUCCESS)
-                {
-                    m_car_wheel_information.range_status &= ~((int)Range_status_wj::Range_car_moving);
-                }else
-                {
-                    m_car_wheel_information.range_status |= Range_status_wj::Range_car_moving;
-                    // if(m_region_param.region_id()==4){
-                    //     std::cout<<"success: "<<car_status_res.to_string()<<std::endl;
-                    // }
-                }
-                Region_status_checker::get_instance_references().add_measure_data(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
+                // // wj don't test if statble
+                // Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
+                // // success means car stable
+                // if(car_status_res == SUCCESS)
+                // {
+                //     m_car_wheel_information.range_status &= ~((int)Range_status_wj::Range_car_moving);
+                // }
+                // else{
+                //     m_car_wheel_information.range_status |= Range_status_wj::Range_car_moving;
+                //     if(m_region_param.region_id()==4){
+                //         LOG(WARNING)<<"success: "<<car_status_res.to_string()<<std::endl;
+                //     }
+                // }
+                // Region_status_checker::get_instance_references().add_measure_data(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
 
                 t_error = Measure_filter::get_instance_references().get_filtered_wheel_information(m_region_param.region_id(), t_wheel_info_stamped.wheel_data);
                 if (t_error == SUCCESS)
@@ -444,26 +445,27 @@ void Region_worker::auto_detect_thread_fun()
                 // else{
                 //     std::cout<<t_error.to_string()<<std::endl;
                 // }
-                // LOG_IF(INFO, m_region_param.region_id() == 3 || m_region_param.region_id() == 5) << m_car_wheel_information.to_string();
+                // LOG_IF(INFO, m_region_param.region_id() == 4 || m_region_param.region_id() == 5) << m_car_wheel_information.to_string();
             }
             else
             {
                 m_car_wheel_information.correctness = false;
-                // LOG_IF(ERROR, m_region_param.region_id() == 3 || m_region_param.region_id() == 5) << m_car_wheel_information.to_string();
+                // LOG_IF(ERROR, m_region_param.region_id() == 4 || m_region_param.region_id() == 5) <<t_error<<", "<< m_car_wheel_information.to_string();
 
                 // 20211228 added by yct, car movement checking, human and door detection
-                Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
-                // success means car stable
-                if(car_status_res == SUCCESS)
-                {
-                    m_car_wheel_information.range_status &= ~((int)Range_status_wj::Range_car_moving);
-                }else
-                {
-                    m_car_wheel_information.range_status |= Range_status_wj::Range_car_moving;
-                    // if(m_region_param.region_id()==4){
-                    //     std::cout<<"failed: "<<car_status_res.to_string()<<std::endl;
-                    // }
-                }
+                // wj don't test if statble
+                // Error_manager car_status_res = Region_status_checker::get_instance_references().get_region_parking_status(m_region_param.region_id(), t_wheel_info_stamped_for_car_move, *mp_cloud_collection);
+                // // success means car stable
+                // if(car_status_res == SUCCESS)
+                // {
+                //     m_car_wheel_information.range_status &= ~((int)Range_status_wj::Range_car_moving);
+                // }else
+                // {
+                //     m_car_wheel_information.range_status |= Range_status_wj::Range_car_moving;
+                //     // if(m_region_param.region_id()==4){
+                //     //     std::cout<<"failed: "<<car_status_res.to_string()<<std::endl;
+                //     // }
+                // }
             }
 		}
 	}

wanji_lidar/wanji_716N_device.cpp

@@ -224,15 +224,15 @@ Error_manager Wanji_716N_lidar_device::get_last_cloud(pcl::PointCloud<pcl::Point
 	int t_scan_cycle_ms = m_protocol.get_scan_cycle();
 	//将指令时间提前一个扫描周期, 然后获取这个时间后面的点云.
 	//就是说, 在收到指令的时候, 可以使用一个周期以前的数据, 不用获取最新的点云.
-	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms) )
+	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms*2) )
 	{
 		Error_manager code = m_protocol.get_scan_points(p_cloud);
-//		LOG(WARNING) << p_cloud->size();
+		// LOG(WARNING) << " !!!!!!!!!!!!! wj scan cloud: "<<p_cloud->size();
 		return code;
 	}
 	else
 	{
-//        LOG(WARNING) << "get cloud failed.....";
+    //    LOG(WARNING) << "get cloud failed..... "<<m_protocol.get_scan_time().time_since_epoch().count()<<", "<<command_time.time_since_epoch().count();
 		return Error_manager(Error_code::WANJI_LIDAR_DEVICE_NO_CLOUD, Error_level::MINOR_ERROR,
 							 " Wanji_716N_lidar_device::get_last_cloud error ");
 	}

wanji_lidar/wanji_manager.cpp

@@ -439,7 +439,7 @@ void Wanji_manager::collect_cloud_thread_fun()
 						LOG_EVERY_N(WARNING, 9000) << "cloud timeout lidar id: "<<iter->first;
 						// 将该雷达对应所有区域点云清空
 					}else if(cloud_count==5){
-						std::string t_filename = std::string("./intrin_cloud_")+std::to_string(iter->first)+".txt";
+						std::string t_filename = std::string("./wj_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;
 					}
@@ -543,28 +543,27 @@ Error_manager Wanji_manager::update_region_cloud()
 
 		// 必须传入空点云以避免入口状态不更新
 		// wj 4角校验不可简单根据点数判断
-		// if(corresponding_lidar_ids.size() == 4 ){
-		// 	// 检查两边点云个数，若差异过大则清除点云，传入空点云以更新结果
-		// 	int max_cloud_size = std::max(corresponding_lidar_cloud_size[0], corresponding_lidar_cloud_size[1]);
-		// 	max_cloud_size = std::max(corresponding_lidar_cloud_size[2], max_cloud_size);
-		// 	max_cloud_size = std::max(corresponding_lidar_cloud_size[3], max_cloud_size);
-		// 	int min_cloud_size = std::min(corresponding_lidar_cloud_size[0], corresponding_lidar_cloud_size[1]);
-		// 	min_cloud_size = std::min(corresponding_lidar_cloud_size[2], min_cloud_size);
-		// 	min_cloud_size = std::min(corresponding_lidar_cloud_size[2], min_cloud_size);
-		// 	// 极限相差4倍，或一边仅不到10个点，则清除点云，传入空点云以更新结果
-		// 	if (max_cloud_size > 4 * min_cloud_size + 10)
-		// 	{
-		// 		LOG_EVERY_N(ERROR, 60) << "入口" << iter->first << "两片点云差异过大，分别为:"<<max_cloud_size<<"个与"<<min_cloud_size<<"个，请检查！！！ ";
-		// 		// 清除点云
-		// 		t_region_cloud->clear();
-		// 	}
-		// 	else
-		// 	{
-		// 		// 不用修改点云内容
-		// 	}
-		// }
-		// else 
-		if (corresponding_lidar_ids.size() < 4)
+		if(corresponding_lidar_ids.size() == 4 ){
+			// 检查两边点云个数，若差异过大则清除点云，传入空点云以更新结果
+			int max_cloud_size = std::max(corresponding_lidar_cloud_size[0], corresponding_lidar_cloud_size[1]);
+			max_cloud_size = std::max(corresponding_lidar_cloud_size[2], max_cloud_size);
+			max_cloud_size = std::max(corresponding_lidar_cloud_size[3], max_cloud_size);
+			int min_cloud_size = std::min(corresponding_lidar_cloud_size[0], corresponding_lidar_cloud_size[1]);
+			min_cloud_size = std::min(corresponding_lidar_cloud_size[2], min_cloud_size);
+			min_cloud_size = std::min(corresponding_lidar_cloud_size[2], min_cloud_size);
+			// 极限相差10倍，或一边仅不到10个点，则清除点云，传入空点云以更新结果
+			if ((min_cloud_size < 10 && max_cloud_size > 30) || (min_cloud_size >= 10 && max_cloud_size > 10 * min_cloud_size + 10))
+			{
+				LOG_EVERY_N(ERROR, 60) << "入口" << iter->first << "两片点云差异过大，分别为:"<<max_cloud_size<<"个与"<<min_cloud_size<<"个，请检查！！！ ";
+				// 清除点云
+				t_region_cloud->clear();
+			}
+			else
+			{
+				// 不用修改点云内容
+			}
+		}
+		else if (corresponding_lidar_ids.size() < 4)
 		{
 			LOG_EVERY_N(WARNING, 60) << "检测到入口" << iter->first << "点云缺失, 找到" << corresponding_lidar_ids.size() << "个雷达的点云";
 			// 只有半边点云，将其清除

wanji_lidar/wj_716N_lidar_protocol.cpp

@@ -101,10 +101,15 @@ Error_manager wj_716N_lidar_protocol::init(Thread_safe_queue<Binary_buf *> *p_co
     t_config.max_ang = polar_coordinates_box.angle_max;
     t_config.range_min = polar_coordinates_box.distance_min;
     t_config.range_max = polar_coordinates_box.distance_max;
+    t_config.frequency_scan = 2;
     setConfig(t_config, 0);
 
 	mp_communication_data_queue = p_communication_data_queue;
 	m_point2D_box = point2D_box;
+    std::cout << "box xmin:" << m_point2D_box.x_min << std::endl;
+    std::cout << "box xmax:" << m_point2D_box.x_max << std::endl;
+    std::cout << "box ymin:" << m_point2D_box.y_min << std::endl;
+    std::cout << "box ymax:" << m_point2D_box.y_max << std::endl;
 	m_point2D_transform = point2D_transform;
 	//接受线程, 默认开启循环, 在内部的wait_and_pop进行等待
 	m_condition_analysis.reset(false, true, false);
@@ -370,6 +375,7 @@ bool wj_716N_lidar_protocol::dataProcess(unsigned char *data, const int reclen)
                         if(scandata[i - index_start] == NAN)
                         {
                             scan.intensities[i - index_start] = 0;
+                            continue;
                         }
                         else
                         {

wanji_lidar/wj_716N_lidar_protocol.h

@@ -98,7 +98,7 @@ public:
 	// 获取扫描点云
 	Error_manager get_scan_points(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
 	// 获取扫描点云更新时间点
-	std::chrono::system_clock::time_point get_scan_time(){return std::chrono::time_point<std::chrono::system_clock, std::chrono::milliseconds>(std::chrono::milliseconds(scan.scan_time));}
+	std::chrono::system_clock::time_point get_scan_time(){return std::chrono::time_point<std::chrono::system_clock, std::chrono::milliseconds>(std::chrono::milliseconds(scan.header.stamp.msecs));}
 
     bool dataProcess(unsigned char *data,const int reclen);
     bool protocl(unsigned char *data,const int len);