浏览代码

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

huli 4 年之前
父节点
当前提交
b5a6ff3b72

+ 49 - 9
communication/communication_message.h

@@ -25,25 +25,65 @@ public:
 		eBase_msg=0x00,
 		eCommand_msg=0x01,                      //指令消息
 
+
 		eLocate_status_msg=0x11,                //定位模块状态消息
 		eLocate_request_msg=0x12,               //定位请求消息
 		eLocate_response_msg=0x13,              //定位反馈消息
 
-		eHarware_statu_msg=0x21,                //调度模块硬件状态消息
-		eExecute_request_msg=0x22,              //请求调度消息
-		eExecute_response_msg=0x23,             //调度结果反馈消息
+		eLocate_sift_request_msg = 0x14,            //预测算法请求消息
+		eLocate_sift_response_msg = 0x15,           //预测算法反馈消息
+
+		eDispatch_status_msg=0x21,                //调度模块硬件状态消息
+		eDispatch_request_msg=0x22,              //请求调度消息
+		eDispatch_response_msg=0x23,             //调度结果反馈消息
+
+		eParkspace_allocation_status_msg=0x31,  //车位分配模块状态消息,包括车位信息
+		eParkspace_allocation_request_msg=0x32, //请求分配车位消息
+		eParkspace_allocation_response_msg=0x33,//分配车位结果反馈消息
+		eParkspace_search_request_msg = 0x34,		//查询车位请求消息
+		eParkspace_search_response_msg = 0x35,		//查询车位反馈消息
+		eParkspace_release_request_msg = 0x36,		//释放车位请求消息
+		eParkspace_release_response_msg = 0x37,		//释放车位反馈消息
+		eParkspace_force_update_request_msg = 0x38,	//手动修改车位消息
+		eParkspace_force_update_response_msg = 0x39,//手动修改车位反馈消息
+		eParkspace_confirm_alloc_request_msg = 0x3A,//确认分配车位请求消息
+		eParkspace_confirm_alloc_response_msg = 0x3B,//确认分配车位反馈消息
+
+
+		eStore_command_request_msg=0x41,        //终端停车请求消息
+		eStore_command_response_msg=0x42,       //停车请求反馈消息
+		ePickup_command_request_msg=0x43,       //取车请求消息
+		ePickup_command_response_msg=0x44,       //取车请求反馈消息
+
+
+
+		eStoring_process_statu_msg=0x90,        //停车指令进度条消息
+		ePicking_process_statu_msg=0x91,        //取车指令进度消息
+
+
+		eCentral_controller_statu_msg=0xa0,     //中控系统状态消息
+
+
+		eEntrance_manual_operation_msg=0xb0,            //针对出入口状态操作的手动消息
+		eProcess_manual_operation_msg=0xb1,             //针对流程的手动消息
+
 	};
 
 //通讯单元
 	enum Communicator
 	{
-		eEmpty=0x0000,		//空
-		eMain=0x0001,    	//主流程
-		eTerminor=0x0100,	//终端
-		eTable=0x0200,		//数据表
-		eMeasurer=0x0300,	//测量单元
-		eProcess=0x0400,	//调度机构
+		eEmpty=0x0000,
+		eMain=0x0001,    //主流程
+
+		eTerminor=0x0100,
+		//车位表
+		eParkspace=0x0200,
+		//测量单元
+		eMeasurer=0x0300,
+		//调度机构
+		eDispatch=0x0400,
 		//...
+
 	};
 public:
 	Communication_message();

+ 3 - 1
error_code/error_code.h

@@ -55,9 +55,10 @@ enum Error_code
 
     NODATA                         = 0x00000010,//没有数据,传入参数容器内部没有数据时,
 	INVALID_MESSAGE					= 0x00000011, //无效的消息,
-    RESPONSE_TIMEOUT                = 0x00000012,
+	PARSE_FAILED					= 0x00000012,//解析失败
 
     TASK_TIMEOVER					= 0x00000020,//任务超时
+	RESPONSE_TIMEOUT                = 0x00000021,//答复超时
 
     POINTER_IS_NULL                 = 0x00000101,//空指针
     PARAMETER_ERROR                 = 0x00000102,//参数错误,传入参数不符合规范时,
@@ -176,6 +177,7 @@ enum Error_code
 	LOCATER_SIFT_CLOUD_VERY_LITTLE,
 	LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
 	LOCATER_SIFT_PREDICT_FAILED,
+	LOCATER_SIFT_PREDICT_TIMEOUT,
 	LOCATER_SIFT_PREDICT_NO_WHEEL_POINT,
 	LOCATER_SIFT_PREDICT_NO_CAR_POINT,
 

+ 9 - 3
locate/locate_manager.cpp

@@ -364,6 +364,11 @@ Locate_manager::Locate_manager_status Locate_manager::get_locate_manager_status(
 	return m_locate_manager_status;
 }
 
+Point_sift_segmentation* Locate_manager::get_point_sift()
+{
+	return mp_point_sift;
+}
+
 //mp_locate_manager_thread 线程执行函数,
 //thread_work 内部线程负责locate定位分析整车的信息,并且回收汇总雷达的数据
 void Locate_manager::thread_work()
@@ -537,7 +542,7 @@ Error_manager Locate_manager::locate_manager_sift()
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tp_cloud_wheel(new pcl::PointCloud<pcl::PointXYZRGB>);
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tp_cloud_car(new pcl::PointCloud<pcl::PointXYZRGB>);
 		//利用PointSift从场景中分割车辆点云
-		t_error = locate_sift(map_iter.second, tp_cloud_wheel, tp_cloud_car, m_save_flag, m_save_path);
+		t_error = locate_sift(map_iter.first, map_iter.second, tp_cloud_wheel, tp_cloud_car, m_save_flag, m_save_path);
 		if (t_error != Error_code::SUCCESS)
 		{
 			char buf[256] = {0};
@@ -557,11 +562,12 @@ Error_manager Locate_manager::locate_manager_sift()
 }
 
 //定位筛选,对具体的点云进行操作,分离出车轮和车身.
+//input::lidar_id 雷达id, 默认为0
 //input::cloud输入点云
 //output::cloud_wheel输出车轮点云
 //output::cloud_car输出车身点云
 //work_dir:中间文件保存路径
-Error_manager Locate_manager::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+Error_manager Locate_manager::locate_sift(int lidar_id, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
                                    pcl::PointCloud<pcl::PointXYZRGB>::Ptr& p_cloud_wheel,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& p_cloud_car,
 										   bool save_flag, std::string work_dir)
 {
@@ -581,7 +587,7 @@ Error_manager Locate_manager::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr p_
 
     //分割车辆点云
     std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
-	t_error=mp_point_sift->segmentation(p_cloud_in, segmentation_clouds, save_flag, work_dir);
+	t_error=mp_point_sift->segmentation( lidar_id, p_cloud_in, segmentation_clouds, save_flag, work_dir);
     if(t_error!=SUCCESS)
     {
         return t_error;

+ 4 - 2
locate/locate_manager.h

@@ -72,6 +72,7 @@ public: //API 对外接口
 
 public://get and set
 	Locate_manager_status get_locate_manager_status();
+	Point_sift_segmentation* get_point_sift();
 
 protected:
 	//mp_locate_manager_thread 线程执行函数,
@@ -82,11 +83,12 @@ protected:
 	Error_manager locate_manager_sift();
 
 	//定位筛选,对具体的点云进行操作,分离出车轮和车身.
-    //input::cloud输入点云
+	//input::lidar_id 雷达id, 默认为0
+	//input::cloud输入点云
     //output::cloud_wheel输出车轮点云
 	//output::cloud_car输出车身点云
     //work_dir:中间文件保存路径
-    Error_manager locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+    Error_manager locate_sift(int lidar_id, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
                            pcl::PointCloud<pcl::PointXYZRGB>::Ptr& p_cloud_wheel,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& p_cloud_car,
 							  bool save_flag, std::string work_dir);
 

+ 166 - 10
locate/point_sift_segmentation.cpp

@@ -9,8 +9,7 @@
 #include <time.h>
 #include <sys/time.h>
 #include <chrono>
-using namespace std;
-using namespace chrono;
+#include <thread>
 
 void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& cloud)
 {
@@ -86,7 +85,7 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
 	float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
 	float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));
 
-	auto start   = system_clock::now();
+	auto start   = std::chrono::system_clock::now();
 
 	if (false == Predict(cloud_data, output))
 	{
@@ -102,10 +101,10 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
 		free(output);
 
 	}
-	auto end   = system_clock::now();
-	auto duration = duration_cast<microseconds>(end - start);
-	cout <<  "花费了"
-		 << double(duration.count()) * microseconds::period::num / microseconds::period::den   << "秒" << endl;
+	auto end   = std::chrono::system_clock::now();
+	auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+	std::cout <<  "花费了"
+		 << double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "秒" << std::endl;
 
 
 	return SUCCESS;
@@ -116,7 +115,7 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
 //分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
 //cloud:输入点云
 //clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
-Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+Error_manager Point_sift_segmentation::segmentation(int lidar_id, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
 													std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
 													bool save_flag, std::string save_dir)
 {
@@ -178,6 +177,10 @@ Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXY
 	}
 	std::cout << "p_cloud_select = "<<p_cloud_select->size() << std::endl;
 
+
+
+
+	/*
 	//第六步
 	//把pcl点云转化为float, 因为TensorFlow算法只能识别float
 	//提前分配内存, 后续记得回收....
@@ -209,6 +212,77 @@ Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXY
 		 << double(duration.count()) * microseconds::period::num / microseconds::period::den   << "秒" << endl;
 	cout <<  double(duration.count()) << " "
 		 <<   microseconds::period::num << " " << microseconds::period::den   << "秒" << endl;
+*/
+
+	//重写6和7步, 将TensorFlow算法放在服务器上, 使用socket通信去发送点云并获取结果
+	//第六步
+	//把pcl点云转化为float, 因为TensorFlow算法只能识别float
+	//提前分配内存, 后续记得回收....
+	float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float));				//8192*3 float, 8192个三维点
+	int* p_data_type = (int*)malloc(m_point_num*sizeof(int));		//8192*2 float, 8192个类别百分比(车轮和车身2类)
+	memset(p_data_point, 0, m_point_num * 3 * sizeof(float));
+	memset(p_data_type, 0, m_point_num * sizeof(int));
+	//将点云数据转换到float*
+	t_error = translate_cloud_to_float(p_cloud_select, p_data_point);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		free(p_data_point);
+		free(p_data_type);
+		return t_error;
+	}
+
+	//第七步
+	//tensonflow预测点云, 计算出每个三维点的类别百分比,
+	auto start = std::chrono::system_clock::now();
+	//预测, 发送socket通信, 在服务器上进行点云算法
+	t_error = m_predict_with_network.predict_for_send(lidar_id, p_data_point);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		free(p_data_point);
+		free(p_data_type);
+		return t_error;
+	}
+
+	//循环接受, 超时等待5000ms, (后续可以在这里加上强制取消)
+	while (std::chrono::system_clock::now() - start < std::chrono::milliseconds(5000))
+	{
+		t_error = m_predict_with_network.predict_for_receive(lidar_id, p_data_type);
+		if ( t_error == Error_code::SUCCESS )
+		{
+			//算法成功, 直接退出循环即可
+			break;
+		}
+		else if ( t_error == Error_code::NODATA )
+		{
+			//继续循环, 延迟1us
+			std::this_thread::yield();
+			std::this_thread::sleep_for(std::chrono::microseconds(1));
+			continue;
+		}
+		else
+		{
+			free(p_data_point);
+			free(p_data_type);
+			return t_error;
+		}
+	}
+	if ( std::chrono::system_clock::now() - start >= std::chrono::milliseconds(5000) )
+	{
+		free(p_data_point);
+		free(p_data_type);
+		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_TIMEOUT, Error_level::MINOR_ERROR,
+							 " m_predict_with_network.predict_for_receive error ");
+	}
+
+
+	auto end   = std::chrono::system_clock::now();
+	auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+	std::cout <<  "花费了"
+		 << double(duration.count()) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "秒" << std::endl;
+	std::cout <<  double(duration.count()) << " "
+		 <<   std::chrono::microseconds::period::num << " " << std::chrono::microseconds::period::den   << "秒" << std::endl;
+
+
 
 	//第八步
 	//恢复点云,并填充到cloud_vector
@@ -242,6 +316,11 @@ Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXY
 }
 
 
+Predict_with_network& Point_sift_segmentation::get_predict_with_network()
+{
+	return m_predict_with_network;
+}
+
 
 //使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
 Error_manager Point_sift_segmentation::filter_cloud_with_voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
@@ -334,7 +413,7 @@ Error_manager Point_sift_segmentation::filter_cloud_with_my_grid(pcl::PointCloud
 //	std::cout << "---------------------------"<< std::endl;
 
 	//创建网格->三维点的map, 每个网格里面只保留一个点,
-	map<int, pcl::PointXYZ> grid_point_map;
+	std::map<int, pcl::PointXYZ> grid_point_map;
 	//遍历输入点云, 将每个点的索引编号写入对应的网格.
 	for (int i = 0; i < p_cloud->size(); ++i)
 	{
@@ -492,7 +571,7 @@ Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud<
 }
 
 //恢复点云, 将float*转换成点云
-//p_data_type:输入点云对应的类别,大小为  点数*类别
+//p_data_type:输入点云对应的类别,  大小为  点数*m_cls_num*float
 //p_data_point:输入点云数据(xyz)
 //cloud_vector::输出带颜色的点云vector
 Error_manager Point_sift_segmentation::recovery_float_to_cloud(float* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
@@ -579,6 +658,83 @@ Error_manager Point_sift_segmentation::recovery_float_to_cloud(float* p_data_typ
 	return Error_code::SUCCESS;
 }
 
+//恢复点云, 将float*转换成点云
+//p_data_type:输入点云对应的类别(取值 0 ~ m_cls_num-1 ),大小为  点数*int
+//p_data_point:输入点云数据(xyz)
+//cloud_vector::输出带颜色的点云vector
+Error_manager Point_sift_segmentation::recovery_float_to_cloud(int* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
+{
+	if ( p_data_type == NULL || p_data_point == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " recovery_float_to_cloud  p_data_type or  p_data_point  POINTER_IS_NULL ");
+	}
+
+	//为cloud_vector 添加m_cls_num个点云, 提前分配内存
+	for (int k = 0; k < m_cls_num; ++k)
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
+		cloud_vector.push_back(cloud_rgb);
+	}
+
+	//遍历data数据,
+	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.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
+			|| t_point.z>m_maxp.z || t_point.z < m_minp.z)
+		{
+			continue;
+		}
+
+		//当前点 的类别, 初始化为0
+		int t_cls = p_data_type[i];
+
+		//根据点的类别, 添加颜色, 并且分别加入到各自的点云里面
+		switch ( t_cls )
+		{
+			case 0: //第0类, 绿色, 轮胎
+				t_point.r = 0;
+				t_point.g = 255;
+				t_point.b = 0;
+				break;
+			case 1://第1类, 白色, 车身
+				t_point.r = 255;
+				t_point.g = 255;
+				t_point.b = 255;
+				break;
+//			case 2:
+//				;
+//				break;
+			default:
+
+				break;
+		}
+		cloud_vector[t_cls]->push_back(t_point);
+	}
+
+	//校验点云的数量, 要求size>0
+	if(cloud_vector[0]->size() <=0)
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_WHEEL_POINT, Error_level::MINOR_ERROR,
+							 " recovery_float_to_cloud NO_WHEEL_POINT ");
+	}
+	if(cloud_vector[1]->size() <=0)
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_CAR_POINT, Error_level::MINOR_ERROR,
+							 " recovery_float_to_cloud NO_CAR_POINT ");
+	}
+
+
+	return Error_code::SUCCESS;
+}
+
+
+
 //保存点云数据
 Error_manager Point_sift_segmentation::save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,std::string save_dir)
 {

+ 22 - 9
locate/point_sift_segmentation.h

@@ -5,6 +5,7 @@
 #include "../error_code/error_code.h"
 #include <pcl/point_types.h>
 #include <pcl/common/common.h>
+#include "../locate/predict_with_network.h"
 
 //Cloud_box,点云的三维方框,
 typedef struct Cloud_box
@@ -35,10 +36,12 @@ public:
 	//分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
 	//cloud:输入点云, (有内存)
 	//cloud_vector:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
-	virtual Error_manager segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+	virtual Error_manager segmentation(int lidar_id, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
 							  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
 							  bool save_flag, std::string save_dir);
 
+public://get or set member variable
+	Predict_with_network& get_predict_with_network();
 
 protected:
 
@@ -58,19 +61,29 @@ protected:
 	//将点云数据转换到float*
 	Error_manager translate_cloud_to_float(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, float* p_data_out);
 
-    //恢复点云, 将float*转换成点云
-    //p_data_type:输入点云对应的类别,大小为  点数*类别
-    //p_data_point:输入点云数据(xyz)
-    //cloud_vector::输出带颜色的点云vector
+
+
+	//恢复点云, 将float*转换成点云
+	//p_data_type:输入点云对应的类别,  大小为  点数*m_cls_num*float
+	//p_data_point:输入点云数据(xyz)
+	//cloud_vector::输出带颜色的点云vector
 	Error_manager recovery_float_to_cloud(float* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector);
+	//恢复点云, 将float*转换成点云
+	//p_data_type:输入点云对应的类别(取值 0 ~ m_cls_num-1 ),大小为  点数*int
+	//p_data_point:输入点云数据(xyz)
+	//cloud_vector::输出带颜色的点云vector
+	Error_manager recovery_float_to_cloud(int* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector);
 
 	//保存点云数据
 	Error_manager save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector, std::string save_dir);
 protected:
-	float		        m_freq;					//抽稀的分辨率,
-    pcl::PointXYZ		m_minp;					//真实点云的最小值, 区域范围
-    pcl::PointXYZ		m_maxp;					//真实点云的最大值, 区域范围
-	Cloud_box			m_cloud_box_limit;		//限制点云的方框, 只有在有效区域内的点才能参与计算.
+	float		        	m_freq;					//抽稀的分辨率,
+    pcl::PointXYZ			m_minp;					//真实点云的最小值, 区域范围
+    pcl::PointXYZ			m_maxp;					//真实点云的最大值, 区域范围
+	Cloud_box				m_cloud_box_limit;		//限制点云的方框, 只有在有效区域内的点才能参与计算.
+
+	Predict_with_network 	m_predict_with_network;	//从服务器进行点云预测算法的功能模块
+
 };
 #endif
 

+ 2 - 0
message/message_base.proto

@@ -12,6 +12,8 @@ enum Message_type
     eLocate_request_msg=0x12;               //定位请求消息
     eLocate_response_msg=0x13;              //定位反馈消息
 
+    eLocate_sift_request_msg = 0x14;            //预测算法请求消息
+    eLocate_sift_response_msg = 0x15;           //预测算法反馈消息
 
     eDispatch_status_msg=0x21;                //调度模块硬件状态消息
     eDispatch_request_msg=0x22;              //请求调度消息

+ 51 - 1
system/system_executor.cpp

@@ -9,6 +9,7 @@
 #include "../system/system_communication.h"
 #include "../wanji_lidar/wanji_manager.h"
 #include "../tool/common_data.h"
+#include "../locate/locate_manager.h"
 
 System_executor::System_executor()
 {
@@ -67,6 +68,24 @@ Error_manager System_executor::check_msg(Communication_message* p_msg)
 			}
 			break;
 		}
+		case Communication_message::Message_type::eLocate_sift_response_msg:
+		{
+			//检查接受人
+			if ( p_msg->get_receiver() == Communication_message::Communicator::eMeasurer )
+			{
+				message::Locate_sift_response_msg t_locate_sift_response_msg;
+				//针对消息类型, 对消息进行二次解析
+				if (t_locate_sift_response_msg.ParseFromString(p_msg->get_message_buf()))
+				{
+					//检查终端id
+					if ( t_locate_sift_response_msg.terminal_id() == m_terminal_id )
+					{
+						return Error_code::SUCCESS;
+					}
+				}
+			}
+			break;
+		}
 		default :
 			;
 			break;
@@ -151,6 +170,12 @@ Error_manager System_executor::check_executer(Communication_message* p_msg)
 			}
 			break;
 		}
+		case Communication_message::Message_type::eLocate_sift_response_msg:
+		{
+			//不用检查状态, 直接返回成功
+			return Error_code::SUCCESS;
+			break;
+		}
 		default :
 			;
 			break;
@@ -172,9 +197,10 @@ Error_manager System_executor::execute_msg(Communication_message* p_msg)
 	switch ( p_msg->get_message_type() )
 	{
 		case Communication_message::eLocate_request_msg:
+		{
 			message::Measure_request_msg t_measure_request_msg;
 			//针对消息类型, 对消息进行二次解析
-			if( t_measure_request_msg.ParseFromString(p_msg->get_message_buf()) )
+			if (t_measure_request_msg.ParseFromString(p_msg->get_message_buf()))
 			{
 				//往线程池添加执行任务, 之后会唤醒一个线程去执行他.
 				m_thread_pool.enqueue(&System_executor::execute_for_measure, this,
@@ -187,7 +213,18 @@ Error_manager System_executor::execute_msg(Communication_message* p_msg)
 									 " message::Measure_request_msg  ParseFromString error ");
 			}
 			break;
+		}
+		case Communication_message::eLocate_sift_request_msg:
+		{
+			//往线程池添加执行任务, 之后会唤醒一个线程去执行他.
+			m_thread_pool.enqueue(&System_executor::execute_for_predict, this,
+								  p_msg->get_message_buf());
+			break;
+		}
+		default:
+		{
 
+		}
 	}
 
 
@@ -311,6 +348,10 @@ System_executor::System_executor_status System_executor::get_system_executor_sta
 	return m_system_executor_status;
 }
 
+int System_executor::get_terminal_id()
+{
+	return m_terminal_id;
+}
 
 
 
@@ -634,4 +675,13 @@ void System_executor::execute_for_measure(std::string command_info, int terminal
 */
 
 	return ;
+}
+
+
+
+
+//签收预测的答复消息, (不能传引用)
+void System_executor::execute_for_predict(std::string message_string)
+{
+	Locate_manager::get_instance_references().get_point_sift()->get_predict_with_network().execute_for_predict(message_string);
 }

+ 9 - 0
system/system_executor.h

@@ -59,13 +59,22 @@ public://API functions
 	bool is_ready();
 public://get or set member variable
 	System_executor_status get_system_executor_status();
+	int get_terminal_id();
+
 public:
+	//注注注注注意了, 线程池的函数独立运行,
+	//传递参数, 不能使用即将销毁的内存
+
+
 	//雷达感测定位 的处理函数
 //input::command_id, 消息指令id, 由主控制系统生成的唯一码
 //input::command_id, 终端id, 对应具体的某个车位
 //return::void, 没有返回, 执行结果直接生成一条答复消息, 然后通过通信返回
 	void execute_for_measure(std::string command_info, int terminal_id,std::chrono::system_clock::time_point receive_time);
 
+	//签收预测的答复消息, (不能传引用)
+	void execute_for_predict(std::string message_string);
+
 
 protected://member variable