20220630chutian

main.cpp

@@ -109,7 +109,7 @@ int main(int argc,char* argv[])
 		return -1;
 	}
 
-	usleep(1000 * 500);
+	// usleep(1000 * 500);
 
 	ec = System_communication::get_instance_references().communication_init();
 	if(ec != SUCCESS)

setting/velodyne_manager.prototxt

@@ -184,8 +184,8 @@ region
     region_id:5
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
+    border_minx:-1.2
+    border_maxx:1.2
     plc_offsetx:1.913
     plc_offsety:-6.078
     plc_offset_degree:-89.5
@@ -236,8 +236,8 @@ region
     region_id:4
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
+    border_minx:-1.2
+    border_maxx:1.2
     # -1.8683
     plc_offsetx:-1.865
     plc_offsety:-6.11489
@@ -283,8 +283,8 @@ region
     region_id:3
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
+    border_minx:-1.2
+    border_maxx:1.2
     plc_offsetx:1.926
     plc_offsety:-6.135
     plc_offset_degree:-89.5
@@ -304,6 +304,7 @@ region
         {
             cx:1.902
             cy:-0.0031
+            cz:0.015
         }
     }
     lidar_exts
@@ -329,8 +330,8 @@ region
     region_id:2
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
+    border_minx:-1.2
+    border_maxx:1.2
     # 0.02
     plc_offsetx:-1.871
     # -0.34
@@ -374,18 +375,18 @@ region
 region
 {
     minx:-1.5
-	maxx:1.5
+	maxx:1.3
 	miny:-2.6
 	maxy:2.3
-	minz:0.03
+	minz:0.032
 	maxz:0.5
     region_id:1
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
+    border_minx:-1.2
+    border_maxx:1.2
     plc_offsetx:1.93
-    plc_offsety:-6.1368
+    plc_offsety:-6.1368  
     plc_offset_degree:-89.5
     plc_border_miny:-7.45
     car_min_width:1.55
@@ -404,7 +405,9 @@ region
         {
             cx:1.89111#1.91111219
             cy:-0.053426
-            p:-0.5
+            cz:-0.005
+            p:-1.5
+            # y:1.0
         }
     }
     lidar_exts
@@ -427,13 +430,13 @@ region
 	maxx:1.5
 	miny:-2.6
 	maxy:2.3
-	minz:0.03
+	minz:0.032
 	maxz:0.5
     region_id:0
     turnplate_cx:0.0
     turnplate_cy:0.0
-    border_minx:-1.3
-    border_maxx:1.3
+    border_minx:-1.2
+    border_maxx:1.2
     plc_offsetx:-1.86
     plc_offsety:-6.1368
     plc_offset_degree:-89.5

system/system_executor.cpp

@@ -266,6 +266,7 @@ Error_manager System_executor::encapsulate_send_status()
 	int region_index = 0;
 	for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
 	{
+		bool has_mulfunctional_device = false;
 		// 以t_ground_status_msg为模板创建各区域心跳消息
 		message::Ground_status_msg t_multi_status_msg;
 		t_multi_status_msg.CopyFrom(t_ground_status_msg);
@@ -280,6 +281,10 @@ Error_manager System_executor::encapsulate_send_status()
 				LOG(WARNING) << "lidar status "<<t_param.lidar_exts(j).lidar_id()<<" cannot be found, param error";
 			}else{
 				t_multi_status_msg.add_wanji_lidar_device_status((message::Wanji_lidar_device_status)t_status_iter->second);
+				if(t_status_iter->second != Velodyne_lidar_device::E_READY && t_status_iter->second != Velodyne_lidar_device::E_BUSY)
+				{
+					has_mulfunctional_device = true;
+				}
 			}
 		}
 		//velodyne雷达的自动定位信息
@@ -383,14 +388,25 @@ Error_manager System_executor::encapsulate_send_status()
 			t_error.set_error_description(t_error_str);
 		}
 
-		t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
-        t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
-        t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
+		if(has_mulfunctional_device)
+		{
+			t_multi_status_msg.mutable_error_manager()->set_error_code(Error_code::VELODYNE_LIDAR_COMMUNICATION_DISCONNECT);
+			t_multi_status_msg.mutable_error_manager()->set_error_level(message::Error_level::MAJOR_ERROR);
+			t_multi_status_msg.mutable_error_manager()->set_error_description("has mulfunctional lidar device, please check.");
+		}
+		else
+		{
+			t_multi_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
+			t_multi_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
+			t_multi_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
+		}
 
 		std::string t_msg = t_multi_status_msg.SerializeAsString();
 		System_communication::get_instance_references().encapsulate_msg(t_msg);
 		if(t_multi_status_msg.id_struct().terminal_id()==DISP_TERM_ID)
         	std::cout<<t_multi_status_msg.DebugString()<<std::endl<<std::endl;
+
+		usleep(1000*10);
 	}
 
 	// 普爱统一一个万集节点, 各终端消息分别发送

velodyne_lidar/velodyne_driver/velodyne_lidar_device.cpp

@@ -606,13 +606,21 @@ void Velodyne_lidar_device::parse_thread_fun()
 					{
 						m_num_packet_inside = 0;
 						m_ring_cloud.clear();
-						m_ring_cloud = m_cloud_buf;
-						m_cloud_buf.clear();
+						// 20220411 changed by yct, A1A2中间1号雷达8号线出现问题
+						// m_ring_cloud = m_cloud_buf;
+						// m_cloud_buf.clear();
 						// 对环点云进行内参变换
-						for (size_t i = 0; i < m_ring_cloud.size(); i++)
+						for (size_t i = 0; i < m_cloud_buf.size(); i++)
 						{
-							m_ring_cloud[i].transform(m_calib_matrix);
+							Lidar_point t_point = m_cloud_buf[i];
+							t_point.transform(m_calib_matrix);
+							// m_ring_cloud[i].transform(m_calib_matrix);
+							if(t_point.ring != 8 || m_lidar_id != 1)
+							{
+								m_ring_cloud.push_back(t_point);
+							}
 						}
+						m_cloud_buf.clear();
 						m_parse_ring_time = std::chrono::steady_clock::now();
 					}
 					// LOG(INFO) << "33333";