20200722, 联调通信

locate/cnn3d_segmentation.cpp

@@ -113,6 +113,7 @@ void save_cloudT(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& pCloud)
 	os.close();
 }
 
+//解析4个轮胎
 Error_manager Cnn3d_segmentation::analytic_wheel(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map, bool cloud_aggregation_flag,
 												 Locate_information* p_locate_information,
 												 bool save_flag, std::string save_dir)

locate/locate_manager.cpp

@@ -594,32 +594,43 @@ Error_manager Locate_manager::locate_manager_locate_car()
 			return t_error;
 		}
 
-		//比较选出整个map点云的边界
-		if ( t_point3d_min.x <= min.x )
+		if ( iter == m_cloud_car_map.begin() )
 		{
 			t_point3d_min.x = min.x;
-		}
-		if ( t_point3d_min.y <= min.y )
-		{
 			t_point3d_min.y = min.y;
-		}
-		if ( t_point3d_min.z <= min.z )
-		{
 			t_point3d_min.z = min.z;
-		}
-		if ( t_point3d_max.x >= max.x )
-		{
 			t_point3d_max.x = max.x;
-		}
-		if ( t_point3d_max.y >= max.y )
-		{
 			t_point3d_max.y = max.y;
+			t_point3d_max.z = max.z;
 		}
-		if ( t_point3d_max.z >= max.z )
+		else
 		{
-			t_point3d_max.z = max.z;
+			//比较选出整个map点云的边界
+			if ( t_point3d_min.x <= min.x )
+			{
+				t_point3d_min.x = min.x;
+			}
+			if ( t_point3d_min.y <= min.y )
+			{
+				t_point3d_min.y = min.y;
+			}
+			if ( t_point3d_min.z <= min.z )
+			{
+				t_point3d_min.z = min.z;
+			}
+			if ( t_point3d_max.x >= max.x )
+			{
+				t_point3d_max.x = max.x;
+			}
+			if ( t_point3d_max.y >= max.y )
+			{
+				t_point3d_max.y = max.y;
+			}
+			if ( t_point3d_max.z >= max.z )
+			{
+				t_point3d_max.z = max.z;
+			}
 		}
-
 	}
 	mp_locate_information->locate_length = t_point3d_max.y - t_point3d_min.y;
 	mp_locate_information->locate_width = t_point3d_max.x - t_point3d_min.x;;

main.cpp

@@ -21,14 +21,45 @@
 
 #define LIVOX_NUMBER	     2
 
-
-void asd(int i)
+GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
 {
-	std::cout << " i = " << i << std::endl;
+	time_t tt;
+	time( &tt );
+	tt = tt + 8*3600;  // transform the time zone
+	tm* t= gmtime( &tt );
+	char buf[255]={0};
+	sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
+			t->tm_year + 1900,
+			t->tm_mon + 1,
+			t->tm_mday,
+			t->tm_hour,
+			t->tm_min,
+			t->tm_sec);
+
+	FILE* tp_file=fopen(buf,"w");
+	fprintf(tp_file,data,strlen(data));
+	fclose(tp_file);
+
 }
 
 int main(int argc,char* argv[])
 {
+
+	const char* logPath = "./";
+	google::InitGoogleLogging("LidarMeasurement");
+	google::SetStderrLogging(google::INFO);
+	google::SetLogDestination(0, logPath);
+	google::SetLogFilenameExtension("zxlog");
+	google::InstallFailureSignalHandler();
+	google::InstallFailureWriter(&shut_down_logging);
+	FLAGS_colorlogtostderr = true;        // Set log color
+	FLAGS_logbufsecs = 0;                // Set log output speed(s)
+	FLAGS_max_log_size = 1024;            // Set max log file size(GB)
+	FLAGS_stop_logging_if_full_disk = true;
+
+
+
+
 	Laser_manager::get_instance_references().laser_manager_init();
 	std::cout << "Laser_manager = " << Laser_manager::get_instance_references().get_laser_manager_status() << std::endl;
 	Locate_manager::get_instance_references().Locate_manager_init();
@@ -48,28 +79,6 @@ int main(int argc,char* argv[])
 
 	return 0;
 
-	Thread_pool pool(4);
-	std::vector< std::future<int> > results;
-
-//	for(int i = 0; i < 8; ++i) {
-//		results.emplace_back(
-//		pool.enqueue([i] {
-//			std::cout << "hello " << i << std::endl;
-//			std::this_thread::sleep_for(std::chrono::seconds(1));
-//			std::cout << "world " << i << std::endl;
-//			return i*i;
-//		})
-//		);
-//	}
-
-	pool.enqueue(&asd,8);
-
-	for(auto && result: results)
-		std::cout << result.get() << ' ';
-	std::cout << std::endl;
-	return 0;
-
-
 
 	LOG(INFO) << " --- main start --- " ;
 

setting/communication.prototxt

@@ -10,7 +10,7 @@ communication_parameters
   # connect_string_vector:"tcp://192.168.2.125:9876"
   # connect_string_vector:"tcp://192.168.2.166:1234"
 
-   bind_string:"tcp://192.168.2.166:4444"
+   bind_string:"tcp://192.168.2.192:4444"
 
 
 }

setting/laser.prototxt

@@ -1,5 +1,5 @@
 #1号雷达
-#  0TFDFG700601881  0TFDFCE00502001
+#  0TFDFG700601881  0TFDFCE00502001 0TFDH5L00684HE1
 
 
 
@@ -7,7 +7,7 @@
 laser_parameters
 {
 	type:"Livox"
-	sn:"0TFDFG700601881"
+	sn:"0TFDH5L00684HE1"
 	frame_num:1000
     mat_r00:1
     mat_r01:0

system/system_communication.cpp

@@ -70,7 +70,7 @@ Error_manager System_communication::execute_msg(Communication_message* p_msg)
 Error_manager System_communication::encapsulate_send_data()
 {
 	Error_manager t_error;
-
+/*
 	message::Measure_request_msg t_measure_request_msg;
 	t_measure_request_msg.mutable_base_info()->set_msg_type(message::Message_type::eLocate_request_msg);
 	t_measure_request_msg.mutable_base_info()->set_timeout_ms(5000);
@@ -80,7 +80,10 @@ Error_manager System_communication::encapsulate_send_data()
 	t_measure_request_msg.set_terminal_id(1);
 	string t_msg = t_measure_request_msg.SerializeAsString();
 	System_communication::get_instance_references().encapsulate_msg(t_msg);
-	/*
+ */
+
+
+
 	//创建一条状态消息
 	message::Measure_status_msg t_measure_status_msg;
 	t_measure_status_msg.mutable_base_info()->set_msg_type(message::Message_type::eLocate_status_msg);
@@ -103,7 +106,7 @@ Error_manager System_communication::encapsulate_send_data()
 
 	t_measure_status_msg.mutable_error_manager()->set_error_code(t_error.get_error_code());
 	t_measure_status_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
-	t_measure_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description());
+	t_measure_status_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length());
 
 	t_measure_status_msg.mutable_locate_information_realtime()->set_locate_x(0);
 	t_measure_status_msg.mutable_locate_information_realtime()->set_locate_y(0);
@@ -117,7 +120,7 @@ Error_manager System_communication::encapsulate_send_data()
 
 	string t_msg = t_measure_status_msg.SerializeAsString();
 	System_communication::get_instance_references().encapsulate_msg(t_msg);
-*/
+
 	return Error_code::SUCCESS;
 }
 

system/system_executor.cpp

@@ -199,9 +199,111 @@ void System_executor::execute_for_measure(int command_id, int terminal_id)
 	Error_manager t_error;
 
 	LOG(INFO) << " System_executor::execute_for_measure run "<< this;
-	//这里要处理.......以后再写
 
-	//创建一条答复消息
+	//第一步, 创建点云缓存, 和 指定雷达,目前就一个
+	std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>		point_cloud_map;
+	std::mutex cloud_lock;
+	std::vector<int> select_laser_id_vector;
+	select_laser_id_vector.push_back(0);
+
+	//第二步, 按照时间生成中间文件的保存路径
+	time_t nowTime;
+	nowTime = time(NULL);
+	struct tm* sysTime = localtime(&nowTime);
+	char t_save_path[256] = { 0 };
+	sprintf(t_save_path, "../data/%04d%02d%02d_%02d%02d%02d",
+			sysTime->tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec);
+
+
+	//第三步, 创建雷达管理模块的任务单, 并发送到 laser_manager
+	Laser_manager_task  laser_manager_task ;
+	laser_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
+								  true,t_save_path,&cloud_lock,&point_cloud_map,false,
+								  1000,false,select_laser_id_vector	);
+	t_error = Task_command_manager::get_instance_references().execute_task(&laser_manager_task);
+//			tp_laser_manager->execute_task(laser_manager_task);
+
+
+	//第四步, 等待任务单完成
+	if ( t_error != Error_code::SUCCESS )
+	{
+		LOG(INFO) << " Laser_manager  execute_task   error "<< this;
+	}
+	else
+	{
+		//判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
+		while ( laser_manager_task.is_task_end() == false)
+		{
+			if ( laser_manager_task.is_over_time() )
+			{
+				//超时处理。取消任务。
+				Laser_manager::get_instance_pointer()->cancel_task();
+				laser_manager_task.set_task_statu(TASK_DEAD);
+				t_error.error_manager_reset(Error_code::LASER_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
+											" laser_manager_task is_over_time ");
+				laser_manager_task.set_task_error_manager(t_error);
+			}
+			else
+			{
+				//继续等待
+				std::this_thread::sleep_for(std::chrono::microseconds(1));
+				std::this_thread::yield();
+			}
+		}
+
+		//提取任务单 的错误码
+		t_error = laser_manager_task.get_task_error_manager();
+		std::cout << "huli laser_manager_task error :::::::"  << laser_manager_task.get_task_error_manager().to_string() << std::endl;
+	}
+
+
+	Locate_manager_task locate_manager_task ;
+	//第五步, 创建定位模块的任务单, 并发送到 Locate_manager
+	if ( t_error != Error_code::SUCCESS )
+	{
+	    LOG(INFO) << " laser_manager_task error "<< this;
+	}
+	else
+	{
+
+		locate_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
+									   true,t_save_path,&cloud_lock,&point_cloud_map,true	);
+		t_error = Task_command_manager::get_instance_references().execute_task(&locate_manager_task);
+
+		//第六步, 等待任务单完成
+		if ( t_error != Error_code::SUCCESS )
+		{
+			LOG(INFO) << " Locate_manager  execute_task error "<< this;
+		}
+		else
+		{
+			//判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
+			while ( locate_manager_task.is_task_end() == false)
+			{
+				if ( locate_manager_task.is_over_time() )
+				{
+					//超时处理。取消任务。
+					Locate_manager::get_instance_pointer()->cancel_task();
+					locate_manager_task.set_task_statu(TASK_DEAD);
+					t_error.error_manager_reset(Error_code::LOCATE_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
+												" locate_manager_task is_over_time ");
+					locate_manager_task.set_task_error_manager(t_error);
+				}
+				else
+				{
+					//继续等待
+					std::this_thread::sleep_for(std::chrono::microseconds(1));
+					std::this_thread::yield();
+				}
+
+			}
+			//提取任务单 的错误码
+			t_error = locate_manager_task.get_task_error_manager();
+			std::cout << "huli locate_manager_task error :::::::"  << locate_manager_task.get_task_error_manager().to_string() << std::endl;
+		}
+	}
+
+	//第七步, 创建一条答复消息
 	message::Measure_response_msg t_measure_response_msg;
 	t_measure_response_msg.mutable_base_info()->set_msg_type(message::Message_type::eLocate_response_msg);
 	t_measure_response_msg.mutable_base_info()->set_timeout_ms(5000);
@@ -214,17 +316,24 @@ void System_executor::execute_for_measure(int command_id, int terminal_id)
 	t_measure_response_msg.mutable_error_manager()->set_error_level((message::Error_level)t_error.get_error_level());
 	t_measure_response_msg.mutable_error_manager()->set_error_description(t_error.get_error_description(), t_error.get_description_length());
 
-	t_measure_response_msg.mutable_locate_information()->set_locate_x(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_y(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_angle(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_length(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_width(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_height(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_wheel_base(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_wheel_width(0);
-	t_measure_response_msg.mutable_locate_information()->set_locate_correct(0);
+	if ( t_error == Error_code::SUCCESS )
+	{
+		t_measure_response_msg.mutable_locate_information()->set_locate_x(locate_manager_task.get_task_locate_information_ex()->locate_x);
+		t_measure_response_msg.mutable_locate_information()->set_locate_y(locate_manager_task.get_task_locate_information_ex()->locate_y);
+		t_measure_response_msg.mutable_locate_information()->set_locate_angle(locate_manager_task.get_task_locate_information_ex()->locate_angle);
+		t_measure_response_msg.mutable_locate_information()->set_locate_length(locate_manager_task.get_task_locate_information_ex()->locate_length);
+		t_measure_response_msg.mutable_locate_information()->set_locate_width(locate_manager_task.get_task_locate_information_ex()->locate_width);
+		t_measure_response_msg.mutable_locate_information()->set_locate_height(locate_manager_task.get_task_locate_information_ex()->locate_height);
+		t_measure_response_msg.mutable_locate_information()->set_locate_wheel_base(locate_manager_task.get_task_locate_information_ex()->locate_wheel_base);
+		t_measure_response_msg.mutable_locate_information()->set_locate_wheel_width(locate_manager_task.get_task_locate_information_ex()->locate_wheel_width);
+		t_measure_response_msg.mutable_locate_information()->set_locate_correct(locate_manager_task.get_task_locate_information_ex()->locate_correct);
+	}
 
 	string t_msg = t_measure_response_msg.SerializeAsString();
 	System_communication::get_instance_references().encapsulate_msg(t_msg);
+
+	std::cout << "huli t_measure_response_msg = " << t_measure_response_msg.DebugString() << std::endl;
+
+
 	return ;
 }