20210317, 万集雷达在普爱测试后,修正bug

laser/LivoxLaser.cpp

@@ -15,7 +15,7 @@ CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
 	m_frame_maxnum = laser_param.frame_num();//初始化从外部参数导入,实际运行前从任务单导入.
 
 	//默认handle为-1,切记不能初始化为0.因为0表示0号雷达,是有效的.....
-	m_livox_device.handle = -1;
+	m_livox_device.handle = 255;
 
 	//判断参数类型，
 	if(laser_param.type()=="Livox")

laser/laser_manager.cpp

@@ -78,6 +78,7 @@ Error_manager Laser_manager::laser_manager_init_from_protobuf(Laser_proto::Laser
 		}
 	}
 
+
 	m_laser_manager_status = LASER_MANAGER_READY;
 
 	//启动雷达管理模块的内部线程。默认wait。

laser/livox_driver.cpp

@@ -75,6 +75,10 @@ Error_manager Livox_driver::Livox_driver_init(bool aoto_create_livox_flag)
 		}
 	}
 
+
+	std::cout << " huli test :::: " << " m_sn_laser_map.size() = " << m_sn_laser_map.size() << std::endl;
+	std::cout << " huli test :::: " << " m_handle_laser_map.size() = " << m_handle_laser_map.size() << std::endl;
+
 	return Error_code::SUCCESS;
 }
 
@@ -225,6 +229,10 @@ void Livox_driver::livox_device_broadcast_callback(const BroadcastDeviceInfo *p_
 					tp_livox_laser->set_device_state( CLivoxLaser::K_DEVICE_STATE_FAULT);
 				}
 			}
+			else
+			{
+
+			}
 		}
 	}
 }

main.cpp

@@ -68,11 +68,12 @@ int main(int argc,char* argv[])
 	FLAGS_max_log_size = 1024;            // Set max log file size(GB)
 	FLAGS_stop_logging_if_full_disk = true;
 
+
+
+
 	Wanji_manager::get_instance_references().wanji_manager_init();
 	std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;
 
-
-/*
 	while (1  )
 	{
 		char ch1 ;
@@ -135,7 +136,6 @@ int main(int argc,char* argv[])
 	return 0;
 
 
-*/
 
 
 
@@ -150,6 +150,7 @@ int main(int argc,char* argv[])
 
 
 
+/*
 	int t_terminal_id = 0;
 	if ( argc == 2 )
 	{
@@ -286,16 +287,18 @@ int main(int argc,char* argv[])
 
 	LOG(INFO) << " --- main start --- " ;
 
+*/
 
 
-
-
+	Error_manager t_code;
 
 	std::cout << "huli 101 main start!" << std::endl;
 
 	std::cout << "1111111111111111111111" << std::endl;
-	Laser_manager::get_instance_pointer()->laser_manager_init();
-	Locate_manager::get_instance_pointer()->Locate_manager_init();
+	t_code = Laser_manager::get_instance_pointer()->laser_manager_init();
+	std::cout << " huli test :::: " << " t_code = " << t_code << std::endl;
+//	t_code = Locate_manager::get_instance_pointer()->Locate_manager_init();
+//	std::cout << " huli test :::: " << " t_code = " << t_code << std::endl;
 
 	Point_sift_segmentation*                mp_point_sift;
 	int point_size = 8192;
@@ -308,7 +311,6 @@ int main(int argc,char* argv[])
 
 
 
-
 	while(1)
 	{
 

wanji_lidar/region_detect.cpp

@@ -34,7 +34,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	// 1.参数合法性检验
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
-	std::cout << " huli test :::: " << " 222 = " << 222 << std::endl;
+//	std::cout << " huli test :::: " << " 222 = " << 222 << std::endl;
 	// 2.直通滤波, 筛选xy
 	pcl::PassThrough<pcl::PointXYZ> pass;
 	pass.setInputCloud(p_cloud);
@@ -42,7 +42,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	pass.setFilterLimits(m_region_box.x_min, m_region_box.x_max); //将x轴的0到1范围内
 	pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
 	pass.filter(*p_cloud);                                            //输出到结果指针
-	std::cout << " huli test :::: " << " 333 = " << 333 << std::endl;
+//	std::cout << " huli test :::: " << " 333 = " << 333 << std::endl;
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 
@@ -51,10 +51,10 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	pass.setFilterLimits(m_region_box.y_min, m_region_box.y_max); //将x轴的0到1范围内
 	pass.setFilterLimitsNegative(false);                                //保留（true就是删除，false就是保留而删除此区间外的）
 	pass.filter(*p_cloud);                                            //输出到结果指针
-	std::cout << " huli test :::: " << " 444 = " << 444 << std::endl;
+//	std::cout << " huli test :::: " << " 444 = " << 444 << std::endl;
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
-	std::cout << " huli test :::: " << " 445 = " << 445 << std::endl;
+//	std::cout << " huli test :::: " << " 445 = " << 445 << std::endl;
 	// 3.离群点过滤
 	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
 	sor.setInputCloud(p_cloud);
@@ -62,7 +62,7 @@ Error_manager Region_detector::preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 	sor.setStddevMulThresh(3.0); //标准差倍数
 	sor.setNegative(false);      //保留未滤波点（内点）
 	sor.filter(*p_cloud);      //保存滤波结果到cloud_filter
-	std::cout << " huli test :::: " << " 555 = " << 555 << std::endl;
+//	std::cout << " huli test :::: " << " 555 = " << 555 << std::endl;
 	if (p_cloud->size() <= 0)
 		return Error_manager(Error_code::WJ_REGION_EMPTY_CLOUD);
 	else

wanji_lidar/wanji_lidar_device.cpp

@@ -57,7 +57,7 @@ Error_manager Wanji_lidar_device::init(wj::wjLidarParams params)
 	std::string	t_ip = params.net_config().ip_address();
 	int t_port = params.net_config().port();
 
-	t_error = m_async_client.init("192.168.0.208", 2110, &m_communication_data_queue);
+	t_error = m_async_client.init(t_ip, t_port, &m_communication_data_queue);
 	if ( t_error != SUCCESS )
 	{
 		return t_error;
@@ -358,6 +358,7 @@ Error_manager Wanji_lidar_device::get_last_cloud(pcl::PointCloud<pcl::PointXYZ>:
 	if( m_protocol.get_scan_time() > command_time - std::chrono::milliseconds(t_scan_cycle_ms) )
 	{
 		Error_manager code = m_protocol.get_scan_points(p_cloud);
+//		std::cout << " huli test :::: " << " p_cloud.size() = " << p_cloud->size() << std::endl;
 		return code;
 	}
 	else
@@ -420,7 +421,6 @@ void Wanji_lidar_device::execute_thread_fun()
 		if ( m_execute_condition.is_alive() )
 		{
 			std::this_thread::yield();
-
 			if ( mp_wanji_lidar_scan_task == NULL )
 			{
 				//没有任务就忽略, 直接关闭线程

wanji_lidar/wanji_manager.cpp

@@ -73,7 +73,7 @@ Error_manager Wanji_manager::wanji_manager_init_from_protobuf(wj::wjManagerParam
 		t_point2d_box.x_min = wanji_parameters.regions(i).minx();
 		t_point2d_box.x_max = wanji_parameters.regions(i).maxx();
 		t_point2d_box.y_min = wanji_parameters.regions(i).miny();
-		t_point2d_box.y_min = wanji_parameters.regions(i).maxy();
+		t_point2d_box.y_max = wanji_parameters.regions(i).maxy();
 		t_error = p_region_worker->init(t_point2d_box, &m_cloud_collection_mutex, mp_cloud_collection);
 		if ( t_error != Error_code::SUCCESS )
 		{
@@ -347,6 +347,8 @@ void Wanji_manager::collect_cloud_thread_fun()
 					//失败则关闭预测算法
 					stop_auto_detect();
 				}
+
+				t_result.error_manager_clear_all();
 			}
 		}
 	}
@@ -403,7 +405,8 @@ void Wanji_manager::execute_thread_fun()
 			//目前楚天是唯一的预测算法, 如果后续有多个出入口,则使用任务单的终端id来获取指定的.
 			if ( m_region_worker_map.size() > 0 )
 			{
-				t_error = m_region_worker_map[0]->get_current_wheel_information(&t_car_wheel_information, mp_wanji_manager_task->get_command_time());
+				int t_terminal = mp_wanji_manager_task->get_terminal_id();
+				t_error = m_region_worker_map[t_terminal]->get_current_wheel_information(&t_car_wheel_information, mp_wanji_manager_task->get_command_time());
 				if ( t_error == Error_code::SUCCESS )
 				{
 					//添加车轮的预测结果