Просмотр исходного кода

20200930, 新增将TensorFlow算法放在服务器上

huli 4 лет назад
Родитель
Сommit
d2ae1e6f28

+ 2 - 0
communication/communication_message.h

@@ -80,6 +80,8 @@ public:
 		eParkspace=0x0200,
 		//测量单元
 		eMeasurer=0x0300,
+		//测量单元
+		eMeasurer_sift_server=0x0301,
 		//调度机构
 		eDispatch=0x0400,
 		//...

+ 11 - 1
communication/communication_socket_base.h

@@ -5,7 +5,8 @@
  * 用户从这个基类继承, 初始化之后, 便可以自动进行通信
  * 重载解析消息和封装消息,
  *
- *
+ *Thread_safe_list<Binary_buf*> 使用 Binary_buf , 而不是string
+ * 主要是为了支持直接发送数字0
  * 
  * 
  * */
@@ -152,4 +153,13 @@ private:
 
 
 
+
+
+
+
+
+
+
+
+
 #endif //__COMMUNICATION_SOCKET_BASE__HH__

+ 12 - 2
error_code/error_code.cpp

@@ -108,23 +108,26 @@ void Error_manager::error_manager_clear_all()
     m_error_code = SUCCESS;
     m_error_level = NORMAL;
     free_description();
+	return;
 }
 
 //重载=
 Error_manager& Error_manager::operator=(const Error_manager & error_manager)
 {
     error_manager_reset(error_manager);
+	return *this;
 }
 //重载=,支持Error_manager和Error_code的直接转化,会清空错误等级和描述
 Error_manager& Error_manager::operator=(Error_code error_code)
 {
     error_manager_clear_all();
     set_error_code(error_code);
+	return *this;
 }
 //重载==
 bool Error_manager::operator==(const Error_manager & error_manager)
 {
-    is_equal_error_manager(error_manager);
+	return is_equal_error_manager(error_manager);
 }
 //重载==,支持Error_manager和Error_code的直接比较
 bool Error_manager::operator==(Error_code error_code)
@@ -142,7 +145,7 @@ bool Error_manager::operator==(Error_code error_code)
 //重载!=
 bool Error_manager::operator!=(const Error_manager & error_manager)
 {
-    ! is_equal_error_manager(error_manager);
+	return (! is_equal_error_manager(error_manager));
 }
 //重载!=,支持Error_manager和Error_code的直接比较
 bool Error_manager::operator!=(Error_code error_code)
@@ -310,6 +313,7 @@ void Error_manager::add_error_description(std::string & error_description_string
 
         reallocate_memory_and_copy_string(t_description_string);
     }
+	return;
 }
 
 //比较错误是否相同,
@@ -361,6 +365,7 @@ void Error_manager::compare_and_cover_error(const Error_manager & error_manager)
             add_error_description(pt_string_inside,t_string_inside_length);
         }
     }
+	return;
 }
 //比较并覆盖错误,讲低级错误转为字符串存放于描述中,
 //如果错误相同,则保留this的,将输入参数转入描述。
@@ -397,6 +402,7 @@ void Error_manager::compare_and_cover_error( Error_manager * p_error_manager)
 			add_error_description(pt_string_inside,t_string_inside_length);
 		}
 	}
+	return;
 }
 
 //将所有的错误信息,格式化为字符串,用作日志打印。
@@ -432,6 +438,7 @@ void Error_manager::translate_error_to_string(char* p_error_aggregate, int aggre
             pt_index_back++;
         }
     }
+	return;
 }
 //output:error_description_string     错误汇总的string
 void Error_manager::translate_error_to_string(std::string & error_aggregate_string)
@@ -448,6 +455,7 @@ void Error_manager::translate_error_to_string(std::string & error_aggregate_stri
     {
         //error_aggregate_string += "NULL";
     }
+	return;
 }
 //错误码转字符串的简易版,可支持cout<<
 //return     错误汇总的string
@@ -470,6 +478,7 @@ void Error_manager::free_description()
         pm_error_description = NULL;
     }
     m_description_length = 0;
+	return;
 }
 
 //重新分配错误描述的内存,并从外部拷贝新的(深拷贝)
@@ -529,6 +538,7 @@ void Error_manager::reallocate_memory_and_copy_string(std::string & error_aggreg
 
         strcpy(pm_error_description ,   error_aggregate_string.c_str()  );
     }
+	return;
 }
 
 

+ 18 - 1
error_code/error_code.h

@@ -53,7 +53,7 @@ enum Error_code
     WARNING                         = 0x00000003,//警告
     FAILED                          = 0x00000004,//失败
 
-    NODATA                         = 0x00000010,//没有数据,传入参数容器内部没有数据时,
+    NODATA                          = 0x00000010,//没有数据,传入参数容器内部没有数据时,
 	INVALID_MESSAGE					= 0x00000011, //无效的消息,
 	PARSE_FAILED					= 0x00000012,//解析失败
 
@@ -329,6 +329,23 @@ enum Error_code
     LOCATER_MSG_REQUEST_INVALID,
     LOCATER_MSG_RESPONSE_HAS_NO_REQUEST,
 
+	//Dispatch_manager 调度管理模块 错误码
+	DISPATCH_MANAGER_ERROR_BASE						= 0x13010000,
+	DISPATCH_MANAGER_READ_PROTOBUF_ERROR,				//调度管理模块,读取参数错误
+	DISPATCH_MANAGER_STATUS_BUSY,						//调度管理模块,状态正忙
+	DISPATCH_MANAGER_STATUS_ERROR,						//调度管理模块,状态错误
+	DISPATCH_MANAGER_TASK_TYPE_ERROR,					//调度管理模块,任务类型错误
+	DISPATCH_MANAGER_IS_NOT_READY,						//调度管理模块,不在准备状态
+
+	//snap7 通信模块 错误码
+	SNAP7_ERROR_BASE								= 0x1401000,
+	SNAP7_READ_PROTOBUF_ERROR,							//snap7通信模块,读取参数错误
+	SNAP7_CONNECT_ERROR,								//snap7通信模块,连接错误
+	SNAP7_DISCONNECT_ERROR,								//snap7通信模块,断连错误
+	SNAP7_READ_ERROR,									//snap7通信模块,读取错误
+	SNAP7_WRITE_ERROR,									//snap7通信模块,写入错误
+	SNAP7_ANALYSIS_TIME_OUT,									//解析超时,
+	SNAP7_EXCUTER_IS_BUSY,										//处理器正忙, 请稍等
 };
 
 //错误等级,用来做故障处理

+ 10 - 12
locate/locate_manager.cpp

@@ -52,13 +52,16 @@ Error_manager Locate_manager::Locate_manager_init_from_protobuf(std::string prot
 //初始化 定位 管理模块。从protobuf读取
 Error_manager Locate_manager::Locate_manager_init_from_protobuf(Measure::LocateParameter& locate_parameters)
 {
-	m_locate_manager_working_flag = false;
 
+	//huli huli huli test test test
+	/*
+	m_locate_manager_working_flag = false;
 	m_locate_manager_status = LOCATE_MANAGER_READY;
-	//启动雷达管理模块的内部线程。默认wait。
+//	启动雷达管理模块的内部线程。默认wait。
 	m_locate_manager_condition.reset(false, false, false);
 	mp_locate_manager_thread = new std::thread(&Locate_manager::thread_work, this);
 	return Error_code::SUCCESS;
+*/
 
 
 	LOG(INFO) << " ---Locate_manager_init_from_protobuf run--- "<< this;
@@ -173,7 +176,6 @@ Error_manager Locate_manager::execute_task(Task_Base* p_locate_task)
 	}
 
 	//检查接收方的状态
-	std::cout << "m_locate_manager_status"<<m_locate_manager_status << std::endl;
 	t_error = check_status();
 	if ( t_error != SUCCESS )
 	{
@@ -203,14 +205,6 @@ Error_manager Locate_manager::execute_task(Task_Base* p_locate_task)
 		}
 		else
 		{
-//			std::cout << "--------------------------------------" << std::endl;
-//
-//			std::cout << mp_locate_manager_task->get_task_point_cloud_map()->size() << std::endl;
-//			std::cout << mp_locate_manager_task->get_cloud_aggregation_flag() << std::endl;
-//			std::cout << "--------------------------------------" << std::endl;
-
-
-
 			//校验map的大小
 			if ( (mp_locate_manager_task->get_task_point_cloud_map()->size() ==1 && mp_locate_manager_task->get_cloud_aggregation_flag() == true)
 				 || ( (mp_locate_manager_task->get_task_point_cloud_map()->size() == CNN3D_WHEEL_NUMBER
@@ -373,6 +367,7 @@ Point_sift_segmentation* Locate_manager::get_point_sift()
 //thread_work 内部线程负责locate定位分析整车的信息,并且回收汇总雷达的数据
 void Locate_manager::thread_work()
 {
+
 	LOG(INFO) << " -------------------------mp_locate_manager_thread start "<< this;
 	Error_manager t_error;
 	Error_manager t_result;
@@ -380,6 +375,7 @@ void Locate_manager::thread_work()
 	//定位管理的独立线程,
 	while (m_locate_manager_condition.is_alive())
 	{
+
 		m_locate_manager_condition.wait();
 		if ( m_locate_manager_condition.is_alive() )
 		{
@@ -388,7 +384,7 @@ void Locate_manager::thread_work()
 			t_error.error_manager_clear_all();
 			t_result.error_manager_clear_all();
 
-
+			//检查任务单
 			if ( mp_locate_manager_task == NULL )
 			{
 				m_locate_manager_status = LOCATE_MANAGER_FAULT;
@@ -530,6 +526,8 @@ Error_manager Locate_manager::locate_manager_sift()
 	m_cloud_wheel_map.clear();
 	m_cloud_car_map.clear();
 
+
+
 	//遍历任务单的输入点云map
 	for (const auto &map_iter : *tp_task_point_cloud_map)
 	{

+ 1 - 1
locate/locate_manager.h

@@ -107,7 +107,7 @@ private:
     Point_sift_segmentation*                mp_point_sift;					//定位模块的点筛选分割, 内存本类管理
     Cnn3d_segmentation*                     mp_cnn3d;						//定位模块的cnn3d分割, 内存本类管理
 
-
+public:
 	Locate_manager_status						m_locate_manager_status;					//定位管理的工作状态
 	std::atomic<bool>                			m_locate_manager_working_flag;          	//定位管理的工作使能标志位
 	std::thread*        						mp_locate_manager_thread;   				//定位管理的线程指针,内存由本类管理

+ 41 - 10
locate/point_sift_segmentation.cpp

@@ -26,6 +26,21 @@ void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& clou
 	os.close();
 }
 
+void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>& cloud)
+{
+	std::ofstream os;
+	os.open(txt, std::ios::out);
+	for (int i = 0; i < cloud.points.size(); i++)
+	{
+		pcl::PointXYZ point = cloud.points[i];
+		char buf[255];
+		memset(buf, 0, 255);
+		sprintf(buf, "%f %f %f \n", point.x, point.y, point.z);
+		os.write(buf, strlen(buf));
+	}
+	os.close();
+}
+
 void save_rgb_cloud_txt(std::string txt, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
 {
 	std::ofstream os;
@@ -214,6 +229,10 @@ Error_manager Point_sift_segmentation::segmentation(int lidar_id, pcl::PointClou
 		 <<   microseconds::period::num << " " << microseconds::period::den   << "秒" << endl;
 */
 
+
+	std::string sift_in = save_dir + "/SIFT_cls_8192.txt";
+	save_rgb_cloud_txt(sift_in, *p_cloud_select);
+
 	//重写6和7步, 将TensorFlow算法放在服务器上, 使用socket通信去发送点云并获取结果
 	//第六步
 	//把pcl点云转化为float, 因为TensorFlow算法只能识别float
@@ -558,11 +577,16 @@ Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud<
 	    					" translate_cloud_to_float  p_data_out POINTER_IS_NULL ");
 	}
 
+	//将点云的中心平移到原点,
+	pcl::PointXYZ center;
+	center.x = (m_minp.x + m_maxp.x) / 2.0;
+	center.y = (m_minp.y + m_maxp.y) / 2.0;
+
 	for (int i = 0; i < m_point_num; ++i)
 	{
 		pcl::PointXYZ point = p_cloud_in->points[i];
-		*(p_data_out + i * 3) = point.x;
-		*(p_data_out + i * 3+1) = point.y;
+		*(p_data_out + i * 3) = point.x-center.x;
+		*(p_data_out + i * 3+1) = point.y-center.y;
 		*(p_data_out + i * 3+2) = point.z ;
 		//注:三维点云的数据 不用做缩放和平移,
 		// 这个在雷达扫描时就进行了标定和转化, 雷达传输过来的坐标就已经是公共坐标系了, (单位米)
@@ -677,12 +701,16 @@ Error_manager Point_sift_segmentation::recovery_float_to_cloud(int* p_data_type,
 		cloud_vector.push_back(cloud_rgb);
 	}
 
-	//遍历data数据,
+	pcl::PointXYZ center;
+	center.x = (m_minp.x + m_maxp.x) / 2.0;
+	center.y = (m_minp.y + m_maxp.y) / 2.0;
+
+	//将点云从原点移动到原处
 	for (int i = 0; i < m_point_num; ++i)
 	{
 		pcl::PointXYZRGB t_point;
-		t_point.x = *(p_data_point + i * 3);
-		t_point.y = *(p_data_point + i * 3 + 1);
+		t_point.x = *(p_data_point + i * 3)+center.x;
+		t_point.y = *(p_data_point + i * 3 + 1)+center.y;
 		t_point.z = *(p_data_point + i * 3 + 2);
 		if (t_point.x > m_maxp.x || t_point.x<m_minp.x
 			|| t_point.y>m_maxp.y || t_point.y<m_minp.y
@@ -701,20 +729,23 @@ Error_manager Point_sift_segmentation::recovery_float_to_cloud(int* p_data_type,
 				t_point.r = 0;
 				t_point.g = 255;
 				t_point.b = 0;
+				cloud_vector[0]->push_back(t_point);
 				break;
 			case 1://第1类, 白色, 车身
+//				t_point.r = 255;
+//				t_point.g = 255;
+//				t_point.b = 255;
+//				cloud_vector[t_cls]->push_back(t_point);
+				break;
+			case 2://第1类, 白色, 车身
 				t_point.r = 255;
 				t_point.g = 255;
 				t_point.b = 255;
-				break;
-//			case 2:
-//				;
-//				break;
+				cloud_vector[1]->push_back(t_point);
 			default:
 
 				break;
 		}
-		cloud_vector[t_cls]->push_back(t_point);
 	}
 
 	//校验点云的数量, 要求size>0

+ 191 - 0
locate/predict_with_network.cpp

@@ -0,0 +1,191 @@
+//
+// Created by huli on 2020/9/16.
+//
+
+#include "predict_with_network.h"
+#include "../system/system_executor.h"
+#include "../system/system_communication.h"
+
+Predict_with_network::Predict_with_network()
+{
+
+}
+
+Predict_with_network::~Predict_with_network()
+{
+	for (auto iter = m_locate_sift_request_msg_map.begin(); iter != m_locate_sift_request_msg_map.end(); ++iter)
+	{
+		delete(iter->second);
+	}
+	m_locate_sift_request_msg_map.clear();
+	for (auto iter = m_locate_sift_response_msg_map.begin(); iter != m_locate_sift_response_msg_map.end(); ++iter)
+	{
+		delete(iter->second);
+	}
+	m_locate_sift_response_msg_map.clear();
+}
+
+
+
+
+//预测, 发送socket通信, 在服务器上进行点云算法
+//data_in:输入数据,大小为  输入点数*3
+//data_out:输出数据,大小为  输入点数*类别数
+Error_manager Predict_with_network::predict_for_send(int data_id, float* data_in)
+{
+	if ( data_in == NULL )
+	{
+	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+	    					"  POINTER IS NULL ");
+	}
+	Error_manager t_error;
+	std::unique_lock<std::mutex> t_lock(m_message_lock);
+	//查重, 把原有的清空就好了.
+	if ( m_locate_sift_request_msg_map.find(data_id) != m_locate_sift_request_msg_map.end() )
+	{
+	    delete(m_locate_sift_request_msg_map[data_id]);
+		m_locate_sift_request_msg_map.erase(data_id);
+	}
+	if ( m_locate_sift_response_msg_map.find(data_id) != m_locate_sift_response_msg_map.end() )
+	{
+		delete(m_locate_sift_response_msg_map[data_id]);
+		m_locate_sift_response_msg_map.erase(data_id);
+	}
+
+
+	//封装一条消息,
+	message::Locate_sift_request_msg* tp_locate_sift_request_msg = new message::Locate_sift_request_msg;
+	tp_locate_sift_request_msg->mutable_base_info()->set_msg_type(message::Message_type::eLocate_sift_request_msg);
+	tp_locate_sift_request_msg->mutable_base_info()->set_timeout_ms(5000);
+	tp_locate_sift_request_msg->mutable_base_info()->set_sender(message::Communicator::eMeasurer);
+	tp_locate_sift_request_msg->mutable_base_info()->set_receiver(message::Communicator::eMeasurer_sift_server);
+
+	int t_terminal_id = System_executor::get_instance_references().get_terminal_id();
+
+	tp_locate_sift_request_msg->set_command_key("");
+	tp_locate_sift_request_msg->set_terminal_id(t_terminal_id);
+	tp_locate_sift_request_msg->set_lidar_id(data_id);
+
+	for (int i = 0; i < PREDICT_CLOUD_SIZE; ++i)
+	{
+		message::Cloud_coordinate* tp_cloud_coordinate = tp_locate_sift_request_msg->add_cloud_coordinates();
+		tp_cloud_coordinate->set_x(data_in[i*3+0]);
+		tp_cloud_coordinate->set_y(data_in[i*3+1]);
+		tp_cloud_coordinate->set_z(data_in[i*3+2]);
+	}
+	//发送消息
+	std::string t_msg = tp_locate_sift_request_msg->SerializeAsString();
+	t_error = System_communication::get_instance_references().encapsulate_msg(t_msg);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		delete(tp_locate_sift_request_msg);
+		return t_error;
+	}
+	else
+	{
+		//将发送消息保存到map
+		//内存的权限转交给map
+		m_locate_sift_request_msg_map[data_id] = tp_locate_sift_request_msg;
+	}
+
+	return Error_code::SUCCESS;
+}
+
+
+//预测, 接受预测结果, 算法时间较长, 请在外部循环接受. (方便中途放弃)
+//data_in:输入数据,大小为  输入点数*3
+//data_out:输出数据,大小为  输入点数*类别数
+//return:返回成功表示数据已经获取到了, 返回 NODATA 表示没有收到数据
+Error_manager Predict_with_network::predict_for_receive(int data_id, int* data_out)
+{
+	if ( data_out == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "  POINTER IS NULL ");
+	}
+	//查询指定的id
+	std::unique_lock<std::mutex> t_lock(m_message_lock);
+	if ( m_locate_sift_response_msg_map.find(data_id) != m_locate_sift_response_msg_map.end() )
+	{
+		if ( m_locate_sift_response_msg_map[data_id]->error_manager().error_code() == 0 )
+		{
+		    int t_cloud_size = m_locate_sift_response_msg_map[data_id]->cloud_type_size();
+		    if ( t_cloud_size == PREDICT_CLOUD_SIZE	 )
+		    {
+				for (int i = 0; i < PREDICT_CLOUD_SIZE; ++i)
+				{
+					data_out[i] = m_locate_sift_response_msg_map[data_id]->cloud_type(i).type();
+				}
+				return Error_code::SUCCESS;
+		    }
+			else
+			{
+				std::string t_string = m_locate_sift_response_msg_map[data_id]->error_manager().error_description();
+				return Error_manager(
+				(Error_code)(m_locate_sift_response_msg_map[data_id]->error_manager().error_code()),
+				(Error_level)(m_locate_sift_response_msg_map[data_id]->error_manager().error_level()),
+				t_string);
+			}
+		}
+		else
+		{
+			return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,"pointSIFT predict ERROR");
+		}
+	}
+	else
+	{
+		return Error_manager(Error_code::NODATA, Error_level::NEGLIGIBLE_ERROR,
+							" fun error ");
+	}
+
+
+	return Error_code::SUCCESS;
+}
+
+//签收预测的答复消息, 给通信模块调用, 将收到的预测答复消息, 存入 m_locate_sift_response_msg_map
+Error_manager Predict_with_network::execute_for_predict(std::string& message_string)
+{
+	message::Locate_sift_response_msg* tp_locate_sift_response_msg = new message::Locate_sift_response_msg;
+	//针对消息类型, 对消息进行二次解析
+	if (tp_locate_sift_response_msg->ParseFromString(message_string) )
+	{
+		std::unique_lock<std::mutex> t_lock(m_message_lock);
+		//对比请求和答复消息, 一致性
+		int index = tp_locate_sift_response_msg->lidar_id();
+		if ( m_locate_sift_request_msg_map.find(index) != m_locate_sift_request_msg_map.end())
+		{
+		    if ( m_locate_sift_request_msg_map[index]->command_key() == tp_locate_sift_response_msg->command_key() &&
+			m_locate_sift_request_msg_map[index]->terminal_id() == tp_locate_sift_response_msg->terminal_id() &&
+			m_locate_sift_request_msg_map[index]->lidar_id() == tp_locate_sift_response_msg->lidar_id() )
+		    {
+		        //校验成功, 直接存入 m_locate_sift_response_msg_map
+		        if ( m_locate_sift_response_msg_map.find(index) != m_locate_sift_response_msg_map.end() )
+		        {
+		            delete(m_locate_sift_response_msg_map[index]);
+		        }
+		        //内存的权限转交给map
+				m_locate_sift_response_msg_map[index] = tp_locate_sift_response_msg;
+
+				return Error_code::SUCCESS;
+		    }
+		}
+		else
+		{
+			delete(tp_locate_sift_response_msg);
+			return Error_manager(Error_code::INVALID_MESSAGE, Error_level::MINOR_ERROR,
+								 " Predict_with_network::execute_for_predict error ");
+		}
+	}
+	else
+	{
+		delete(tp_locate_sift_response_msg);
+		return Error_manager(Error_code::PARSE_FAILED, Error_level::MINOR_ERROR,
+							 " Predict_with_network::execute_for_predict error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+
+

+ 58 - 0
locate/predict_with_network.h

@@ -0,0 +1,58 @@
+//202009016新增需求
+//将深度学习的算法转到一个高配的服务器上运行.
+//每个测量程序节点, 通过socket通信, 将8192个点云上传, 返回8192个点的类型.
+
+#ifndef NNXX_TESTS_PREDICT_FROM_SERVER_H
+#define NNXX_TESTS_PREDICT_FROM_SERVER_H
+
+#include "../error_code/error_code.h"
+#include "../message/measure_message.pb.h"
+#include "../message/message_base.pb.h"
+
+#include <map>
+#include <mutex>
+
+//从服务器进行点云预测算法的功能模块
+class Predict_with_network
+{
+public:
+	//预测算法固定大小 8192
+#define PREDICT_CLOUD_SIZE			8192
+
+public:
+	Predict_with_network();
+	Predict_with_network(const Predict_with_network& other)= default;
+	Predict_with_network& operator =(const Predict_with_network& other)= default;
+	~Predict_with_network();
+public://API functions
+	//预测, 发送点云, 在服务器上进行点云算法
+	//data_in:输入数据,大小为  输入点数*3
+	//data_out:输出数据,大小为  输入点数*类别数
+	Error_manager predict_for_send(int data_id, float* data_in);
+	//预测, 接受预测结果, 算法时间较长, 请在外部循环接受. (方便中途放弃)
+	//data_in:输入数据,大小为  输入点数*3
+	//data_out:输出数据,大小为  输入点数*类别数
+	//return:返回成功表示数据已经获取到了, 返回 NODATA 表示没有收到数据
+	Error_manager predict_for_receive(int data_id, int* data_out);
+
+	//签收预测的答复消息, 给通信模块调用, 将收到的预测答复消息, 存入 m_locate_sift_response_msg_map
+	Error_manager execute_for_predict(std::string& message_string);
+
+public://get or set member variable
+
+protected://member functions
+
+protected://member variable
+
+	//通信的消息, 索引编号为数据编号, (可以近似于雷达的编号, 比如楚天的就是0~3)
+	std::mutex												m_message_lock;					//消息加锁
+	std::map<int, message::Locate_sift_request_msg*> 		m_locate_sift_request_msg_map;	//发送消息, 内存由本类管理
+	std::map<int, message::Locate_sift_response_msg*> 		m_locate_sift_response_msg_map;	//接受消息, 内存由本类管理
+
+
+private:
+
+};
+
+
+#endif //NNXX_TESTS_PREDICT_FROM_SERVER_H

+ 110 - 2
main.cpp

@@ -47,6 +47,7 @@ GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
 }
 
 
+
 int main(int argc,char* argv[])
 {
 
@@ -69,6 +70,8 @@ int main(int argc,char* argv[])
 
 	Wanji_manager::get_instance_references().wanji_manager_init();
 	std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;
+
+
 /*
 	while (1  )
 	{
@@ -165,8 +168,113 @@ int main(int argc,char* argv[])
 
 	System_communication::get_instance_references().communication_init();
 
-	char ch ;
-	std::cin >> ch ;
+	char ch;
+	while ( 1 )
+	{
+		std::cin >> ch ;
+
+		if ( ch == 'b' )
+		{
+			break;
+		}
+
+
+		//第一步, 创建点云缓存, 和 指定雷达,目前就一个
+		std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>		point_cloud_map;
+		std::mutex cloud_lock;
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+		std::ifstream is;
+		is.open("/home/huli/huli/Terminal_project/hl_measure/Terminal_process_CT/test_data/car_cloud/20200907-084334_src - Cloud.txt", std::ios::out);
+		char t_buf[256];
+		while ( !is.eof() )
+		{
+			is.getline(t_buf, 256);
+			pcl::PointXYZ t_point;
+			sscanf(t_buf, "%f %f %f", &t_point.x, &t_point.y, &t_point.z);
+			t_point.x = t_point.x / 1000.0;
+			t_point.y = t_point.y / 1000.0;
+			t_point.z = t_point.z / 1000.0;
+
+			p_cloud->push_back(t_point);
+		}
+		std::cout << " huli test :::: " << " p_cloud->size() = " << p_cloud->size() << std::endl;
+		point_cloud_map[0] = p_cloud;
+
+
+
+
+
+		Locate_manager_task locate_manager_task ;
+		Common_data::Car_measure_information t_car_information_by_livox;
+
+		//第五步, 创建定位模块的任务单, 并发送到 Locate_manager
+		locate_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
+									  true,"/home/huli/huli/Terminal_project/hl_measure/Terminal_process_CT/data",
+									  &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 ";
+		}
+		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();
+			if ( t_error == Error_code::SUCCESS )
+			{
+				t_car_information_by_livox.center_x = locate_manager_task.get_task_locate_information_ex()->locate_x;
+				t_car_information_by_livox.center_y = locate_manager_task.get_task_locate_information_ex()->locate_y;
+				t_car_information_by_livox.car_angle = locate_manager_task.get_task_locate_information_ex()->locate_angle;
+				t_car_information_by_livox.car_length = locate_manager_task.get_task_locate_information_ex()->locate_length;
+				t_car_information_by_livox.car_width = locate_manager_task.get_task_locate_information_ex()->locate_width;
+				t_car_information_by_livox.car_height = locate_manager_task.get_task_locate_information_ex()->locate_height;
+				t_car_information_by_livox.wheel_base = locate_manager_task.get_task_locate_information_ex()->locate_wheel_base;
+				t_car_information_by_livox.wheel_width = locate_manager_task.get_task_locate_information_ex()->locate_wheel_width;
+				t_car_information_by_livox.front_theta = 0;
+				t_car_information_by_livox.correctness = true;
+
+				std::cout << " huli test :::: " << " t_car_information_by_livox.center_x = " << t_car_information_by_livox.center_x << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.center_y = " << t_car_information_by_livox.center_y << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.car_angle = " << t_car_information_by_livox.car_angle << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.car_length = " << t_car_information_by_livox.car_length << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.car_width = " << t_car_information_by_livox.car_width << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.car_height = " << t_car_information_by_livox.car_height << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.wheel_base = " << t_car_information_by_livox.wheel_base << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.wheel_width = " << t_car_information_by_livox.wheel_width << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.front_theta = " << t_car_information_by_livox.front_theta << std::endl;
+				std::cout << " huli test :::: " << " t_car_information_by_livox.correctness = " << t_car_information_by_livox.correctness << std::endl;
+			}
+			else
+			{
+				LOG(INFO) << " locate_manager_task error :::::::"  << locate_manager_task.get_task_error_manager().to_string() ;
+			}
+		}
+
+	}
 
 	System_communication::get_instance_references().communication_uninit();
 	System_executor::get_instance_references().system_executor_uninit();

+ 49 - 44
message/message_base.pb.cc

@@ -345,54 +345,56 @@ void AddDescriptorsImpl() {
       "\001(\002\0223\n\020parkspace_status\030\010 \001(\0162\031.message."
       "Parkspace_status\022#\n\010car_info\030\t \001(\0132\021.mes"
       "sage.Car_info\022\022\n\nentry_time\030\n \001(\t\022\022\n\nlea"
-      "ve_time\030\013 \001(\t*\253\007\n\014Message_type\022\r\n\teBase_"
+      "ve_time\030\013 \001(\t*\350\007\n\014Message_type\022\r\n\teBase_"
       "msg\020\000\022\020\n\014eCommand_msg\020\001\022\026\n\022eLocate_statu"
       "s_msg\020\021\022\027\n\023eLocate_request_msg\020\022\022\030\n\024eLoc"
-      "ate_response_msg\020\023\022\030\n\024eDispatch_status_m"
-      "sg\020!\022\031\n\025eDispatch_request_msg\020\"\022\032\n\026eDisp"
-      "atch_response_msg\020#\022$\n eParkspace_alloca"
-      "tion_status_msg\0201\022%\n!eParkspace_allocati"
-      "on_request_msg\0202\022&\n\"eParkspace_allocatio"
-      "n_response_msg\0203\022!\n\035eParkspace_search_re"
-      "quest_msg\0204\022\"\n\036eParkspace_search_respons"
-      "e_msg\0205\022\"\n\036eParkspace_release_request_ms"
-      "g\0206\022#\n\037eParkspace_release_response_msg\0207"
-      "\022\'\n#eParkspace_force_update_request_msg\020"
-      "8\022(\n$eParkspace_force_update_response_ms"
-      "g\0209\022(\n$eParkspace_confirm_alloc_request_"
-      "msg\020:\022)\n%eParkspace_confirm_alloc_respon"
-      "se_msg\020;\022\036\n\032eStore_command_request_msg\020A"
-      "\022\037\n\033eStore_command_response_msg\020B\022\037\n\033ePi"
-      "ckup_command_request_msg\020C\022 \n\034ePickup_co"
-      "mmand_response_msg\020D\022\037\n\032eStoring_process"
-      "_statu_msg\020\220\001\022\037\n\032ePicking_process_statu_"
-      "msg\020\221\001\022\"\n\035eCentral_controller_statu_msg\020"
-      "\240\001\022#\n\036eEntrance_manual_operation_msg\020\260\001\022"
-      "\"\n\035eProcess_manual_operation_msg\020\261\001*f\n\014C"
-      "ommunicator\022\n\n\006eEmpty\020\000\022\t\n\005eMain\020\001\022\016\n\teT"
-      "erminor\020\200\002\022\017\n\neParkspace\020\200\004\022\016\n\teMeasurer"
-      "\020\200\006\022\016\n\teDispatch\020\200\010**\n\014Process_type\022\014\n\010e"
-      "Storing\020\001\022\014\n\010ePicking\020\002*e\n\013Error_level\022\n"
-      "\n\006NORMAL\020\000\022\024\n\020NEGLIGIBLE_ERROR\020\001\022\017\n\013MINO"
-      "R_ERROR\020\002\022\017\n\013MAJOR_ERROR\020\003\022\022\n\016CRITICAL_E"
-      "RROR\020\004*q\n\020Parkspace_status\022\024\n\020eParkspace"
-      "_empty\020\000\022\027\n\023eParkspace_occupied\020\001\022\030\n\024ePa"
-      "rkspace_reserverd\020\002\022\024\n\020eParkspace_error\020"
-      "\003*(\n\tDirection\022\014\n\010eForward\020\001\022\r\n\teBackwar"
-      "d\020\002*\335\002\n\tStep_type\022\017\n\013eAlloc_step\020\000\022\021\n\reM"
-      "easure_step\020\001\022\021\n\reCompare_step\020\002\022\022\n\016eDis"
-      "patch_step\020\003\022\021\n\reConfirm_step\020\004\022\020\n\014eSear"
-      "ch_step\020\005\022\016\n\neWait_step\020\006\022\021\n\reRelease_st"
-      "ep\020\007\022\r\n\teComplete\020\010\022\025\n\021eBackConfirm_step"
-      "\020\t\022\026\n\022eBack_compare_step\020\n\022\025\n\021eBackMeasu"
-      "re_step\020\013\022\023\n\017eBackAlloc_step\020\014\022\022\n\016eBackW"
-      "ait_step\020\r\022\026\n\022eBackDispatch_step\020\016\022\024\n\020eB"
-      "ackSearch_step\020\017\022\021\n\reBackComplete\020\020*C\n\nS"
-      "tep_statu\022\014\n\010eWaiting\020\000\022\014\n\010eWorking\020\001\022\n\n"
-      "\006eError\020\002\022\r\n\teFinished\020\003"
+      "ate_response_msg\020\023\022\034\n\030eLocate_sift_reque"
+      "st_msg\020\024\022\035\n\031eLocate_sift_response_msg\020\025\022"
+      "\030\n\024eDispatch_status_msg\020!\022\031\n\025eDispatch_r"
+      "equest_msg\020\"\022\032\n\026eDispatch_response_msg\020#"
+      "\022$\n eParkspace_allocation_status_msg\0201\022%"
+      "\n!eParkspace_allocation_request_msg\0202\022&\n"
+      "\"eParkspace_allocation_response_msg\0203\022!\n"
+      "\035eParkspace_search_request_msg\0204\022\"\n\036ePar"
+      "kspace_search_response_msg\0205\022\"\n\036eParkspa"
+      "ce_release_request_msg\0206\022#\n\037eParkspace_r"
+      "elease_response_msg\0207\022\'\n#eParkspace_forc"
+      "e_update_request_msg\0208\022(\n$eParkspace_for"
+      "ce_update_response_msg\0209\022(\n$eParkspace_c"
+      "onfirm_alloc_request_msg\020:\022)\n%eParkspace"
+      "_confirm_alloc_response_msg\020;\022\036\n\032eStore_"
+      "command_request_msg\020A\022\037\n\033eStore_command_"
+      "response_msg\020B\022\037\n\033ePickup_command_reques"
+      "t_msg\020C\022 \n\034ePickup_command_response_msg\020"
+      "D\022\037\n\032eStoring_process_statu_msg\020\220\001\022\037\n\032eP"
+      "icking_process_statu_msg\020\221\001\022\"\n\035eCentral_"
+      "controller_statu_msg\020\240\001\022#\n\036eEntrance_man"
+      "ual_operation_msg\020\260\001\022\"\n\035eProcess_manual_"
+      "operation_msg\020\261\001*\202\001\n\014Communicator\022\n\n\006eEm"
+      "pty\020\000\022\t\n\005eMain\020\001\022\016\n\teTerminor\020\200\002\022\017\n\nePar"
+      "kspace\020\200\004\022\016\n\teMeasurer\020\200\006\022\032\n\025eMeasurer_s"
+      "ift_server\020\201\006\022\016\n\teDispatch\020\200\010**\n\014Process"
+      "_type\022\014\n\010eStoring\020\001\022\014\n\010ePicking\020\002*e\n\013Err"
+      "or_level\022\n\n\006NORMAL\020\000\022\024\n\020NEGLIGIBLE_ERROR"
+      "\020\001\022\017\n\013MINOR_ERROR\020\002\022\017\n\013MAJOR_ERROR\020\003\022\022\n\016"
+      "CRITICAL_ERROR\020\004*q\n\020Parkspace_status\022\024\n\020"
+      "eParkspace_empty\020\000\022\027\n\023eParkspace_occupie"
+      "d\020\001\022\030\n\024eParkspace_reserverd\020\002\022\024\n\020eParksp"
+      "ace_error\020\003*(\n\tDirection\022\014\n\010eForward\020\001\022\r"
+      "\n\teBackward\020\002*\335\002\n\tStep_type\022\017\n\013eAlloc_st"
+      "ep\020\000\022\021\n\reMeasure_step\020\001\022\021\n\reCompare_step"
+      "\020\002\022\022\n\016eDispatch_step\020\003\022\021\n\reConfirm_step\020"
+      "\004\022\020\n\014eSearch_step\020\005\022\016\n\neWait_step\020\006\022\021\n\re"
+      "Release_step\020\007\022\r\n\teComplete\020\010\022\025\n\021eBackCo"
+      "nfirm_step\020\t\022\026\n\022eBack_compare_step\020\n\022\025\n\021"
+      "eBackMeasure_step\020\013\022\023\n\017eBackAlloc_step\020\014"
+      "\022\022\n\016eBackWait_step\020\r\022\026\n\022eBackDispatch_st"
+      "ep\020\016\022\024\n\020eBackSearch_step\020\017\022\021\n\reBackCompl"
+      "ete\020\020*C\n\nStep_statu\022\014\n\010eWaiting\020\000\022\014\n\010eWo"
+      "rking\020\001\022\n\n\006eError\020\002\022\r\n\teFinished\020\003"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 2744);
+      descriptor, 2834);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "message_base.proto", &protobuf_RegisterTypes);
 }
@@ -420,6 +422,8 @@ bool Message_type_IsValid(int value) {
     case 17:
     case 18:
     case 19:
+    case 20:
+    case 21:
     case 33:
     case 34:
     case 35:
@@ -460,6 +464,7 @@ bool Communicator_IsValid(int value) {
     case 256:
     case 512:
     case 768:
+    case 769:
     case 1024:
       return true;
     default:

+ 3 - 0
message/message_base.pb.h

@@ -92,6 +92,8 @@ enum Message_type {
   eLocate_status_msg = 17,
   eLocate_request_msg = 18,
   eLocate_response_msg = 19,
+  eLocate_sift_request_msg = 20,
+  eLocate_sift_response_msg = 21,
   eDispatch_status_msg = 33,
   eDispatch_request_msg = 34,
   eDispatch_response_msg = 35,
@@ -137,6 +139,7 @@ enum Communicator {
   eTerminor = 256,
   eParkspace = 512,
   eMeasurer = 768,
+  eMeasurer_sift_server = 769,
   eDispatch = 1024
 };
 bool Communicator_IsValid(int value);

+ 2 - 0
message/message_base.proto

@@ -63,6 +63,8 @@ enum Communicator
     eParkspace=0x0200;
     //测量单元
     eMeasurer=0x0300;
+    //测量单元
+    eMeasurer_sift_server=0x0301;
     //调度机构
     eDispatch=0x0400;
     //...

+ 1 - 1
proto.sh

@@ -1,6 +1,6 @@
 protoc -I=./message message_base.proto --cpp_out=./message
 protoc -I=./message measure_message.proto --cpp_out=./message
-#protoc -I=./message hardware_message.proto --cpp_out=./message
+
 protoc -I=./locate locate_parameter.proto --cpp_out=./locate
 protoc -I=./laser laser_parameter.proto --cpp_out=./laser
 protoc -I=./laser laser_message.proto --cpp_out=./laser

+ 2 - 2
setting/communication.prototxt

@@ -19,10 +19,10 @@ communication_parameters
 
 
   # connect_string_vector:"tcp://192.168.2.183:30002"
- #  connect_string_vector:"tcp://192.168.2.125:7001"
+   connect_string_vector:"tcp://192.168.2.125:7001"
 
 
-   connect_string_vector:"tcp://192.168.2.233:2333"
+#   connect_string_vector:"tcp://192.168.2.233:2333"
 
 }
 

+ 19 - 12
setting/locate.prototxt

@@ -1,10 +1,17 @@
 area{
-    x_min:-1227.5
-    x_max:1802.9
-    y_min:-2789.8
-    y_max:3777.19
-    z_min:0
-    z_max:1800
+ #   x_min:-1227.5
+ #   x_max:1802.9
+ #   y_min:-2789.8
+ #   y_max:3777.19
+  #  z_min:0
+  #  z_max:1800
+
+        x_min:-1000000
+        x_max:1000000
+        y_min:-1000000
+        y_max:1000000
+        z_min:-1000000
+        z_max:1000000
 }
 
 net_3dcnn_parameter
@@ -24,12 +31,12 @@ seg_parameter
     freq:0.020
     area
     {
-        x_min:-10000.0
-    	x_max:10000.0
-    	y_min:-10000.0
-    	y_max:10000.0
-    	z_min:-10000.0
-    	z_max:10000.0
+        x_min:-100000.0
+    	x_max:100000.0
+    	y_min:-100000.0
+    	y_max:100000.0
+    	z_min:-100000.0
+    	z_max:100000.0
     }
     graph:"../model_param/seg_model_404500.ckpt.meta"
     cpkt:"../model_param/seg_model_404500.ckpt"

+ 3 - 3
system/system_executor.cpp

@@ -214,7 +214,7 @@ Error_manager System_executor::execute_msg(Communication_message* p_msg)
 			}
 			break;
 		}
-		case Communication_message::eLocate_sift_request_msg:
+		case Communication_message::eLocate_sift_response_msg:
 		{
 			//往线程池添加执行任务, 之后会唤醒一个线程去执行他.
 			m_thread_pool.enqueue(&System_executor::execute_for_predict, this,
@@ -320,8 +320,8 @@ Error_manager System_executor::encapsulate_send_status()
 	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_error.get_description_length());
 
-	std::cout << " huli test :::: " << " t_measure_status_msg.DebugString() = "  << std::endl;
-	std::cout << t_measure_status_msg.DebugString() << std::endl;
+//	std::cout << " huli test :::: " << " t_measure_status_msg.DebugString() = "  << std::endl;
+//	std::cout << t_measure_status_msg.DebugString() << std::endl;
 
 
 	string t_msg = t_measure_status_msg.SerializeAsString();