20200623, 重写locate, 新增communication

1.优化laser模块,
2.重写locate模块
3.新增communication_base通信基类, 具体的实现还没有写

CMakeLists.txt

@@ -17,7 +17,7 @@ set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
 set(CMAKE_BUILD_TYPE Debug)
 
 #find_package(Eigen3 REQUIRED)
-
+MESSAGE(WARN "pcl:: ${PCL_INCLUDE_DIRS} --- ${PCL_LIBRARIES}")
 include_directories(
         laser
 #        plc
@@ -48,7 +48,7 @@ add_executable(hl_chutian ./main.cpp ./error_code/error_code.cpp
         ./laser/laser_task_command
         ${LASER_SRC}
 #        ${PLC_SRC}
-#        ${TERMINOR_SRC}
+        ${TERMINOR_SRC}
         ${LOCATE_SRC}
         ${TASK_MANAGER_SRC}
         ${TOOL_SRC}
@@ -63,7 +63,7 @@ add_executable(hl_chutian ./main.cpp ./error_code/error_code.cpp
 
 
 target_link_libraries(hl_chutian
-        #${OpenCV_LIBS}
+        ${OpenCV_LIBS}
         ${GLOG_LIBRARIES}
         ${PCL_LIBRARIES}
         ${PROTOBUF_LIBRARIES}

error_code/error_code.h

@@ -53,7 +53,8 @@ enum Error_code
     WARNING                         = 0x00000003,//警告
     FAILED                          = 0x00000004,//失败
 
-    NO_DATA                          = 0x00000010,//没有数据，传入参数容器内部没有数据时，
+    NO_DATA                         = 0x00000010,//没有数据，传入参数容器内部没有数据时，
+
 
     POINTER_IS_NULL                 = 0x00000101,//空指针
     PARAMETER_ERROR                 = 0x00000102,//参数错误，传入参数不符合规范时，
@@ -61,6 +62,8 @@ enum Error_code
 
     CLASS_BASE_FUNCTION_CANNOT_USE  = 0x00000201,//基类函数不允许使用，必须使用子类的
 
+	CONTAINER_IS_TERMINATE			= 0x00000301,//容器被终止
+
 
 //    错误码的规范，
 //    错误码是int型，32位，十六进制。
@@ -74,6 +77,7 @@ enum Error_code
 
 //    laser扫描模块
     LASER_ERROR_BASE                = 0x01000000,
+
 //    laser_base基类
 	LASER_BASE_ERROR_BASE			= 0x01010000,
     LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达基类模块, 任务输入参数错误
@@ -97,10 +101,10 @@ enum Error_code
 
 	//laser_manager 雷达管理模块
 	LASER_MANAGER_ERROR_BASE						= 0x01030000,
+	LASER_MANAGER_READ_PROTOBUF_ERROR,				//雷达管理模块，读取参数错误
 	LASER_MANAGER_STATUS_BUSY,						//雷达管理模块，状态正忙
 	LASER_MANAGER_STATUS_ERROR,						//雷达管理模块，状态错误
 	LASER_MANAGER_TASK_TYPE_ERROR,					//雷达管理模块，任务类型错误
-	LASER_MANAGER_READ_PROTOBUF_ERROR,				//雷达管理模块，读取参数错误
 	LASER_MANAGER_IS_NOT_READY,						//雷达管理模块，不在准备状态
 	LASER_MANAGER_LASER_INDEX_ERRPR,				//雷达管理模块，雷达索引错误，编号错误。
 	LASER_MANAGER_TASK_OVER_TIME,					//雷达管理模块，任务超时
@@ -127,8 +131,24 @@ enum Error_code
     PLC_WRITE_FAILED,
     PLC_NOT_ENOUGH_DATA_ERROR,
 
-    //Locater.cpp error from 0x0301000-0x030100FF
-        LOCATER_TASK_INIT_CLOUD_EMPTY=0x03010000,
+
+
+    //locate 定位模块,
+	LOCATER_ERROR_BASE                				= 0x03000000,
+
+	//LASER_MANAGER 定位管理模块
+	LOCATER_MANAGER_ERROR_BASE                		= 0x03010000,
+	LOCATER_MANAGER_READ_PROTOBUF_ERROR,				//定位管理模块，读取参数错误
+	LOCATER_MANAGER_STATUS_BUSY,						//定位管理模块，状态正忙
+	LOCATER_MANAGER_STATUS_ERROR,						//定位管理模块，状态错误
+	LOCATER_MANAGER_TASK_TYPE_ERROR,					//定位管理模块，任务类型错误
+	LOCATER_MANAGER_IS_NOT_READY,						//定位管理模块，不在准备状态
+	LOCATER_MANAGER_CLOUD_MAP_ERROR,					//定位管理模块，任务输入点云map的error
+	LOCATE_MANAGER_TASK_OVER_TIME,
+
+
+	//Locater.cpp error from 0x0301000-0x030100FF
+	LOCATER_TASK_INIT_CLOUD_EMPTY ,
     LOCATER_TASK_ERROR,
     LOCATER_TASK_INPUT_CLOUD_UNINIT,
     LOCATER_INPUT_CLOUD_EMPTY,
@@ -143,30 +163,40 @@ enum Error_code
 
 
     //point sift from 0x03010100-0x030101FF
-        LOCATER_SIFT_INIT_FAILED=0x03010100,
+    LOCATER_SIFT_INIT_FAILED=0x03010100,
     LOCATER_SIFT_INPUT_CLOUD_UNINIT,
-    LOCATER_SIFT_PREDICT_FAILED,
-    LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
+	LOCATER_SIFT_INPUT_CLOUD_EMPTY,
+	LOCATER_SIFT_GRID_ERROR,
+	LOCATER_SIFT_SELECT_ERROR,
+	LOCATER_SIFT_CLOUD_VERY_LITTLE,
+	LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,
+	LOCATER_SIFT_PREDICT_FAILED,
+	LOCATER_SIFT_PREDICT_NO_WHEEL_POINT,
+	LOCATER_SIFT_PREDICT_NO_CAR_POINT,
+
     LOCATER_SIFT_FILTE_OBS_FAILED,
     LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,
-    LOCATER_SIFT_INPUT_CLOUD_EMPTY,
-    LOCATER_SIFT_PREDICT_NO_CAR_POINT,
 
-    //yolo from 0x03010200-0x030102FF
-        LOCATER_YOLO_DETECT_FAILED=0x03010200,
-    LOCATER_YOLO_DETECT_NO_TARGET,
-    LOCATER_YOLO_PARAMETER_INVALID,
-    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
+//    //yolo from 0x03010200-0x030102FF
+//        LOCATER_YOLO_DETECT_FAILED=0x03010200,
+//    LOCATER_YOLO_DETECT_NO_TARGET,
+//    LOCATER_YOLO_PARAMETER_INVALID,
+//    LOCATER_YOLO_INPUT_CLOUD_UNINIT,
 
     //3dcnn from 0x03010300-0x030103FF
     LOCATER_3DCNN_INIT_FAILED=0x03010300,
     LOCATER_3DCNN_INPUT_CLOUD_UNINIT,
+	LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
+	LOCATER_3DCNN_INPUT_CLOUD_MAP_ERROR,
+	LOCATER_3DCNN_PCA_OUT_ERROR,
+	LOCATER_3DCNN_EXTRACT_RECT_ERROR,
+	LOCATER_3DCNN_RECT_SIZE_ERROR,
+
     LOCATER_3DCNN_PREDICT_FAILED,
     LOCATER_3DCNN_VERIFY_RECT_FAILED_3,
     LOCATER_3DCNN_VERIFY_RECT_FAILED_4,
     LOCATER_3DCNN_KMEANS_FAILED,
     LOCATER_3DCNN_IIU_FAILED,
-    LOCATER_3DCNN_INPUT_CLOUD_EMPTY,
     LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,
 
     //System_manager error from 0x04010000-0x0401FFFF
@@ -238,9 +268,18 @@ enum Error_code
 
 
 
-    //task module,  error from 0x10010000-0x1001FFFF
+    //task module, 任务模块  error from 0x10010000-0x1001FFFF
 	TASK_MODULE_ERROR_BASE 							= 0x10010000,
 	TASK_TYPE_IS_UNKNOW,
+	TASK_NO_RECEIVER,
+
+
+	//Communication module, 通信模块
+	COMMUNICATION_BASE_ERROR_BASE					= 0x11010000,
+	COMMUNICATION_BIND_ERROR,
+	COMMUNICATION_CONNECT_ERROR,
+	COMMUNICATION_ANALYSIS_TIME_OUT,									//解析超时,
+	COMMUNICATION_EXCUTER_IS_BUSY,										//处理器正忙, 请稍等
 };
 
 //错误等级，用来做故障处理

laser/Laser.cpp

@@ -42,6 +42,12 @@ Error_manager Laser_base::connect_laser()
 	}
 	else
 	{
+		//清空队列
+		m_queue_laser_data.clear_and_delete();
+		//唤醒队列，
+		m_queue_laser_data.wake_queue();
+
+
 		//这里不建议用detach，而是应该在disconnect里面使用join
 		//创建接受缓存的线程，不允许重复创建
 		if (mp_thread_receive != NULL)
@@ -309,7 +315,7 @@ Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
 		m_save_path = save_path;
 		char pts_file[255] = { 0 };
 //		sprintf(pts_file, "%s/pts%d.txt", save_path.c_str(), m_laser_id+1);
-		sprintf(pts_file, "%s/laser_pst_%s__%d.txt", fullPath.c_str(), time_string,m_laser_id+1);
+		sprintf(pts_file, "%s/laser_pst_%s__%d.txt", fullPath.c_str(), time_string, m_laser_id+0);
 		m_points_log_tool.open(pts_file);
 
 //		char bin_file[255] = { 0 };
@@ -334,7 +340,7 @@ Error_manager Laser_base::close_save_path()
 //子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
 bool Laser_base::is_ready()
 {
-	return ( get_laser_statu() == LASER_READY );
+	return  get_laser_statu() == LASER_READY ;
 }
 //获取雷达状态
 Laser_statu Laser_base::get_laser_statu()
@@ -359,7 +365,8 @@ void Laser_base::thread_receive()
 		m_condition_receive.wait();
 		if ( m_condition_receive.is_alive() )
 		{
-			//获取雷达的通信消息缓存, ，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，
+			std::this_thread::yield();
+			//获取雷达的通信消息缓存, ，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，(循环执行)
 			t_error = receive_buf_to_queue();
 			if (t_error == SUCCESS)
 			{
@@ -374,14 +381,15 @@ void Laser_base::thread_receive()
 			{
 				if ( !m_laser_scan_flag )
 				{
-					//停止线程，m_condition_receive.wait() 函数将会阻塞。
+					//停止线程，m_condition_receive.wait() 函数将会阻塞。正常停止接受线程
 					m_condition_receive.set_pass_ever(false);
+					//注:此时thread_transform线程仍然继续,任务也没有停止.
 				}
 			}
 			else
 			{
 				//因为故障,而提前结束任务.
-				mp_laser_task->set_task_error_manager(t_error);
+				mp_laser_task->compare_and_cover_task_error_manager(t_error);
 				end_task();
 			}
 		}
@@ -403,6 +411,7 @@ void Laser_base::thread_transform()
 		m_condition_transform.wait();
 		if ( m_condition_transform.is_alive() )
 		{
+			std::this_thread::yield();
 			t_error = transform_buf_to_points();
 			if ( t_error == SUCCESS )
 			{
@@ -414,9 +423,9 @@ void Laser_base::thread_transform()
 				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。防止数据遗留。
 			else if (t_error == NO_DATA )
 			{
-				if(!m_laser_scan_flag && m_condition_receive.get_pass_ever() == false)
+				if(m_laser_scan_flag == false && m_condition_receive.get_pass_ever() == false)
 				{
-					//停止线程，m_condition_transform.wait() 函数将会阻塞。
+					//停止转化线程，m_condition_transform.wait() 函数将会阻塞。
 					m_condition_transform.set_pass_ever(false);
 
 					//mp_thread_transform 停止时，雷达扫描任务彻底完成，此时任务正常结束
@@ -426,7 +435,7 @@ void Laser_base::thread_transform()
 			else
 			{
 				//因为故障,而提前结束任务.
-				mp_laser_task->set_task_error_manager(t_error);
+				mp_laser_task->compare_and_cover_task_error_manager(t_error);
 				end_task();
 			}
 

laser/LivoxLaser.cpp

@@ -73,12 +73,13 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 							 "laser task type error ！= LASER_BASE_SCAN_TASK");
 	}
 
+
+
 	//检查雷达状态
 	t_error = check_status();
 	if ( t_error != SUCCESS )
 	{
 		t_result.compare_and_cover_error(t_error);
-		return t_result;
 	}
 	else
 	{
@@ -90,7 +91,7 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 		//检查三维点云指针
 		if (mp_laser_task->get_task_point_cloud().get() == NULL)
 		{
-			t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
 										 Error_level::MINOR_ERROR,
 										 "execute_task mp_task_point_cloud is null");
 			t_result.compare_and_cover_error(t_error);
@@ -126,23 +127,22 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 				mp_laser_task->set_task_statu(TASK_WORKING);
 			}
 		}
-	}
-
 
-	//返回错误码
-	if (t_result != Error_code::SUCCESS)
-	{
-		//忽略轻微故障
-		if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+		//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
 		{
-			//将任务的状态改为 TASK_ERROR 结束错误
-			mp_laser_task->set_task_statu(TASK_ERROR);
-		}
+			//忽略轻微故障
+			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+				//将任务的状态改为 TASK_ERROR 结束错误
+				mp_laser_task->set_task_statu(TASK_ERROR);
+			}
+			//返回错误码
+			mp_laser_task->compare_and_cover_task_error_manager(t_result);
 
-		//返回错误码
-		mp_laser_task->set_task_error_manager(t_result);
-		return t_result;
+		}
 	}
+
 	return t_result;
 }
 
@@ -304,7 +304,7 @@ Error_manager CLivoxLaser::receive_buf_to_queue()
 	if (m_queue_livox_data.try_pop(tp_livox_buf))
 	{
 		//tp_livox_buf此时是有内存的, 是 livox_data_callback 里面new出来的
-		//Binary_buf 数据的管理权限    从 m_queue_livox_data 直接进入到 m_queue_laser_data,
+		//Binary_buf 内存的管理权限    从 m_queue_livox_data 直接进入到 m_queue_laser_data,
 		//数据最终在 Laser_base::thread_transform()  transform_buf_to_points  释放.
 		if(m_queue_laser_data.push(tp_livox_buf))
 		{
@@ -322,7 +322,7 @@ Error_manager CLivoxLaser::receive_buf_to_queue()
 	{
 		return NO_DATA;// m_queue_livox_data 没有数据并不意味着程序就出错了,
 	}
-return Error_code::SUCCESS;
+	return Error_code::SUCCESS;
 }
 //将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
 // 纯虚函数，必须由子类重载，
@@ -358,9 +358,10 @@ Error_manager CLivoxLaser::transform_buf_to_points()
 			//LivoxRawPoint 转为 pcl::PointXYZ
 			//int32_t 转 double。不要信号强度
 			pcl::PointXYZ  pointXYZ;
-			pointXYZ.x = point_destination.x;
-			pointXYZ.y = point_destination.y;
-			pointXYZ.z = point_destination.z;
+			pointXYZ.x = point_destination.x/1000.0;
+			pointXYZ.y = point_destination.y/1000.0;
+			pointXYZ.z = point_destination.z/1000.0;
+			//注:单位毫米转为米, 最终数据单位统一为米.....
 			t_error =  mp_laser_task->task_push_point(&pointXYZ);
 			if ( t_error != Error_code::SUCCESS )
 			{

laser/laser_manager.cpp

@@ -27,19 +27,19 @@ Laser_manager::~Laser_manager()
 //初始化 雷达 管理模块。如下三选一
 Error_manager Laser_manager::laser_manager_init()
 {
-	return  laser_manager_init_from_protobuf(LASER_PARAMETER_ALL_PATH);
+	return  laser_manager_init_from_protobuf(LASER_PARAMETER_PATH);
 }
 //初始化 雷达 管理模块。从文件读取
 Error_manager Laser_manager::laser_manager_init_from_protobuf(std::string prototxt_path)
 {
-	Laser_proto::Laser_parameter_all laser_parameters;
+	Laser_proto::Laser_parameter_all t_laser_parameters;
 
-	if(!  proto_tool::read_proto_param(prototxt_path,laser_parameters) )
+	if(!  proto_tool::read_proto_param(prototxt_path,t_laser_parameters) )
 	{
 		return Error_manager(LASER_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Laser_manager read_proto_param  failed");
 	}
 
-	return laser_manager_init_from_protobuf(laser_parameters);
+	return laser_manager_init_from_protobuf(t_laser_parameters);
 }
 //初始化 雷达 管理模块。从protobuf读取
 Error_manager Laser_manager::laser_manager_init_from_protobuf(Laser_proto::Laser_parameter_all& laser_parameters)
@@ -154,8 +154,13 @@ Error_manager Laser_manager::execute_task(Task_Base* p_laser_task)
 							 "laser task type error ！= LASER_MANGER_SCAN_TASK ");
 	}
 
+	//检查接收方的状态
 	t_error = check_status();
-	if ( t_error == SUCCESS )
+	if ( t_error != SUCCESS )
+	{
+		t_result.compare_and_cover_error(t_error);
+	}
+	else
 	{
 		//接受任务，并将任务的状态改为TASK_SIGNED已签收
 		mp_laser_manager_task = (Laser_manager_task *) p_laser_task;
@@ -163,7 +168,7 @@ Error_manager Laser_manager::execute_task(Task_Base* p_laser_task)
 
 		//检查消息内容是否正确，
 		//检查三维点云指针
-		if (mp_laser_manager_task->get_task_point_cloud().get() == NULL)
+		if (mp_laser_manager_task->get_task_point_cloud_map() == NULL)
 		{
 			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
 										Error_level::MINOR_ERROR,
@@ -188,20 +193,21 @@ Error_manager Laser_manager::execute_task(Task_Base* p_laser_task)
 			//将任务的状态改为 TASK_WORKING 处理中
 			mp_laser_manager_task->set_task_statu(TASK_WORKING);
 		}
-	}
-	else
-	{
-		t_result.compare_and_cover_error(t_error);
-	}
 
-	//return 之前要填充任务单里面的错误码.
-	if (t_result != Error_code::SUCCESS)
-	{
-		//将任务的状态改为 TASK_ERROR 结束错误
-		mp_laser_manager_task->set_task_statu(TASK_ERROR);
-		//返回错误码
-		mp_laser_manager_task->set_task_error_manager(t_result);
+		//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
+		{
+			//忽略轻微故障
+			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+				//将任务的状态改为 TASK_ERROR 结束错误
+				mp_laser_manager_task->set_task_statu(TASK_ERROR);
+			}
+			//返回错误码
+			mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
+		}
 	}
+
 	return t_result;
 
 }
@@ -225,7 +231,7 @@ Error_manager Laser_manager::check_status()
 	}
 	return Error_code::SUCCESS;
 }
-//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
 Error_manager Laser_manager::end_task()
 {
 	LOG(INFO) << " ---Laser_manager::end_task run---"<< this;
@@ -234,6 +240,7 @@ Error_manager Laser_manager::end_task()
 	m_laser_manager_working_flag=false;
 	m_laser_manager_condition.notify_all(false);
 
+
 	//释放下发的任务单
 	laser_task_map_clear_and_delete();
 
@@ -353,6 +360,30 @@ Error_manager Laser_manager::issued_task(int laser_index)
 
 }
 
+//故障处理,下发任务的故障处理.
+Error_manager Laser_manager::troubleshoot_for_issued_task(Error_manager error, int laser_index)
+{
+	if (error == LASER_MANAGER_LASER_INDEX_ERRPR || error == LASER_MANAGER_LASER_INDEX_REPEAT   )
+	{
+	    //雷达索引错误,降级为可忽略的故障,然后返回轻微故障.
+		error.set_error_level_down(NEGLIGIBLE_ERROR);
+		// 此时没有创建子任务,后面等待任务返回时会忽略掉.
+		return error;
+	}
+	if (error == POINTER_IS_NULL || error == LIVOX_TASK_TYPE_ERROR   )
+	{
+		//雷达对象接受任务,发现任务单下发错误.应该是整个任务通信模块错误了.将主任务错误码升级
+		error.set_error_level_up(CRITICAL_ERROR);
+		return error;
+	}
+	//其他情况雷达对象会签收任务,之后出错会填充到任务单里面的错误码.
+	//即使接收方的状态不对,也会签收任务,然后把错误填充到任务单里面的错误码.
+	//后续在等待答复时,会统一处理子任务单所有的错误.
+
+
+	return error;
+}
+
 //mp_laser_manager_thread 线程执行函数，
 //thread_work 内部线程负责分发扫描任务给下面的雷达，并且回收汇总雷达的数据
 void Laser_manager::thread_work()
@@ -367,13 +398,19 @@ void Laser_manager::thread_work()
 		m_laser_manager_condition.wait();
 		if ( m_laser_manager_condition.is_alive() )
 		{
+			std::this_thread::yield();
+			//重新循环必须清除错误码.
+			t_error.error_manager_clear_all();
+			t_result.error_manager_clear_all();
+
 			if ( mp_laser_manager_task == NULL )
 			{
 				m_laser_manager_status = LASER_MANAGER_FAULT;
 				m_laser_manager_working_flag = false;
 				m_laser_manager_condition.notify_all(false);
 			}
-			//下发任务
+			//第一步
+			//下发任务, 雷达管理模块给子雷达下发扫描任务.
 			else if(m_laser_manager_status == LASER_MANAGER_ISSUED_TASK)
 			{
 				//分发扫描任务给下面的雷达
@@ -386,6 +423,8 @@ void Laser_manager::thread_work()
 						t_error = issued_task(i);
 						if ( t_error != SUCCESS)
 						{
+							//下发子任务的故障处理
+							t_error = troubleshoot_for_issued_task(t_error, i);
 							t_result.compare_and_cover_error(t_error);
 						}
 					}
@@ -400,14 +439,29 @@ void Laser_manager::thread_work()
 						t_error = issued_task(t_select_laser_id_vector[i]);
 						if ( t_error != SUCCESS)
 						{
+							//下发子任务的故障处理
+							t_error = troubleshoot_for_issued_task(t_error, i);
 							t_result.compare_and_cover_error(t_error);
 						}
 					}
 				}
 
-				//下发任务之后，修改状态为等待答复。
-				m_laser_manager_status = LASER_MANAGER_WAIT_REPLY;
+				//故障汇总, 只处理2级以上的故障
+				if(t_result.get_error_level() >= MINOR_ERROR)
+				{
+					m_laser_manager_status = LASER_MANAGER_FAULT;
+					mp_laser_manager_task->set_task_statu(TASK_ERROR);
+					//因为故障,而提前结束任务.
+					mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
+					end_task();
+				}
+				else
+				{
+					//下发任务之后，修改状态为等待答复。
+					m_laser_manager_status = LASER_MANAGER_WAIT_REPLY;
+				}
 			}
+			//第二步
 			//等待答复, 注意不能写成死循环阻塞,
 			//子任务没完成就直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive()),
 			// 这样线程仍然被 m_laser_manager_condition 控制
@@ -416,24 +470,33 @@ void Laser_manager::thread_work()
 				map<int, Laser_task*>::iterator map_iter;
 				for (  map_iter = m_laser_task_map.begin(); map_iter != m_laser_task_map.end(); ++map_iter)
 				{
-					if ( map_iter->second->is_over_time() )
-					{
-						//超时处理。取消任务。
-						m_laser_vector[map_iter->first]->cancel_task();
-						map_iter->second->set_task_statu(TASK_DEAD);
-						t_error.error_manager_reset(Error_code::LASER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
-													" LASER_TASK is_over_time ");
-						map_iter->second->set_task_error_manager(t_error);
-					}
+
 					if ( map_iter->second->is_task_end() == false)
 					{
-						//如果任务还在执行, 那就等待任务继续完成，等1ms
-//						std::this_thread::sleep_for(std::chrono::milliseconds(1));
-						std::this_thread::yield();
-						break;
-						//注意了:这里break之后,map_iter不会指向 m_laser_task_map.end()
-						//这就表示仍然有子任务单没有结束.那就继续等待,,,直到所有任务完成或者超时.
+						if ( map_iter->second->is_over_time() )
+						{
+							//超时处理。取消任务。
+							m_laser_vector[map_iter->first]->cancel_task();
+							map_iter->second->set_task_statu(TASK_DEAD);
+							t_error.error_manager_reset(Error_code::LASER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
+														" LASER_TASK is_over_time ");
+							map_iter->second->set_task_error_manager(t_error);
+
+							continue;
+							//本次子任务超时死亡.直接进行下一个
+						}
+						else
+						{
+							//如果任务还在执行, 那就等待任务继续完成，等1ms
+							//std::this_thread::sleep_for(std::chrono::milliseconds(1));
+							std::this_thread::yield();
+							break;
+							//注意了:这里break之后,map_iter不会指向 m_laser_task_map.end()
+							//这就表示仍然有子任务单没有结束.那就继续等待,,,直到所有任务完成或者超时.
+						}
+
 					}
+					//else 任务完成就判断下一个
 				}
 
 				//如果下发的任务全部结束，那么汇总所有的结果, 并销毁下发的任务单。
@@ -455,8 +518,8 @@ void Laser_manager::thread_work()
 					}
 
 					m_laser_manager_working_flag = false;
-					mp_laser_manager_task->set_task_error_manager(t_result);
-					//结束任务
+					mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
+					//正常结束任务
 					end_task();
 				}
 				//else 直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive())

laser/laser_manager.h

@@ -28,7 +28,7 @@
 #include "../laser/laser_manager_task.h"
 
 
-#define LASER_PARAMETER_ALL_PATH "../setting/laser.prototxt"
+#define LASER_PARAMETER_PATH "../setting/laser.prototxt"
 
 class Laser_manager:public Singleton<Laser_manager>
 {
@@ -72,7 +72,7 @@ public: //API 对外接口
 	Error_manager execute_task(Task_Base* p_laser_task);
 	//检查雷达状态,是否正常运行
 	Error_manager check_status();
-	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
 	Error_manager end_task();
 	//取消任务单，由发送方提前取消任务单
 	Error_manager cancel_task();
@@ -90,6 +90,9 @@ protected:
 	//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
 	Error_manager issued_task(int laser_index);
 
+	//故障处理,下发任务的故障处理.
+	Error_manager troubleshoot_for_issued_task(Error_manager error, int laser_index);
+
 	//mp_laser_manager_thread 线程执行函数，
 	//thread_work 内部线程负责分发扫描任务给下面的雷达，并且回收汇总雷达的数据
 	void thread_work();
@@ -100,18 +103,18 @@ protected:
 
 protected://member variable
 
-	Laser_manager_status						m_laser_manager_status;			//雷达管理模块的工作状态
+	//子雷达
 	int 										m_laser_number;					//雷达的个数
 	std::vector<Laser_base*>                    m_laser_vector;					//雷达的对象实例，内存由本类管理
 	//注意：里面的Laser_base*是在init里面new出来的，析构之前必须使用ununit来销毁
 
+	Laser_manager_status						m_laser_manager_status;			//雷达管理模块的工作状态
 	std::atomic<bool>                			m_laser_manager_working_flag;          	//雷达管理的工作使能标志位
 	map<int, Laser_task*>						m_laser_task_map;						//雷达管理下发给雷达子模块的任务map，内存由本类管理
 
 
-	std::thread*        						mp_laser_manager_thread;   		//发布信息的线程指针，内存由本类管理
-	Thread_condition							m_laser_manager_condition;		//发布信息的条件变量
-
+	std::thread*        						mp_laser_manager_thread;   		//雷达管理的线程指针，内存由本类管理
+	Thread_condition							m_laser_manager_condition;		//雷达管理的条件变量
 
 	//雷达管理的任务单指针，实际内存由发送方控制管理，//接受任务后，指向新的任务单
 	Laser_manager_task *  						mp_laser_manager_task;        		//雷达管理任务单的指针，内存由发送方管理。

laser/laser_manager_task.cpp

@@ -25,8 +25,9 @@ Laser_manager_task::Laser_manager_task()
 	m_task_frame_maxnum = 0;
 	m_task_save_flag = false;//默认不保存，false
 	m_task_save_path.clear();
-	mp_task_point_cloud = NULL;
+	mp_task_point_cloud_map = NULL;
 	mp_task_cloud_lock=NULL;
+	m_cloud_aggregation_flag = false;
 
 	m_select_all_laser_flag = true;
 	m_select_laser_id_vector.clear();
@@ -43,9 +44,9 @@ Laser_manager_task::~Laser_manager_task()
 // output:p_task_point_cloud 三维点云容器的智能指针
 //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
 Error_manager Laser_manager_task::task_init(std::mutex* p_task_cloud_lock,
-											pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
+											std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map)
 {
-	if(p_task_point_cloud.get() == NULL)
+	if(p_task_point_cloud_map == NULL)
 	{
 		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
 							 "Laser_task::task_init p_task_point_cloud is null");
@@ -64,7 +65,8 @@ Error_manager Laser_manager_task::task_init(std::mutex* p_task_cloud_lock,
 	m_task_save_flag = false;
 	m_task_save_path.clear();
 	mp_task_cloud_lock=p_task_cloud_lock;
-	mp_task_point_cloud = p_task_point_cloud;
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+	m_cloud_aggregation_flag = false;
 
 	m_select_all_laser_flag = true;
 	m_select_laser_id_vector.clear();
@@ -89,16 +91,17 @@ Error_manager Laser_manager_task::task_init(Task_statu task_statu,
 											std::string task_statu_information,
 											void* p_tast_receiver,
 											std::chrono::milliseconds task_over_time,
-											unsigned int task_frame_maxnum,
 											bool task_save_flag,
 											std::string task_save_path,
 											std::mutex* p_task_cloud_lock,
-											pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+											std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map,
+											bool cloud_aggregation_flag,
+											unsigned int task_frame_maxnum,
 											bool select_all_laser_flag,
 											std::vector<int> select_laser_id_vector
 )
 {
-	if(p_task_point_cloud.get() == NULL)
+	if(p_task_point_cloud_map == NULL)
 	{
 		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
 							 "Laser_task::task_init p_task_point_cloud is null");
@@ -119,7 +122,8 @@ Error_manager Laser_manager_task::task_init(Task_statu task_statu,
 	m_task_save_flag = task_save_flag;
 	m_task_save_path = task_save_path;
 	mp_task_cloud_lock=p_task_cloud_lock;
-	mp_task_point_cloud = p_task_point_cloud;
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+	m_cloud_aggregation_flag = cloud_aggregation_flag;
 
 	m_select_all_laser_flag = select_all_laser_flag;
 	m_select_laser_id_vector = select_laser_id_vector;
@@ -129,59 +133,67 @@ Error_manager Laser_manager_task::task_init(Task_statu task_statu,
 
 
 
-//push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
-Error_manager Laser_manager_task::task_push_point(pcl::PointXYZ point_xyz)
+//把 Laser_manager_task 转化为 Laser_task，注意 task_statu 从外部载入。
+//output：p_laser_task 雷达扫描任务的指针，
+//input：laser_index 雷达编号
+//input：p_laser_base 接受任务的 子雷达
+//input：task_statu 外部载入的任务状态，默认为 TASK_CREATED
+//input：task_over_time 超时时间，一般要比上级任务的时间短一些
+Error_manager Laser_manager_task::trans_to_laser_task(Laser_task * p_laser_task, int laser_index,Laser_base* p_laser_base,
+													  Task_statu task_statu,
+													  std::chrono::milliseconds task_over_time)
 {
-	if(mp_task_cloud_lock==NULL)
-	{
-		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
-							 "push_point laser task input lock is null");
-	}
-	if(mp_task_point_cloud.get()==NULL)
+	if ( p_laser_task ==NULL || p_laser_base == NULL )
 	{
 		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-							 "Laser_task::task_push_point p_task_point_cloud is null");
+							" trans_to_laser_task input parameter POINTER_IS_NULL ");
 	}
-	//加锁，并添加三维点。
-	mp_task_cloud_lock->lock();
-	mp_task_point_cloud->push_back(point_xyz);
-	mp_task_cloud_lock->unlock();
 
-	return SUCCESS;
-}
+	//该子任务存放在哪个cloud map
+	int map_index;
+	if ( m_cloud_aggregation_flag  )
+	{
+		//如果汇总, 就全部存放于map[0]
+		map_index = 0;
+	}
+	else
+	{
+		//如果不汇总, 就分开存放于map[laser_index]
+		map_index = laser_index;
+	}
 
-//把 Laser_manager_task 转化为 Laser_task，注意 task_statu 从外部载入。
-//output：p_laser_task 雷达扫描任务的指针，
-//inpit：task_statu 外部载入的任务状态，默认为 TASK_CREATED
-//inpit：task_over_time 超时时间，一般要比上级任务的时间短一些
-Error_manager Laser_manager_task::trans_to_laser_task(Laser_task * p_laser_task, int laser_index,Laser_base* p_laser_base,
-Task_statu task_statu, std::chrono::milliseconds task_over_time)
-{
-//	char t_buf[256] = {0};
-//	sprintf(t_buf,"%s/%d", m_task_save_path.c_str(), laser_index);
-//
-//
-//	p_laser_task->task_init(task_statu,
-//							m_task_statu_information,
-//							p_laser_base,
-//							m_task_over_time,
-//							m_task_frame_maxnum,
-//							m_task_save_flag,
-//							t_buf,
-//							mp_task_cloud_lock,
-//							mp_task_point_cloud
-//	);
-
-	p_laser_task->task_init(task_statu,
-							m_task_statu_information,
-							p_laser_base,
-							m_task_over_time,
-							m_task_frame_maxnum,
-							m_task_save_flag,
-							m_task_save_path,
-							mp_task_cloud_lock,
-							mp_task_point_cloud
-	);
+	//查看map[map_index] 是否存在
+	if ( mp_task_point_cloud_map->count(map_index) == 0 )
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		(*mp_task_point_cloud_map)[map_index] = tp_cloud;
+
+		p_laser_task->task_init(task_statu,
+								m_task_statu_information,
+								p_laser_base,
+								task_over_time,
+								m_task_frame_maxnum,
+								m_task_save_flag,
+								m_task_save_path,
+								mp_task_cloud_lock,
+								tp_cloud
+		);
+	}
+	else
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud = (*mp_task_point_cloud_map)[map_index];
+
+		p_laser_task->task_init(task_statu,
+								m_task_statu_information,
+								p_laser_base,
+								task_over_time,
+								m_task_frame_maxnum,
+								m_task_save_flag,
+								m_task_save_path,
+								mp_task_cloud_lock,
+								tp_cloud
+		);
+	}
 
 	return Error_code::SUCCESS;
 }
@@ -203,15 +215,21 @@ std::string Laser_manager_task::get_task_save_path()
 {
 	return m_task_save_path;
 }
-//获取 三维点云容器的智能指针
-std::mutex*  Laser_manager_task::get_task_cloud_lock()
+
+//获取 三维点云容器的锁
+std::mutex* Laser_manager_task::get_task_cloud_lock()
 {
 	return mp_task_cloud_lock;
 }
-//获取 三维点云容器的智能指针
-pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_manager_task::get_task_point_cloud()
+//获取 三维点云容器的智能指针map
+std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* Laser_manager_task::get_task_point_cloud_map()
 {
-	return mp_task_point_cloud;
+	return mp_task_point_cloud_map;
+}
+//获取 所有雷达的三维点云是否聚集的标志位
+bool Laser_manager_task::get_cloud_aggregation_flag()
+{
+	return m_cloud_aggregation_flag;
 }
 
 
@@ -238,6 +256,21 @@ void Laser_manager_task::set_task_save_flag_and_path(bool task_save_flag, std::s
 	m_task_save_flag=task_save_flag;
 	m_task_save_path=task_save_path;
 }
+//设置 三维点云容器的锁
+void Laser_manager_task::set_task_cloud_lock(std::mutex* mp_task_cloud_lock)
+{
+	mp_task_cloud_lock = mp_task_cloud_lock;
+}
+//设置 三维点云容器的智能指针
+void Laser_manager_task::set_task_point_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map)
+{
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+}
+//设置 所有雷达的三维点云是否聚集的标志位
+void Laser_manager_task::set_cloud_aggregation_flag(bool cloud_aggregation_flag)
+{
+	m_cloud_aggregation_flag = cloud_aggregation_flag;
+}
 
 //获取 是否选取所有的雷达
 bool Laser_manager_task::get_select_all_laser_flag()

laser/laser_manager_task.h

@@ -43,7 +43,7 @@ public://API functions
 	// output:p_task_point_cloud 三维点云容器的智能指针
 	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
 	Error_manager task_init(std::mutex* p_task_cloud_lock,
-							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
+							std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map);
 
 	//初始化任务单，必须初始化之后才可以使用，（可选参数）
 	//    input:task_statu 任务状态
@@ -62,25 +62,24 @@ public://API functions
 							std::string task_statu_information,
 							void* p_tast_receiver,
 							std::chrono::milliseconds task_over_time,
-							unsigned int task_frame_maxnum,
 							bool task_save_flag,
 							std::string task_save_path,
 							std::mutex* p_task_cloud_lock,
-							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+							std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map,
+							bool cloud_aggregation_flag,
+							unsigned int task_frame_maxnum,
 							bool is_select_all_laser,
 							std::vector<int> select_laser_id_vector
 	);
 
-	//push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
-	Error_manager task_push_point(pcl::PointXYZ point_xyz);
-
 	//把 Laser_manager_task 转化为 Laser_task，注意 task_statu 从外部载入。
 	//output：p_laser_task 雷达扫描任务的指针，
-	//inpit：laser_index 雷达编号
-	//inpit：task_statu 外部载入的任务状态，默认为 TASK_CREATED
-	//inpit：task_over_time 超时时间，一般要比上级任务的时间短一些
+	//input：laser_index 雷达编号
+	//input：p_laser_base 接受任务的 子雷达
+	//input：task_statu 外部载入的任务状态，默认为 TASK_CREATED
+	//input：task_over_time 超时时间，一般要比上级任务的时间短一些
 	Error_manager trans_to_laser_task(Laser_task * p_laser_task, int laser_index,Laser_base* p_laser_base,
-	Task_statu task_statu = TASK_CREATED,
+									  Task_statu task_statu = TASK_CREATED,
 									  std::chrono::milliseconds task_over_time = std::chrono::milliseconds(TASK_OVER_TIME_DEFAULT));
 
 public://get or set member variable
@@ -92,9 +91,10 @@ public://get or set member variable
 	std::string get_task_save_path();
 	//获取 三维点云容器的智能指针
 	std::mutex*  get_task_cloud_lock();
-	//获取 三维点云容器的智能指针
-	pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
-
+	//获取 三维点云容器的智能指针map
+	std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* get_task_point_cloud_map();
+	//获取 所有雷达的三维点云是否聚集的标志位
+	bool get_cloud_aggregation_flag();
 
 
 	//设置 点云的采集帧数最大值
@@ -105,7 +105,12 @@ public://get or set member variable
 	void set_task_save_path(std::string task_save_path);
 	//设置采集的点云保存标志位和路径
 	void set_task_save_flag_and_path(bool task_save_flag, std::string task_save_path);
-
+	//设置 三维点云容器的锁
+	void set_task_cloud_lock(std::mutex* p_task_cloud_lock);
+	//设置 三维点云容器的智能指针
+	void set_task_point_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map);
+	//设置 所有雷达的三维点云是否聚集的标志位
+	void set_cloud_aggregation_flag(bool cloud_aggregation_flag);
 
 
 	//获取 是否选取所有的雷达
@@ -128,9 +133,13 @@ protected://member variable
 
 	//三维点云的数据保护锁，任务输入
 	std::mutex*                     mp_task_cloud_lock;
-	//采集结果，三维点云容器的智能指针，任务输出
-	//这里只是智能指针，实际内存由 任务发送方管理，初始化时必须保证内存有效。
-	pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+	//采集结果，三维点云容器的map指针，任务输出, map的内存由发送方管理,
+	//map内部是空的, 处理任务过程再往里面push点云
+	std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>*		mp_task_point_cloud_map;
+	//所有雷达的三维点云是否聚集的标志位,
+	bool 										m_cloud_aggregation_flag;
+	//m_cloud_aggregation_flag==true :: mp_task_point_cloud_map里面只有一个cloud
+	//m_cloud_aggregation_flag==false:: mp_task_point_cloud_map里面有很多个cloud, 每个雷达的数据都是独立的.
 
 
 	bool 							m_select_all_laser_flag;					//是否选取所有的雷达。默认为true

laser/laser_task_command.cpp

@@ -20,7 +20,7 @@ Laser_task::Laser_task()
 	m_task_save_flag = false;//默认不保存，false
 	m_task_save_path.clear();
     mp_task_point_cloud = NULL;
-    mp_task_cloud_lock=NULL;
+    mp_task_cloud_lock = NULL;
 }
 //析构函数
 Laser_task::~Laser_task()

laser/laser_task_command.h

@@ -49,7 +49,7 @@ public:
 	//    input:task_save_flag 是否保存
 	//    input:task_save_path 保存路径
 	//    input:p_task_cloud_lock 三维点云的数据保护锁
-	//    output:p_task_point_cloud 三维点云容器的智能指针
+	//    output:p_task_pmutexoint_cloud 三维点云容器的智能指针
 	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
 	Error_manager task_init(Task_statu task_statu,
 							std::string task_statu_information,
@@ -79,9 +79,6 @@ public:
     pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
 
 
-
-
-
     //设置 点云的采集帧数最大值
     void set_task_frame_maxnum(unsigned int task_frame_maxnum);
 	//设置采集的点云保存文件的使能标志位
@@ -103,7 +100,7 @@ protected:
     //点云保存文件的路径，任务输入
     std::string                     m_task_save_path;
 
-    //三维点云的数据保护锁，任务输入
+    //三维点云的数据保护锁，任务输入, (实际上多个雷达会同时往一个pcl点云容器里面填充数据,防止数据冲突,需要对容器加锁.)
     std::mutex*                     mp_task_cloud_lock;
     //采集结果，三维点云容器的智能指针，任务输出
     //这里只是智能指针，实际内存由任务发送方管理管理，初始化时必须保证内存有效。

laser/livox_driver.cpp

@@ -334,7 +334,6 @@ void Livox_driver::livox_get_extrinsic_parameter_callback(uint8_t status,
 			std::cos(t_roll) * std::cos(t_pitch),
 			t_z
 			};
-
 			//设置变换矩阵
 			tp_livox_laser->set_laser_matrix(rotate, 12);
 		}

locate/cnn3d_segmentation.cpp

@@ -0,0 +1,704 @@
+#include "cnn3d_segmentation.h"
+#include "tf_wheel_3Dcnn_api.h"
+#include <iostream>
+#include <glog/logging.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/filters/project_inliers.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/features/moment_of_inertia_estimation.h>
+#include <pcl/common/centroid.h>
+void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>::Ptr pCloud)
+{
+	std::ofstream os;
+	os.open(txt, std::ios::out);
+	for (int i = 0; i < pCloud->points.size(); i++)
+	{
+		pcl::PointXYZRGB point = pCloud->points[i];
+		char buf[255];
+		memset(buf, 0, 255);
+		sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
+		os.write(buf, strlen(buf));
+	}
+	os.close();
+}
+
+double Cnn3d_segmentation::distance(cv::Point2f p1, cv::Point2f p2)
+{
+	return sqrt(pow(p1.x - p2.x, 2.0) + pow(p1.y - p2.y, 2.0));
+}
+
+cv::RotatedRect minAreaRect_cloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
+{
+	std::vector<cv::Point2f> points;
+	for (int i = 0; i < cloud->size(); ++i)
+	{
+		pcl::PointXYZRGB point = cloud->points[i];
+		points.push_back(cv::Point2f(point.x, point.y));
+	}
+	cv::RotatedRect rect= cv::minAreaRect(points);
+	return rect;
+}
+
+cv::RotatedRect minAreaRect_cloud(std::vector<cv::Point2f> centers)
+{
+	cv::RotatedRect rect = cv::minAreaRect(centers);
+	return rect;
+}
+
+Error_manager Cnn3d_segmentation::set_parameter(int l, int w, int h,int freq,int classes)
+{
+	m_lenth = l;
+	m_width = w;
+	m_height = h;
+	m_freq = freq;
+	m_nClass = classes;
+	return SUCCESS;
+}
+Cnn3d_segmentation::Cnn3d_segmentation(int l, int w, int h,int freq,int classes)
+{
+	m_lenth = l;
+	m_width = w;
+	m_height = h;
+	m_freq = freq;
+	m_nClass = classes;
+}
+
+Cnn3d_segmentation::~Cnn3d_segmentation()
+{
+	//3dcnn 原校验功能保留,不加载网络
+	//tf_wheel_3dcnn_close();
+}
+
+Error_manager Cnn3d_segmentation::init(std::string weights)
+{
+	//3dcnn 原校验功能保留,不加载网络
+	return SUCCESS;
+	/*if (0 != tf_wheel_3dcnn_init(weights, m_lenth,m_width,m_height))
+	{
+		std::string error_description="tf_wheel_3Dcnn model load failed :";
+		error_description+=weights;
+		return Error_manager(LOCATER_3DCNN_INIT_FAILED,MINOR_ERROR,error_description);
+	}
+    //空跑一次
+	float* data = (float*)malloc(m_lenth*m_width*m_height*sizeof(float));
+	float* pout = (float*)malloc(m_lenth*m_width*m_height*m_nClass*sizeof(float));
+
+	if (!tf_wheel_3dcnn_predict(data, pout))
+	{
+		free(data);
+		free(pout);
+		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,MINOR_ERROR,"first locate 3dcnn failed");
+	}
+	free(data);
+	free(pout);
+	return SUCCESS;*/
+}
+
+
+void save_cloudT(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& pCloud)
+{
+	std::ofstream os;
+	os.open(txt, std::ios::out);
+	for (int i = 0; i < pCloud.points.size(); i++)
+	{
+		pcl::PointXYZRGB point = pCloud.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();
+}
+
+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)
+{
+	Error_manager t_error;
+	if ( p_locate_information == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " analytic_wheel POINTER_IS_NULL ");
+	}
+
+	//第一步 离群点过滤
+	t_error = filter_cloud_with_outlier_for_map(wheel_cloud_map);
+	if ( t_error != Error_code::SUCCESS)
+	{
+		return t_error;
+	}
+
+	//第二步,	//将map分为4个轮胎
+	//注: 前面的雷达扫描并point sift之后, 可能只能识别3个轮胎, 此时允许3个轮胎 继续运行.
+	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> wheel_source_vector; //拆分后的轮胎点云, 不同的轮胎点是分开的,
+	t_error = split_wheel_cloud(wheel_cloud_map, cloud_aggregation_flag, wheel_source_vector, save_flag, save_dir);
+	if ( t_error != Error_code::SUCCESS)
+	{
+		return t_error;
+	}
+
+
+	//第三步, pca过滤, 并计算每个轮子的中心点
+	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> wheel_pca_vector;	//pca过滤后的点云
+	t_error = filter_cloud_with_pca(wheel_source_vector, wheel_pca_vector, save_flag, save_dir);
+	if ( t_error != Error_code::SUCCESS)
+	{
+		return t_error;
+	}
+
+
+	//第四步,提取关键矩形,
+	cv::RotatedRect rect_of_wheel_center; 	//4个轮胎中心点构成的矩形
+	cv::RotatedRect rect_of_wheel_cloud;	//所有点云构成的矩形
+	t_error = extract_key_rect(wheel_pca_vector, rect_of_wheel_center, rect_of_wheel_cloud, save_flag, save_dir);
+	if ( t_error != Error_code::SUCCESS)
+	{
+		return t_error;
+	}
+
+	//第五步,	//通过关键矩形, 计算出车辆的定位信息, (输出中心点的x和y, 前后轮距, 左右轮距, 旋转角)
+	t_error = calculate_key_rect(rect_of_wheel_center, rect_of_wheel_cloud, p_locate_information);
+	if ( t_error != Error_code::SUCCESS)
+	{
+		return t_error;
+	}
+
+	return Error_code::SUCCESS;
+}
+
+
+//过滤离群点,
+Error_manager Cnn3d_segmentation::filter_cloud_with_outlier_for_map(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map)
+{
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//遍历map, 对点云进行 离群点过滤
+	for (auto iter = wheel_cloud_map.begin(); iter != wheel_cloud_map.end(); ++iter)
+	{
+		//过滤离群点,
+		t_error = filter_cloud_with_outlier(iter->second);
+		if (t_error != Error_code::SUCCESS)
+		{
+			char buf[256] = {0};
+			sprintf(buf, " map.id = %d, filter_cloud_with_outlier error", iter->first );
+			t_error.add_error_description(buf, strlen(buf) );
+			t_result.compare_and_cover_error(t_error);
+			//注:这里不直接return,而是要把map全部执行完成
+		}
+	}
+	if ( t_result != Error_code::SUCCESS)
+	{
+		return t_result;
+	}
+	return Error_code::SUCCESS;
+}
+//过滤离群点,
+Error_manager Cnn3d_segmentation::filter_cloud_with_outlier(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud)
+{
+	if(p_cloud.get()==NULL)
+	{
+		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,MINOR_ERROR,"Cnn3d_segmentation input cloud uninit");
+	}
+	if(p_cloud->size()<=0)
+	{
+		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,MINOR_ERROR,"Cnn3d_segmentation input cloud empty");
+	}
+
+	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sorfilter(true); // Initializing with true will allow us to extract the removed indices
+	sorfilter.setInputCloud(p_cloud);
+	sorfilter.setMeanK(20);
+	sorfilter.setStddevMulThresh(2.0);
+	sorfilter.filter(*p_cloud);
+
+	return Error_code::SUCCESS;
+}
+
+//拆分车轮, 将map分为4个轮胎
+//wheel_cloud_map, 输入点云
+//cloud_out_vector, 输出4轮点云vector, (vector是空的, 函数内部会添加4个点云)
+Error_manager Cnn3d_segmentation::split_wheel_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map, bool cloud_aggregation_flag,
+													std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & wheel_cloud_vector,
+													bool save_flag, std::string save_dir)
+{
+	Error_manager t_error;
+	wheel_cloud_vector.clear();
+
+	//如果输入的map只有一个cloud, 那么就要进行聚类, 拆分为4个轮胎
+	if ( wheel_cloud_map.size() == 1 && cloud_aggregation_flag == true )
+	{
+		t_error = split_cloud_with_kmeans(wheel_cloud_map[0], wheel_cloud_vector, save_flag, save_dir);
+		//注:此时wheel_clouds_vector里面可能有一个的点云为空的,
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+		//如果输入的map有3~4个cloud, 那么就直接复制,
+	else if ( (wheel_cloud_map.size() == CNN3D_WHEEL_NUMBER || wheel_cloud_map.size() == CNN3D_WHEEL_NUMBER - 1 )
+			  && cloud_aggregation_flag == false )
+	{
+		for (auto iter = wheel_cloud_map.begin(); iter != wheel_cloud_map.end(); ++iter)
+		{
+			wheel_cloud_vector.push_back(iter->second);
+			//注:此时的 clouds_vector 可能有3~4个点云,
+		}
+	}
+	else
+	{
+		return Error_manager(Error_code::LOCATER_3DCNN_INPUT_CLOUD_MAP_ERROR, Error_level::MINOR_ERROR,
+							 " split_wheel_cloud p_wheel_cloud_map size error ");
+	}
+	return Error_code::SUCCESS;
+}
+//聚类, 将一个车轮点云拆分为4个轮胎点云,
+//p_cloud_in, 输入点云
+//cloud_out_vector, 输出4轮点云vector, (vector是空的, 函数内部会添加4个点云)
+Error_manager Cnn3d_segmentation::split_cloud_with_kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_in,
+														  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_out_vector,
+														  bool save_flag, std::string save_dir)
+{
+	if(p_cloud_in.get()==NULL)
+	{
+		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,MINOR_ERROR,"Cnn3d_segmentation input cloud uninit");
+	}
+	if(p_cloud_in->size()<=0)
+	{
+		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,MINOR_ERROR,"Cnn3d_segmentation input cloud empty");
+	}
+
+	//为cloud_out_vector重新分配内存, 添加4个轮胎点云
+	cloud_out_vector.clear();
+	for(int i=0;i<CNN3D_WHEEL_NUMBER;++i)
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+		cloud_out_vector.push_back(t_cloud);
+	}
+
+	//开始分割,
+	//注: 实际上只考虑了x和y轴, 以水平面为标准,
+	// 根据每个点到的4个角落的距离, 分为4个车轮
+
+	//p_cloud_in转化为opencv的点云
+	std::vector<cv::Point2f> cv_points;
+	for (int i = 0; i < p_cloud_in->size(); ++i)
+	{
+		cv::Point2f point(p_cloud_in->points[i].x, p_cloud_in->points[i].y);
+		cv_points.push_back(point);
+	}
+	//rect 为输入点云的外截最小矩形
+	cv::RotatedRect rect = cv::minAreaRect(cv_points);
+	//corner 为矩形的4个角落的点
+	cv::Point2f corner[CNN3D_WHEEL_NUMBER];
+	rect.points(corner);
+
+	//遍历输入点云
+	for (int i = 0; i < p_cloud_in->size(); ++i)
+	{
+		//计算出当前点距离哪个角落最近,
+		double min_distance = 9999999.9;		//当前点距离角落的最小距离
+		int cluste_index = 0;					//当前点最近角落的索引编号
+		cv::Point2f point(p_cloud_in->points[i].x, p_cloud_in->points[i].y);
+		for (int j = 0; j < CNN3D_WHEEL_NUMBER; ++j)
+		{
+			double dis=distance(point, corner[j]);
+			if (dis < min_distance)
+			{
+				min_distance = dis;
+				cluste_index = j;
+			}
+		}
+
+		//根据点位的归属, 将其重新标色并填充到对应的输出点云
+		if (cluste_index == 0)
+		{
+			p_cloud_in->points[i].r = 255;
+			p_cloud_in->points[i].g = 0;
+			p_cloud_in->points[i].b = 0;
+		}
+		else if (cluste_index == 1)
+		{
+			p_cloud_in->points[i].r = 0;
+			p_cloud_in->points[i].g = 255;
+			p_cloud_in->points[i].b = 0;
+		}
+		else if (cluste_index == 2)
+		{
+			p_cloud_in->points[i].r = 0;
+			p_cloud_in->points[i].g = 0;
+			p_cloud_in->points[i].b = 255;
+		}
+		else
+		{
+			p_cloud_in->points[i].r = 255;
+			p_cloud_in->points[i].g = 255;
+			p_cloud_in->points[i].b = 255;
+		}
+		cloud_out_vector[cluste_index]->push_back(p_cloud_in->points[i]);
+	}
+
+	//保存修改颜色后的点云.
+	if ( save_flag )
+	{
+		std::string save_file=save_dir+"/cnn3d_with_kmeans.txt";
+		save_rgb_cloud_txt(save_file, p_cloud_in);
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//pca, 使用pca算法对每个车轮的点云进行过滤,
+Error_manager Cnn3d_segmentation::filter_cloud_with_pca(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_in_vector,
+													   std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_out_vector,
+													   bool save_flag, std::string save_dir)
+{
+	Error_manager t_error;
+	cloud_out_vector.clear();
+
+	for (int i = 0; i < cloud_in_vector.size(); ++i)
+	{
+		if ( cloud_in_vector[i]->size() != 0 )
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tp_cloud_pca(new pcl::PointCloud<pcl::PointXYZRGB>);
+			//对点云作pca分析,然后在最小轴方向过滤, 防止轮胎左右两边出现噪声
+			t_error = pca_minist_axis_filter(cloud_in_vector[i],tp_cloud_pca);
+			if ( t_error != Error_code::SUCCESS )
+			{
+				return t_error;
+			}
+			cloud_out_vector.push_back(tp_cloud_pca);
+
+			if ( save_flag )
+			{
+				char buf[255]={0};
+				sprintf(buf,"%s/cnn3d_pca_wheel_%d.txt",save_dir.c_str(),i);
+				save_cloudT(buf,*tp_cloud_pca);
+			}
+		}
+	}
+	//验证 vector 的大小
+	if ( cloud_out_vector.size() <  CNN3D_WHEEL_NUMBER - 1)
+	{
+		return Error_manager(Error_code::LOCATER_3DCNN_PCA_OUT_ERROR, Error_level::MINOR_ERROR,
+							 " wheel_pca_vector.size() < 3  ");
+	}
+	return Error_code::SUCCESS;
+}
+//对点云作pca分析,然后在最小轴方向过滤, 防止轮胎左右两边出现噪声
+Error_manager Cnn3d_segmentation::pca_minist_axis_filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_in,
+														 pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_out)
+{
+	if(p_cloud_in.get()==NULL)
+	{
+		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,MINOR_ERROR,"Cnn3d_segmentation input cloud uninit");
+	}
+	if(p_cloud_in->size()<=0)
+	{
+		return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,MINOR_ERROR,"Cnn3d_segmentation input cloud empty");
+	}
+
+	//pca寻找主轴
+	pcl::MomentOfInertiaEstimation <pcl::PointXYZRGB> feature_extractor;
+	feature_extractor.setInputCloud (p_cloud_in);
+	feature_extractor.compute ();
+
+	std::vector <float> moment_of_inertia;
+	std::vector <float> eccentricity;
+	pcl::PointXYZRGB min_point_AABB;
+	pcl::PointXYZRGB max_point_AABB;
+	pcl::PointXYZRGB min_point_OBB;
+	pcl::PointXYZRGB max_point_OBB;
+	pcl::PointXYZRGB position_OBB;
+	Eigen::Matrix3f rotational_matrix_OBB;
+	float major_value, middle_value, minor_value;
+	Eigen::Vector3f major_vector, middle_vector, minor_vector;
+	Eigen::Vector3f mass_center;
+
+	feature_extractor.getMomentOfInertia (moment_of_inertia);
+	feature_extractor.getEccentricity (eccentricity);
+	feature_extractor.getAABB (min_point_AABB, max_point_AABB);
+	feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
+	feature_extractor.getEigenValues (major_value, middle_value, minor_value);
+	feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
+	feature_extractor.getMassCenter (mass_center);
+
+	//根据minor_vector 最小主轴方向,以及中心点, 计算轴所在三维平面
+	float x=mass_center[0];
+	float y=mass_center[1];
+	float z=mass_center[2];
+	float a=minor_vector[0];
+	float b=minor_vector[1];
+	float c=minor_vector[2];
+	float d=-(a*x+b*y+c*z);
+
+	//计算各点距离平面的距离,距离操过轮胎的一半厚,舍弃(100mm)
+	float S=sqrt(a*a+b*b+c*c);
+	for(int i=0;i<p_cloud_in->size();++i)
+	{
+		pcl::PointXYZRGB point=p_cloud_in->points[i];
+		if(fabs(point.x*a+point.y*b+point.z*c+d)/S <40 )
+		{
+			p_cloud_out->push_back(point);
+		}
+	}
+
+	/*if(cloud_out->size()==0)
+	{
+		return Error_manager(LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,MINOR_ERROR,"wheel pca filter out cloud empty");
+	}*/
+	return SUCCESS;
+}
+
+//提取关键矩形,
+//cloud_in_vector, 输入点云
+//rect_of_wheel_center, 输出4轮中心点构成的矩形
+//rect_of_wheel_cloud, 输出所有车轮点构成的矩形
+Error_manager Cnn3d_segmentation::extract_key_rect(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_in_vector,
+												   cv::RotatedRect & rect_of_wheel_center, cv::RotatedRect & rect_of_wheel_cloud,
+												   bool save_flag, std::string save_dir)
+{
+	//验证 vector 的大小
+	if ( cloud_in_vector.size() <  CNN3D_WHEEL_NUMBER - 1)
+	{
+		return Error_manager(Error_code::LOCATER_3DCNN_EXTRACT_RECT_ERROR, Error_level::MINOR_ERROR,
+							 " cloud_in_vector.size() < 3  ");
+	}
+
+	Error_manager t_error;
+
+	//4个轮子的中心点, 只要x和y轴的坐标,
+	std::vector<cv::Point2f> wheel_center_vector;
+	//取出轮胎的中心点
+	for (int i = 0; i < cloud_in_vector.size(); ++i)
+	{
+		if ( cloud_in_vector[i]->size() != 0 )
+		{
+			//提取每个车轮中心点
+			Eigen::Vector4f centroid;
+			pcl::compute3DCentroid(*(cloud_in_vector[i]), centroid);
+
+			wheel_center_vector.push_back(cv::Point2f(centroid[0],centroid[1]));
+		}
+	}
+	//验证 vector 的大小
+	int t_vector_size = wheel_center_vector.size();
+	if ( wheel_center_vector.size() <  CNN3D_WHEEL_NUMBER - 1)
+	{
+		return Error_manager(Error_code::LOCATER_3DCNN_EXTRACT_RECT_ERROR, Error_level::MINOR_ERROR,
+							 " wheel_center_vector.size() < 3  ");
+	}
+
+	//检查并修复关键点, 此时可以通过3个轮胎模拟出第四个轮胎的中心点位置, 并添加到 vector
+//	t_error = check_and_repair_center(wheel_center_vector);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	//rect_of_wheel_center, 输出4轮中心点构成的矩形
+	rect_of_wheel_center = minAreaRect_cloud(wheel_center_vector);
+//	t_error = check_rect_size(rect_of_wheel_center);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	//将输入的车轮点云, 汇总为 cv::Point2f的vector
+	std::vector<cv::Point2f> wheel_points_vector;
+	for(int i=0;i<cloud_in_vector.size();++i)
+	{
+		for(int j=0;j<cloud_in_vector[i]->size();++j)
+			wheel_points_vector.push_back(cv::Point2f(cloud_in_vector[i]->points[j].x,
+													  cloud_in_vector[i]->points[j].y));
+	}
+	//如果前面check_and_repair_center为wheel_center_vector添加了第四个中心点, 那么也要加入到wheel_points_vector
+	if(t_vector_size == CNN3D_WHEEL_NUMBER - 1 &&  wheel_center_vector.size() == CNN3D_WHEEL_NUMBER)
+	{
+		wheel_points_vector.push_back(wheel_center_vector[CNN3D_WHEEL_NUMBER - 1]);
+	}
+
+	//rect_of_wheel_cloud, 输出所有车轮点构成的矩形
+	rect_of_wheel_cloud = minAreaRect_cloud(wheel_points_vector);
+
+
+	return Error_code::SUCCESS;
+}
+
+//检查并修复关键点, 此时可以通过3个轮胎模拟出第四个轮胎的中心点位置, 并添加到 vector
+Error_manager Cnn3d_segmentation::check_and_repair_center(std::vector<cv::Point2f> & cloud_centers)
+{
+	//取四轮中心计算轴长
+	return centers_is_rect(cloud_centers);
+}
+//判断矩形框是否找到
+//points:输入轮子中心点, 只能是3或者4个
+Error_manager Cnn3d_segmentation::centers_is_rect(std::vector<cv::Point2f>& points)
+{
+	if (points.size() == 4)
+	{
+		double L[3] = {0.0};
+		L[0] = distance(points[0], points[1]);
+		L[1] = distance(points[1], points[2]);
+		L[2] = distance(points[0], points[2]);
+		double max_l = L[0];
+		double l1 = L[1];
+		double l2 = L[2];
+		int max_index = 0;
+		cv::Point2f ps = points[0], pt = points[1];
+		cv::Point2f pc = points[2];
+		for (int i = 1; i < 3; ++i) {
+			if (L[i] > max_l) {
+				max_index = i;
+				max_l = L[i];
+				l1 = L[abs(i + 1) % 3];
+				l2 = L[abs(i + 2) % 3];
+				ps = points[i % 3];
+				pt = points[(i + 1) % 3];
+				pc = points[(i + 2) % 3];
+			}
+		}
+		double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+		if (fabs(cosa) >= 0.13) {
+			char description[255]={0};
+			sprintf(description,"angle cos value(%.2f) >0.13 ",cosa);
+			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_4,MINOR_ERROR,description);
+		}
+
+		float width=std::min(l1,l2);
+		float length=std::max(l1,l2);
+		if(width<1400 || width >2000 || length >3300 ||length < 2200)
+		{
+			char description[255]={0};
+			sprintf(description,"width<1400 || width >2100 || length >3300 ||length < 2100 l:%.1f,w:%.1f",length,width);
+			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_4,MINOR_ERROR,description);
+		}
+
+		double d = distance(pc, points[3]);
+		cv::Point2f center1 = (ps + pt) * 0.5;
+		cv::Point2f center2 = (pc + points[3]) * 0.5;
+		if (fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 150) {
+			LOG(INFO) << "d:" << d << " maxl:" << max_l << "  center1:" << center1 << "  center2:" << center2;
+			char description[255]={0};
+			sprintf(description,"Verify failed-4  fabs(d - max_l) > max_l * 0.1 || distance(center1, center2) > 150 ");
+			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_4,MINOR_ERROR,description);
+		}
+		LOG(INFO) << " rectangle verify OK  cos angle=" << cosa << "  length off=" << fabs(d - max_l)
+				  << "  center distance=" << distance(center1, center2);
+		return SUCCESS;
+	}
+	else if(points.size()==3)
+	{
+		double L[3] = {0.0};
+		L[0] = distance(points[0], points[1]);
+		L[1] = distance(points[1], points[2]);
+		L[2] = distance(points[0], points[2]);
+		double max_l = L[0];
+		double l1 = L[1];
+		double l2 = L[2];
+		int max_index = 0;
+		cv::Point2f ps = points[0], pt = points[1];
+		cv::Point2f pc = points[2];
+		for (int i = 1; i < 3; ++i) {
+			if (L[i] > max_l) {
+				max_index = i;
+				max_l = L[i];
+				l1 = L[abs(i + 1) % 3];
+				l2 = L[abs(i + 2) % 3];
+				ps = points[i % 3];
+				pt = points[(i + 1) % 3];
+				pc = points[(i + 2) % 3];
+			}
+		}
+		double cosa = (l1 * l1 + l2 * l2 - max_l * max_l) / (2.0 * l1 * l2);
+		if (fabs(cosa) >= 0.12) {
+			char description[255]={0};
+			sprintf(description,"3 wheels angle cos value(%.2f) >0.12 ",cosa);
+			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_3,MINOR_ERROR,description);
+		}
+
+		double l=std::max(l1,l2);
+		double w=std::min(l1,l2);
+		if(l>2100 && l<3300 && w>1400 && w<2100)
+		{
+			//生成第四个点
+			cv::Point2f vec1=ps-pc;
+			cv::Point2f vec2=pt-pc;
+			cv::Point2f point4=(vec1+vec2)+pc;
+			points.push_back(point4);
+			LOG(INFO) << "3 wheels rectangle verify OK  cos angle=" << cosa << "  L=" << l
+					  << "  w=" << w;
+			return SUCCESS;
+		}
+		else
+		{
+
+			char description[255]={0};
+			sprintf(description,"3 wheels rectangle verify Failed  cos angle=%.2f,L=%.1f, w=%.1f",cosa,l,w);
+			return Error_manager(LOCATER_3DCNN_VERIFY_RECT_FAILED_3,MINOR_ERROR,description);
+		}
+
+	}
+	return Error_code::SUCCESS;
+}
+
+//检查矩形的大小
+Error_manager Cnn3d_segmentation::check_rect_size(cv::RotatedRect& rect)
+{
+	if ( std::min(rect.size.width, rect.size.height)>2.000 || std::min(rect.size.width, rect.size.height)<1.200)
+	{
+		return Error_manager(Error_code::LOCATER_3DCNN_RECT_SIZE_ERROR, Error_level::MINOR_ERROR,
+							 " check_rect_size error ");
+	}
+	if (std::max(rect.size.width, rect.size.height) < 1.900 || std::max(rect.size.width, rect.size.height) > 3.600)
+	{
+		return Error_manager(Error_code::LOCATER_3DCNN_RECT_SIZE_ERROR, Error_level::MINOR_ERROR,
+							" check_rect_size error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+//通过关键矩形, 计算出车辆的定位信息, (输出中心点的x和y, 前后轮距, 左右轮距, 旋转角)
+Error_manager Cnn3d_segmentation::calculate_key_rect(cv::RotatedRect & rect_of_wheel_center, cv::RotatedRect & rect_of_wheel_cloud,
+								 Locate_information* p_locate_information)
+{
+	if ( p_locate_information == NULL )
+	{
+	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+	    					" calculate_key_rect input parameter POINTER_IS_NULL ");
+	}
+
+	//计算中心点
+	p_locate_information->locate_x=rect_of_wheel_center.center.x;
+	p_locate_information->locate_y=rect_of_wheel_center.center.y;
+	//计算轮距, 矩形的长边为前后轮距, 短边为左右轮距
+	p_locate_information->locate_wheel_base=std::max(rect_of_wheel_center.size.height,rect_of_wheel_center.size.width);
+	p_locate_information->locate_wheel_width=std::min(rect_of_wheel_center.size.height,rect_of_wheel_center.size.width);
+	//计算旋转角
+	const double C_PI=3.14159265;
+	cv::Point2f vec;
+	cv::Point2f vertice[4];
+	rect_of_wheel_cloud.points(vertice);
+	float len1 = pow(vertice[0].x - vertice[1].x, 2.0) + pow(vertice[0].y - vertice[1].y, 2.0);
+	float len2 = pow(vertice[1].x - vertice[2].x, 2.0) + pow(vertice[1].y - vertice[2].y, 2.0);
+	if (len1 > len2) {
+		vec.x = vertice[0].x - vertice[1].x;
+		vec.y = vertice[0].y - vertice[1].y;
+	}
+	else
+	{
+		vec.x = vertice[1].x - vertice[2].x;
+		vec.y = vertice[1].y - vertice[2].y;
+	}
+
+	p_locate_information->locate_angle = 180.0 / C_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
+
+	LOG(INFO)<<"3dcnn rotate rect center :"<<rect_of_wheel_center.center<<"   size : "<<rect_of_wheel_center.size<<"  angle:"<<p_locate_information->locate_angle;
+
+	return SUCCESS;
+}
+
+
+

locate/cnn3d_segmentation.h

@@ -0,0 +1,90 @@
+#ifndef __3DCNN__LOCATER__HH_
+#define __3DCNN__LOCATER__HH_
+#include "opencv/highgui.h"
+#include "opencv2/opencv.hpp"
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include <string>
+#include "../error_code/error_code.h"
+#include "locate_manager_task.h"
+
+
+
+//Cnn3d 解析 车轮的数量, 默认为4个轮胎
+#define CNN3D_WHEEL_NUMBER 4
+
+//3dcnn网络解析轮胎点
+class  Cnn3d_segmentation
+{
+public:
+	Cnn3d_segmentation(int l,int w,int h,int freq,int nClass);
+	//设置3dcnn网络参数
+    Error_manager set_parameter(int l, int w, int h,int freq,int classes);
+	virtual ~Cnn3d_segmentation();
+	//初始化网络参数
+	//weights:参数文件
+	virtual Error_manager init(std::string weights);
+
+	//解析4个轮胎
+	virtual Error_manager 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);
+
+
+protected:
+	//过滤离群点,
+	Error_manager filter_cloud_with_outlier_for_map(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map);
+	//过滤离群点,
+	Error_manager filter_cloud_with_outlier(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud);
+
+	//拆分车轮, 将map分为4个轮胎
+	//wheel_cloud_map, 输入点云
+	//cloud_out_vector, 输出4轮点云vector, (vector是空的, 函数内部会添加4个点云)
+	Error_manager split_wheel_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& wheel_cloud_map, bool cloud_aggregation_flag,
+										  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & wheel_cloud_vector,
+										  bool save_flag, std::string save_dir);
+	//聚类, 将一个车轮点云拆分为4个轮胎点云,
+	//p_cloud_in, 输入点云
+	//cloud_out_vector, 输出4轮点云vector, (vector是空的, 函数内部会添加4个点云)
+	Error_manager split_cloud_with_kmeans(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_in,
+										  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_out_vector,
+										  bool save_flag, std::string save_dir);
+
+	//pca, 使用pca算法对每个车轮的点云进行过滤,
+	Error_manager filter_cloud_with_pca(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_in_vector,
+										  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_out_vector,
+									   bool save_flag, std::string save_dir);
+	//对点云作pca分析,然后在最小轴方向过滤, 防止轮胎左右两边出现噪声
+	Error_manager pca_minist_axis_filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_in,
+										 pcl::PointCloud<pcl::PointXYZRGB>::Ptr p_cloud_out);
+
+	//提取关键矩形,
+	//cloud_in_vector, 输入点云
+	//rect_of_wheel_center, 输出4轮中心点构成的矩形
+	//rect_of_wheel_cloud, 输出所有车轮点构成的矩形
+	Error_manager extract_key_rect(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & cloud_in_vector,
+								   cv::RotatedRect & rect_of_wheel_center, cv::RotatedRect & rect_of_wheel_cloud,
+								   bool save_flag, std::string save_dir);
+	//检查并修复关键点, 此时可以通过3个轮胎模拟出第四个轮胎的中心点位置,
+	Error_manager check_and_repair_center(std::vector<cv::Point2f> & cloud_centers);
+	//判断矩形框是否找到
+	//points:输入轮子中心点, 只能是3或者4个
+	Error_manager centers_is_rect(std::vector<cv::Point2f>& points);
+	//检查矩形的大小
+	Error_manager check_rect_size(cv::RotatedRect& rect);
+	//通过关键矩形, 计算出车辆的定位信息, (输出中心点的x和y, 前后轮距, 左右轮距, 旋转角)
+	Error_manager calculate_key_rect(cv::RotatedRect & rect_of_wheel_center, cv::RotatedRect & rect_of_wheel_cloud,
+								 Locate_information* p_locate_information);
+
+
+	//计算两点距离
+    static double distance(cv::Point2f p1, cv::Point2f p2);
+protected:
+	int m_lenth;
+	int m_width;
+	int m_height;
+	int m_freq;
+	int m_nClass;
+};
+#endif
+

locate/locate_manager.cpp

@@ -0,0 +1,695 @@
+//
+// Created by zx on 2019/12/30.
+//
+
+#include "locate_manager.h"
+#include "../tool/proto_tool.h"
+#include "locate_parameter.pb.h"
+
+#include <pcl/filters//voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+#include <glog/logging.h>
+
+#include <string.h>
+
+
+Locate_manager::Locate_manager()
+{
+	mp_point_sift = NULL;
+	mp_cnn3d = NULL;
+
+	m_locate_manager_status = LOCATE_MANAGER_UNKNOW;
+	m_locate_manager_working_flag = false;
+	mp_locate_manager_thread = NULL;
+
+	mp_locate_manager_task = NULL;
+	mp_locate_information = NULL;
+	m_save_flag = false;
+//	m_save_path = "";
+}
+Locate_manager::~Locate_manager()
+{
+	Locate_manager_uninit();
+}
+
+
+//初始化 定位 管理模块。如下三选一LOCATE_PARAMETER_PATH
+Error_manager Locate_manager::Locate_manager_init()
+{
+	return Locate_manager_init_from_protobuf(LOCATE_PARAMETER_PATH);
+}
+//初始化 定位 管理模块。从文件读取
+Error_manager Locate_manager::Locate_manager_init_from_protobuf(std::string prototxt_path)
+{
+	Measure::LocateParameter t_locate_parameters;
+	if(!  proto_tool::read_proto_param(prototxt_path,t_locate_parameters) )
+	{
+		return Error_manager(LOCATER_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Locate_manager read_proto_param  failed");
+	}
+
+	return Locate_manager_init_from_protobuf(t_locate_parameters);
+}
+//初始化 定位 管理模块。从protobuf读取
+Error_manager Locate_manager::Locate_manager_init_from_protobuf(Measure::LocateParameter& locate_parameters)
+{
+	LOG(INFO) << " ---Locate_manager_init_from_protobuf run--- "<< this;
+	Error_manager t_error;
+	m_locate_manager_working_flag = false;
+
+	int point_size = locate_parameters.seg_parameter().point_size();
+	int cls_num = locate_parameters.seg_parameter().cls_num();
+	double freq = locate_parameters.seg_parameter().freq();
+	std::string graph = locate_parameters.seg_parameter().graph();
+	std::string cpkt = locate_parameters.seg_parameter().cpkt();
+	Measure::Area3d area = locate_parameters.seg_parameter().area();
+	Cloud_box t_cloud_box;
+	t_cloud_box.x_min = area.x_min();
+	t_cloud_box.x_max = area.x_max();
+	t_cloud_box.y_min = area.y_min();
+	t_cloud_box.y_max = area.y_max();
+	t_cloud_box.z_min = area.z_min();
+	t_cloud_box.z_max = area.z_max();
+	mp_point_sift = new Point_sift_segmentation(point_size,cls_num,freq,t_cloud_box);
+
+	std::string	graph_file =  graph;
+	std::string	cpkt_file =  cpkt;
+	std::cout << "graph_file" << graph_file << std::endl;
+	std::cout << "cpkt_file" << cpkt_file << std::endl;
+	t_error=mp_point_sift->init(graph_file,cpkt_file);
+	if(t_error!=SUCCESS)
+	{
+		LOG(ERROR)<<t_error.to_string();
+		return t_error;
+	}
+	LOG(INFO)<<"  pointSift init SUCCESS,  graph = "<< graph;
+
+
+	int t_cnn3d_length = locate_parameters.net_3dcnn_parameter().length();
+	int t_cnn3d_width = locate_parameters.net_3dcnn_parameter().width();
+	int t_cnn3d_height = locate_parameters.net_3dcnn_parameter().height();
+	int t_cnn3d_nclass = locate_parameters.net_3dcnn_parameter().nclass();
+	int t_cnn3d_freq = locate_parameters.net_3dcnn_parameter().freq();
+	mp_cnn3d = new Cnn3d_segmentation(t_cnn3d_length, t_cnn3d_width, t_cnn3d_height, t_cnn3d_freq, t_cnn3d_nclass);
+
+	std::string	weights = locate_parameters.net_3dcnn_parameter().weights_file();
+	t_error=mp_cnn3d->init(weights);
+	if(t_error!=SUCCESS)
+	{
+		LOG(ERROR)<<t_error.to_string();
+		return t_error;
+	}
+	LOG(INFO)<<" 3dcnn Init SUCCESS ";
+
+
+	m_locate_manager_status = LOCATE_MANAGER_READY;
+	//启动雷达管理模块的内部线程。默认wait。
+	m_locate_manager_condition.reset(false, false, false);
+	mp_locate_manager_thread = new std::thread(&Locate_manager::thread_work, this);
+
+	return SUCCESS;
+}
+//反初始化 定位 管理模块。
+Error_manager Locate_manager::Locate_manager_uninit()
+{
+	LOG(INFO) << " ---Locate_manager_uninit run--- "<< this;
+
+	//关闭线程
+	if (mp_locate_manager_thread)
+	{
+		m_locate_manager_condition.kill_all();
+	}
+	//回收线程的资源
+	if (mp_locate_manager_thread)
+	{
+		mp_locate_manager_thread->join();
+		delete mp_locate_manager_thread;
+		mp_locate_manager_thread = NULL;
+	}
+
+	//回收内存
+	if ( mp_point_sift !=NULL )
+	{
+		delete mp_point_sift;
+		mp_point_sift = NULL;
+	}
+	if ( mp_cnn3d !=NULL )
+	{
+		delete mp_cnn3d;
+		mp_cnn3d = NULL;
+	}
+	m_locate_manager_status = LOCATE_MANAGER_UNKNOW;
+	m_locate_manager_working_flag = false;
+
+	return Error_code::SUCCESS;
+}
+
+//对外的接口函数，负责接受并处理任务单，
+//input：p_locate_task 定位任务单，基类的指针，指向子类的实例，（多态）
+Error_manager Locate_manager::execute_task(Task_Base* p_locate_task)
+{
+	LOG(INFO) << " ---Locate_manager::execute_task run---  "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//检查指针
+	if (p_locate_task == NULL) {
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Laser_manager::execute_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型，
+	if (p_locate_task->get_task_type() != LOCATE_MANGER_TASK)
+	{
+		return Error_manager(Error_code::LOCATER_MANAGER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 "Locate_manager::execute_task  get_task_type() != LOCATE_MANGER_TASK ");
+	}
+
+	//检查接收方的状态
+	std::cout << "m_locate_manager_status"<<m_locate_manager_status << std::endl;
+	t_error = check_status();
+	if ( t_error != SUCCESS )
+	{
+		t_result.compare_and_cover_error(t_error);
+	}
+	else
+	{
+		//接受任务，并将任务的状态改为TASK_SIGNED已签收
+		mp_locate_manager_task = (Locate_manager_task *) p_locate_task;
+		mp_locate_manager_task->set_task_statu(TASK_SIGNED);
+
+		//检查消息内容是否正确，
+		//检查三维点云指针
+		if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_point_cloud is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_cloud_lock is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else
+		{
+			//校验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
+					   || mp_locate_manager_task->get_task_point_cloud_map()->size() == CNN3D_WHEEL_NUMBER-1 )
+					  && mp_locate_manager_task->get_cloud_aggregation_flag() == false) )
+			{
+				//解析任务单,将任务单的数据保存在本地.
+				mp_locate_information = mp_locate_manager_task->get_task_locate_information_ex();
+				m_save_flag = mp_locate_manager_task->get_task_save_flag();
+				m_save_path = mp_locate_manager_task->get_task_save_path();
+				//启动定位管理模块，的核心工作线程
+				m_locate_manager_status = LOCATE_MANAGER_SIFT;
+				m_locate_manager_working_flag = true;
+				m_locate_manager_condition.notify_all(true);
+				//通知 thread_work 子线程启动。
+
+				//将任务的状态改为 TASK_WORKING 处理中
+				mp_locate_manager_task->set_task_statu(TASK_WORKING);
+			}
+			else
+			{
+				t_error.error_manager_reset(Error_code::LOCATER_MANAGER_CLOUD_MAP_ERROR,
+											Error_level::MINOR_ERROR,
+											"execute_task input map error");
+				t_result.compare_and_cover_error(t_error);
+			}
+
+
+		}
+
+		//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
+		{
+			//忽略轻微故障
+			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+				//将任务的状态改为 TASK_ERROR 结束错误
+				mp_locate_manager_task->set_task_statu(TASK_ERROR);
+			}
+			//返回错误码
+			mp_locate_manager_task->set_task_error_manager(t_result);
+		}
+	}
+
+
+	return t_result;
+
+}
+
+
+//检查状态,是否正常运行
+Error_manager Locate_manager::check_status()
+{
+	if ( m_locate_manager_status == LOCATE_MANAGER_READY )
+	{
+		return Error_code::SUCCESS;
+	}
+	else if ( m_locate_manager_status == LOCATE_MANAGER_SIFT
+			  || m_locate_manager_status == LOCATE_MANAGER_CAR
+			  || m_locate_manager_status == LOCATE_MANAGER_WHEEL)
+	{
+		return Error_manager(Error_code::LOCATER_MANAGER_STATUS_BUSY, Error_level::MINOR_ERROR,
+							 " Locate_manager::check_status error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::LOCATER_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " Locate_manager::check_status error ");
+	}
+	return Error_code::SUCCESS;
+}
+//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+Error_manager Locate_manager::end_task()
+{
+	LOG(INFO) << " ---Locate_manager::end_task run---"<< this;
+
+	//关闭子线程
+	m_locate_manager_working_flag=false;
+	m_locate_manager_condition.notify_all(false);
+
+	//释放缓存
+	mp_locate_information = NULL;
+	m_cloud_wheel_map.clear();
+	m_cloud_car_map.clear();
+
+	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
+	if(mp_locate_manager_task !=NULL)
+	{
+		//判断任务单的错误等级，
+		if ( mp_locate_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
+		{
+			if ( m_locate_manager_status == LOCATE_MANAGER_SIFT
+				 || m_locate_manager_status == LOCATE_MANAGER_CAR
+					|| m_locate_manager_status == LOCATE_MANAGER_WHEEL)
+			{
+				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR，则恢复待机，可以接受下一次的任务单
+				m_locate_manager_status = LOCATE_MANAGER_READY;
+			}
+			//else 状态不变
+
+			//强制改为TASK_OVER，不管它当前在做什么。
+			mp_locate_manager_task->set_task_statu(TASK_OVER);
+		}
+		else
+		{
+			//故障等级为 MAJOR_ERROR CRITICAL_ERROR，则雷达故障，不可以再接受任务单。
+			m_locate_manager_status = LOCATE_MANAGER_FAULT;
+
+			//强制改为 TASK_ERROR，不管它当前在做什么。
+			mp_locate_manager_task->set_task_statu(TASK_ERROR);
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+//取消任务单，由发送方提前取消任务单
+Error_manager Locate_manager::cancel_task()
+{
+	end_task();
+	//强制改为 TASK_DEAD，不管它当前在做什么。
+	mp_locate_manager_task->set_task_statu(TASK_DEAD);
+	return Error_code::SUCCESS;
+}
+
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Locate_manager::is_ready()
+{
+	return m_locate_manager_status == LOCATE_MANAGER_READY;;
+}
+
+//mp_locate_manager_thread 线程执行函数，
+//thread_work 内部线程负责locate定位分析整车的信息，并且回收汇总雷达的数据
+void Locate_manager::thread_work()
+{
+	LOG(INFO) << " -------------------------mp_locate_manager_thread start "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//定位管理的独立线程，
+	while (m_locate_manager_condition.is_alive())
+	{
+		m_locate_manager_condition.wait();
+		if ( m_locate_manager_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//重新循环必须清除错误码.
+			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;
+				m_locate_manager_working_flag = false;
+				m_locate_manager_condition.notify_all(false);
+			}
+			else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
+			{
+				t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+											Error_level::MINOR_ERROR,
+											"thread_work mp_task_point_cloud is null");
+				t_result.compare_and_cover_error(t_error);
+				//因为故障,而提前结束任务.
+				mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
+				end_task();
+			}
+			else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
+			{
+				t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+											Error_level::MINOR_ERROR,
+											"thread_work mp_task_cloud_lock is null");
+				t_result.compare_and_cover_error(t_error);
+				//因为故障,而提前结束任务.
+				mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
+				end_task();
+			}
+				//第一步
+				//point_sift 点筛选分割 , 将输入点云分割为车身和轮胎,
+				//注:地面会在box切割的时候,使用z轴 z_min 直接切除
+			else if(m_locate_manager_status == LOCATE_MANAGER_SIFT)
+			{
+				m_cloud_wheel_map.clear();
+				m_cloud_car_map.clear();
+
+				//定位筛选,将输入点云map拆分为车轮和车身的map
+				t_error = locate_manager_sift();
+				t_result.compare_and_cover_error(t_error);
+
+				//故障汇总, 只处理2级以上的故障
+				if(t_result.get_error_level() >= MINOR_ERROR)
+				{
+					m_locate_manager_status = LOCATE_MANAGER_FAULT;
+					mp_locate_manager_task->set_task_statu(TASK_ERROR);
+					//因为故障,而提前结束任务.
+					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
+					end_task();
+				}
+				else
+				{
+					//成功则 进入到下一个阶段,
+					m_locate_manager_status = LOCATE_MANAGER_CAR;
+				}
+			}
+				//第二步
+				//_measure_height 计算汽车的长宽高
+			else if(m_locate_manager_status == LOCATE_MANAGER_CAR)
+			{
+				//计算汽车长宽高,
+				t_error = locate_manager_locate_car();
+				t_result.compare_and_cover_error(t_error);
+
+				//故障汇总, 只处理2级以上的故障
+				if(t_result.get_error_level() >= MINOR_ERROR)
+				{
+					m_locate_manager_status = LOCATE_MANAGER_FAULT;
+					mp_locate_manager_task->set_task_statu(TASK_ERROR);
+					//因为故障,而提前结束任务.
+					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
+					end_task();
+				}
+				else
+				{
+					//成功则 进入到下一个阶段,
+					m_locate_manager_status = LOCATE_MANAGER_WHEEL;
+				}
+			}
+				//第三步
+				//locate_wheel 解析车轮信息
+			else if(m_locate_manager_status == LOCATE_MANAGER_WHEEL)
+			{
+				//解析车轮信息,
+				t_error = locate_manager_locate_wheel();
+				t_result.compare_and_cover_error(t_error);
+
+				//故障汇总, 只处理2级以上的故障
+				if(t_result.get_error_level() >= MINOR_ERROR)
+				{
+					m_locate_manager_status = LOCATE_MANAGER_FAULT;
+					mp_locate_manager_task->set_task_statu(TASK_ERROR);
+					//因为故障,而提前结束任务.
+					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
+					end_task();
+				}
+				else
+				{
+					m_locate_manager_working_flag = false;
+					m_locate_manager_condition.set_pass_ever(false);
+					//正常结束任务
+					mp_locate_manager_task->compare_and_cover_task_error_manager(t_result);
+					end_task();
+				}
+			}
+		}
+	}
+	LOG(INFO) << " ---------------------------mp_locate_manager_thread end :"<<this;
+	return;
+}
+
+
+//定位筛选,将输入点云map拆分为车轮和车身的map.(数据可以直接从locate_manager内部获取)
+Error_manager Locate_manager::locate_manager_sift()
+{
+	if ( mp_locate_manager_task == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							" Locate_manager::locate_manager_sift() error ");
+	}
+	else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL,
+									Error_level::MINOR_ERROR,
+									"locate_manager_sift mp_task_point_cloud is null");
+	}
+	else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL,
+									Error_level::MINOR_ERROR,
+									"locate_manager_sift mp_task_cloud_lock is null");
+	}
+
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//获取任务单的输入点云
+	std::unique_lock<std::mutex> lck (*(mp_locate_manager_task->get_task_cloud_lock()));
+	std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> *tp_task_point_cloud_map = mp_locate_manager_task->get_task_point_cloud_map();
+
+	//清除map
+	m_cloud_wheel_map.clear();
+	m_cloud_car_map.clear();
+
+	//遍历任务单的输入点云map
+	for (const auto &map_iter : *tp_task_point_cloud_map)
+	{
+		if (map_iter.second.get() == NULL)
+		{
+			return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+								 " Locate_manager::locate_manager_sift() pcl::PointCloud<pcl::PointXYZ>::Ptr is NULL ");
+		}
+		//提前为拆分之后的点云分配内存.
+		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);
+		if (t_error != Error_code::SUCCESS)
+		{
+			char buf[256] = {0};
+			sprintf(buf, " map.id = %d, locate_sift error", map_iter.first );
+			t_error.add_error_description(buf, strlen(buf) );
+			t_result.compare_and_cover_error(t_error);
+			//注:这里不直接return,而是要把map全部执行完成
+		}
+		else
+		{
+			//将结果存入本地的中间缓存,此时map点云是有内存的,后续要记得回收
+			m_cloud_wheel_map[map_iter.first] = tp_cloud_wheel;
+			m_cloud_car_map[map_iter.first] = tp_cloud_car;
+		}
+	}
+	return t_result;
+}
+
+//定位筛选,对具体的点云进行操作,分离出车轮和车身.
+//input::cloud输入点云
+//output::cloud_wheel输出车轮点云
+//output::cloud_car输出车身点云
+//work_dir:中间文件保存路径
+Error_manager Locate_manager::locate_sift(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)
+{
+    if(p_cloud_in.get()==0)
+    {
+        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+    }
+    if(p_cloud_in->size()==0)
+    {
+        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+    }
+    if(mp_point_sift==NULL)
+    {
+        return Error_manager(LOCATER_POINTSIFT_UNINIT,MINOR_ERROR,"Point Sift unInit");
+    }
+	Error_manager t_error;
+
+    //分割车辆点云
+    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
+	t_error=mp_point_sift->segmentation(p_cloud_in, segmentation_clouds, save_flag, work_dir);
+    if(t_error!=SUCCESS)
+    {
+        return t_error;
+    }
+    //第0类即是轮胎点云,第1类为车身点云
+    pcl::copyPointCloud(*segmentation_clouds[0], *p_cloud_wheel);
+    pcl::copyPointCloud(*segmentation_clouds[1], *p_cloud_car);
+
+    return SUCCESS;
+}
+
+
+//计算汽车高度,
+Error_manager Locate_manager::locate_manager_locate_car()
+{
+	if ( mp_locate_manager_task == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " Locate_manager::locate_manager_locate_car() error ");
+	}
+	else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL,
+							 Error_level::MINOR_ERROR,
+							 "locate_manager_locate_car mp_task_point_cloud is null");
+	}
+	else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL,
+							 Error_level::MINOR_ERROR,
+							 "locate_manager_locate_car mp_task_cloud_lock is null");
+	}
+	Error_manager t_error;
+
+	pcl::PointXYZRGB t_point3d_min, t_point3d_max;
+
+	std::unique_lock<std::mutex> lck (*(mp_locate_manager_task->get_task_cloud_lock()));
+
+	for (auto iter = m_cloud_car_map.begin(); iter != m_cloud_car_map.end(); ++iter)
+	{
+		pcl::PointXYZRGB min, max;
+		t_error = locate_car(iter->second, min, max);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+
+		//比较选出整个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;;
+	mp_locate_information->locate_height = t_point3d_max.z;
+	//注意了:在雷达扫描时, 就已经将地面标定位为 z = 0
+	return Error_code::SUCCESS;
+}
+
+//根据汽车点云计算汽车的边界
+Error_manager Locate_manager::locate_car(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_car, pcl::PointXYZRGB& min, pcl::PointXYZRGB& max)
+{
+    if(cloud_car.get()==0)
+    {
+        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,MINOR_ERROR,"measure height input cloud uninit");
+    }
+    if(cloud_car->size()==0)
+    {
+        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,MINOR_ERROR,"measure height input cloud is empty");
+    }
+
+    //获取点云的边界
+    pcl::getMinMax3D(*cloud_car,min,max);
+
+    return SUCCESS;
+}
+
+
+
+
+//计算汽车的车轮信息
+Error_manager Locate_manager::locate_manager_locate_wheel()
+{
+	if ( mp_locate_manager_task == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " Locate_manager::locate_manager_locate_car() error ");
+	}
+	else if (mp_locate_manager_task->get_task_point_cloud_map() == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL,
+							 Error_level::MINOR_ERROR,
+							 "locate_manager_locate_car mp_task_point_cloud is null");
+	}
+	else if ( mp_locate_manager_task->get_task_cloud_lock() == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL,
+							 Error_level::MINOR_ERROR,
+							 "locate_manager_locate_car mp_task_cloud_lock is null");
+	}
+	if(mp_cnn3d==NULL)
+	{
+		return Error_manager(LOCATER_3DCNN_UNINIT,MINOR_ERROR,"locate_wheel 3dcnn is not init");
+	}
+
+	Error_manager t_error;
+	std::unique_lock<std::mutex> lck (*(mp_locate_manager_task->get_task_cloud_lock()));
+	//解析车轮
+	t_error=mp_cnn3d->analytic_wheel(m_cloud_wheel_map,mp_locate_manager_task->get_cloud_aggregation_flag(),
+									 mp_locate_manager_task->get_task_locate_information_ex(),
+									 m_save_flag,m_save_path);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//保存点云成txt到文件
+void Locate_manager::save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string save_file)
+{
+    FILE* pfile=fopen(save_file.c_str(),"w");
+    for(int i=0;i<cloud->size();++i)
+    {
+        fprintf(pfile,"%.3f %.3f %.3f\n",cloud->points[i].x,cloud->points[i].y,cloud->points[i].z);
+    }
+    fclose(pfile);
+}

locate/locate_manager.h

@@ -0,0 +1,122 @@
+/*
+ * Locate_manager 雷达定位管理模块
+ *
+ * */
+
+#ifndef LOCATER_H
+#define LOCATER_H
+
+#include "locate_manager_task.h"
+#include "locate_parameter.pb.h"
+
+#include "../error_code/error_code.h"
+#include "../tool/singleton.h"
+#include "../tool/thread_condition.h"
+
+#include "point_sift_segmentation.h"
+#include "cnn3d_segmentation.h"
+
+#define LOCATE_PARAMETER_PATH "../setting/locate.prototxt"
+
+
+class Locate_manager:public Singleton<Locate_manager>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<Locate_manager>;
+
+public:
+	//定位管理模块的工作状态
+	enum Locate_manager_status
+	{//default LOCATE_MANAGER_UNKNOW = 0
+		LOCATE_MANAGER_UNKNOW               	= 0,    //未知
+		LOCATE_MANAGER_READY               		= 1,    //准备，待机
+		LOCATE_MANAGER_SIFT						= 2,	//sift点云筛选, 将车身和轮胎的点剥离出来
+		LOCATE_MANAGER_CAR						= 3,	//通过车身 计算汽车的定位信息.
+		LOCATE_MANAGER_WHEEL					= 4,	//通过车轮 计算汽车的定位信息.
+
+		LOCATE_MANAGER_FAULT					= 5,	//故障
+	};
+
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	Locate_manager();
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	Locate_manager(const Locate_manager&)=delete;
+	Locate_manager& operator =(const Locate_manager&)= delete;
+	~Locate_manager();
+
+
+public: //API 对外接口
+	//初始化 定位 管理模块。如下三选一
+	Error_manager Locate_manager_init();
+	//初始化 定位 管理模块。从文件读取
+	Error_manager Locate_manager_init_from_protobuf(std::string prototxt_path);
+	//初始化 定位 管理模块。从protobuf读取
+	Error_manager Locate_manager_init_from_protobuf(Measure::LocateParameter& locate_parameters);
+	//反初始化 定位 管理模块。
+	Error_manager Locate_manager_uninit();
+
+	//对外的接口函数，负责接受并处理任务单，
+	//input：p_locate_task 定位任务单，基类的指针，指向子类的实例，（多态）
+    Error_manager execute_task(Task_Base* p_locate_task);
+	//检查状态,是否正常运行
+	Error_manager check_status();
+	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+	Error_manager end_task();
+	//取消任务单，由发送方提前取消任务单
+	Error_manager cancel_task();
+
+	//判断是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+protected:
+	//mp_locate_manager_thread 线程执行函数，
+	//thread_work 内部线程负责locate定位分析整车的信息，并且回收汇总雷达的数据
+	void thread_work();
+
+    //定位筛选,将输入点云map拆分为车轮和车身的map.(数据可以直接从locate_manager内部获取)
+	Error_manager locate_manager_sift();
+
+	//定位筛选,对具体的点云进行操作,分离出车轮和车身.
+    //input::cloud输入点云
+    //output::cloud_wheel输出车轮点云
+	//output::cloud_car输出车身点云
+    //work_dir:中间文件保存路径
+    Error_manager locate_sift(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);
+
+    //计算汽车的长宽高
+	Error_manager locate_manager_locate_car();
+	//根据汽车点云计算汽车的边界
+    Error_manager locate_car(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_car,pcl::PointXYZRGB& point3d_min, pcl::PointXYZRGB& point3d_max);
+
+	//计算汽车的车轮信息
+	Error_manager locate_manager_locate_wheel();
+
+	//保存点云成txt到文件
+    static void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string save_file);
+private:
+    Measure::LocateParameter           		m_locate_parameter;				//定位模块的配置参数
+    Point_sift_segmentation*                mp_point_sift;					//定位模块的点筛选分割, 内存本类管理
+    Cnn3d_segmentation*                     mp_cnn3d;						//定位模块的cnn3d分割, 内存本类管理
+
+
+	Locate_manager_status						m_locate_manager_status;					//定位管理的工作状态
+	std::atomic<bool>                			m_locate_manager_working_flag;          	//定位管理的工作使能标志位
+	std::thread*        						mp_locate_manager_thread;   				//定位管理的线程指针，内存由本类管理
+	Thread_condition							m_locate_manager_condition;					//定位管理的条件变量
+
+	//雷达管理的任务单指针，实际内存由发送方控制管理，//接受任务后，指向新的任务单
+	Locate_manager_task *  						mp_locate_manager_task;        				//定位管理任务单的指针，内存由发送方管理。
+	Locate_information*							mp_locate_information;						//测量结果 整车的定位信息,，内存由发送方管理。
+	std::atomic<bool> 							m_save_flag;           						//保存文件的使能标志位
+	std::string									m_save_path;            					//保存文件的保存路径
+
+	//中间缓存,
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> 		m_cloud_wheel_map;				//车轮的点云map,(4个轮胎)
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> 		m_cloud_car_map;				//车身的点云map
+};
+
+
+#endif //LOCATER_H

locate/locate_manager_task.cpp

@@ -0,0 +1,211 @@
+//
+// Created by zx on 2019/12/30.
+//
+
+#include "locate_manager_task.h"
+#include <glog/logging.h>
+Locate_manager_task::Locate_manager_task()
+{
+	//构造函数锁定任务类型为 LOCATE_MANGER_TASK，后续不允许更改
+	m_task_type = LOCATE_MANGER_TASK;
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_save_flag = false;//默认不保存，false
+	m_task_save_path.clear();
+	mp_task_point_cloud_map = NULL;
+	mp_task_cloud_lock = NULL;
+
+	m_task_locate_information.locate_x = 0;					//整车的中心点x值
+	m_task_locate_information.locate_y = 0;					//整车的中心点y值
+	m_task_locate_information.locate_angle = 0;				//整车的旋转角
+	m_task_locate_information.locate_length = 0;				//整车的长度
+	m_task_locate_information.locate_width = 0;				//整车的宽度, 也是左右轮的距离
+	m_task_locate_information.locate_height = 0;				//整车的高度
+	m_task_locate_information.locate_wheel_base = 0;			//整车的轮距, 前后轮的距离
+	m_task_locate_information.locate_wheel_width = 0;		//整车的轮距, 左右轮的距离
+	m_task_locate_information.locate_correct = false;		//整车的校准标记位
+}
+Locate_manager_task::~Locate_manager_task()
+{
+
+}
+
+
+//初始化任务单，必须初始化之后才可以使用，（必选参数）
+// input:p_task_cloud_lock 三维点云的数据保护锁
+// output:p_task_point_cloud 三维点云容器的智能指针
+Error_manager Locate_manager_task::task_init(std::mutex* p_task_cloud_lock,
+											 std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>*		p_task_point_cloud_map)
+{
+	if(p_task_point_cloud_map == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Laser_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_save_flag = false;
+	m_task_save_path.clear();
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+
+	m_task_locate_information.locate_x = 0;					//整车的中心点x值
+	m_task_locate_information.locate_y = 0;					//整车的中心点y值
+	m_task_locate_information.locate_angle = 0;				//整车的旋转角
+	m_task_locate_information.locate_length = 0;			//整车的长度
+	m_task_locate_information.locate_width = 0;				//整车的宽度, 也是左右轮的距离
+	m_task_locate_information.locate_height = 0;				//整车的高度
+	m_task_locate_information.locate_wheel_base = 0;		//整车的轮距, 前后轮的距离
+	m_task_locate_information.locate_wheel_width = 0;		//整车的轮距, 左右轮的距离
+	m_task_locate_information.locate_correct = false;		//整车的校准标记位
+
+	return Error_code::SUCCESS;
+}
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_pmutexoint_cloud 三维点云容器的智能指针
+Error_manager Locate_manager_task::task_init(Task_statu task_statu,
+											 std::string task_statu_information,
+											 void* p_tast_receiver,
+											 std::chrono::milliseconds task_over_time,
+											 bool task_save_flag,
+											 std::string task_save_path,
+											 std::mutex* p_task_cloud_lock,
+											 std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>*		p_task_point_cloud_map,
+											 bool cloud_aggregation_flag
+)
+{
+	if(p_task_point_cloud_map == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Laser_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = task_statu;
+	m_task_statu_information = task_statu_information;
+	mp_tast_receiver = p_tast_receiver;
+	m_task_over_time = task_over_time;
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_save_flag = task_save_flag;
+	m_task_save_path = task_save_path;
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+	m_cloud_aggregation_flag = cloud_aggregation_flag;
+
+	m_task_locate_information.locate_x = 0;					//整车的中心点x值
+	m_task_locate_information.locate_y = 0;					//整车的中心点y值
+	m_task_locate_information.locate_angle = 0;				//整车的旋转角
+	m_task_locate_information.locate_length = 0;				//整车的长度
+	m_task_locate_information.locate_width = 0;				//整车的宽度, 也是左右轮的距离
+	m_task_locate_information.locate_height = 0;				//整车的高度
+	m_task_locate_information.locate_wheel_base = 0;			//整车的轮距, 前后轮的距离
+	m_task_locate_information.locate_wheel_width = 0;		//整车的轮距, 左右轮的距离
+	m_task_locate_information.locate_correct = false;		//整车的校准标记位
+
+	return Error_code::SUCCESS;
+}
+
+//更新定位信息
+Error_manager Locate_manager_task::task_update_locate_information(Locate_information locate_information)
+{
+	set_task_locate_information(locate_information);
+	return Error_code::SUCCESS;
+}
+
+
+
+//获取保存文件的使能标志位
+bool Locate_manager_task::get_task_save_flag()
+{
+	return m_task_save_flag;
+}
+//获取保存路径
+std::string Locate_manager_task::get_task_save_path()
+{
+	return m_task_save_path;
+}
+//获取 三维点云容器的锁
+std::mutex* Locate_manager_task::get_task_cloud_lock()
+{
+	return mp_task_cloud_lock;
+}
+//获取 三维点云容器的智能指针map
+std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* Locate_manager_task::get_task_point_cloud_map()
+{
+	return mp_task_point_cloud_map;
+}
+//获取 所有雷达的三维点云是否聚集的标志位
+bool Locate_manager_task::get_cloud_aggregation_flag()
+{
+	return m_cloud_aggregation_flag;
+}
+//获取测量结果 整车的定位信息
+Locate_information Locate_manager_task::get_task_locate_information()
+{
+	return m_task_locate_information;
+}
+//获取测量结果 整车的定位信息
+Locate_information* Locate_manager_task::get_task_locate_information_ex()
+{
+	return &m_task_locate_information;
+}
+
+//设置保存文件的使能标志位
+void Locate_manager_task::set_task_save_flag(bool task_save_flag)
+{
+	m_task_save_flag = task_save_flag;
+}
+//设置保存路径
+void Locate_manager_task::set_task_save_path(std::string task_save_path)
+{
+	m_task_save_path = task_save_path;
+}
+//设置 保存标志位和路径
+void Locate_manager_task::set_task_save_flag_and_path(bool task_save_flag, std::string task_save_path)
+{
+	m_task_save_flag = task_save_flag;
+	m_task_save_path = task_save_path;
+}
+//设置 三维点云容器的锁
+void Locate_manager_task::set_task_cloud_lock(std::mutex* mp_task_cloud_lock)
+{
+	mp_task_cloud_lock = mp_task_cloud_lock;
+}
+//设置 三维点云容器的智能指针
+void Locate_manager_task::set_task_point_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map)
+{
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+}
+//设置 所有雷达的三维点云是否聚集的标志位
+void Locate_manager_task::set_cloud_aggregation_flag(bool cloud_aggregation_flag)
+{
+	m_cloud_aggregation_flag = cloud_aggregation_flag;
+}
+//设置测量结果 整车的定位信息
+void Locate_manager_task::set_task_locate_information(Locate_information task_locate_information)
+{
+	m_task_locate_information = task_locate_information;
+}

locate/locate_manager_task.h

@@ -0,0 +1,121 @@
+//
+// Created by zx on 2019/12/30.
+//
+
+#ifndef LOCATE_TASK_H
+#define LOCATE_TASK_H
+#include "../task/task_command_manager.h"
+#include "../error_code/error_code.h"
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include <mutex>
+#include <map>
+
+//lacate测量结果结构体, 整车的信息,
+typedef struct Locate_information
+{
+    float locate_x;				//整车的中心点x值, 四轮的中心
+    float locate_y;				//整车的中心点y值, 四轮的中心
+    float locate_angle;			//整车的旋转角, 四轮的旋转角
+    float locate_length;		//整车的长度, 用于规避碰撞
+    float locate_width;			//整车的宽度, 用于规避碰撞
+    float locate_height;		//整车的高度, 用于规避碰撞
+    float locate_wheel_base;	//整车的轮距, 前后轮的距离, 用于机器人或agv的抓车
+	float locate_wheel_width;	//整车的轮距, 左右轮的距离, 用于机器人或agv的抓车
+    bool locate_correct;		//整车的校准标记位
+    //注:理论上, 车宽和左右轮距应该是一样的, 但是实际上车宽比左右轮距略大,
+
+}Locate_information;
+
+/*
+ * 测量任务类,负责维护测量任务所需的输入参数以及测量的结果
+ */
+class Locate_manager_task: public Task_Base
+{
+public:
+    Locate_manager_task();
+    ~Locate_manager_task();
+
+	//初始化任务单，必须初始化之后才可以使用，（必选参数）
+	// input:p_task_cloud_lock 三维点云的数据保护锁
+	// output:p_task_point_cloud 三维点云容器的智能指针
+	Error_manager task_init(std::mutex* p_task_cloud_lock,
+							std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>*		p_task_point_cloud_map
+							);
+
+	//初始化任务单，必须初始化之后才可以使用，（可选参数）
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	//    input:task_save_flag 是否保存
+	//    input:task_save_path 保存路径
+	//    input:p_task_cloud_lock 三维点云的数据保护锁
+	//    output:p_task_pmutexoint_cloud 三维点云容器的智能指针
+	Error_manager task_init(Task_statu task_statu,
+							std::string task_statu_information,
+							void* p_tast_receiver,
+							std::chrono::milliseconds task_over_time,
+							bool task_save_flag,
+							std::string task_save_path,
+							std::mutex* p_task_cloud_lock,
+							std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>*		p_task_point_cloud_map,
+							bool cloud_aggregation_flag
+							);
+
+	//更新定位信息
+	Error_manager task_update_locate_information(Locate_information locate_information);
+
+public: //get or set member variable
+	//获取保存文件的使能标志位
+	bool get_task_save_flag();
+	//获取保存路径
+	std::string get_task_save_path();
+	//获取 三维点云容器的锁
+	std::mutex* get_task_cloud_lock();
+	//获取 三维点云容器的智能指针map
+	std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* get_task_point_cloud_map();
+	//获取 所有雷达的三维点云是否聚集的标志位
+	bool get_cloud_aggregation_flag();
+	//获取测量结果 整车的定位信息
+	Locate_information get_task_locate_information();
+	//获取测量结果 整车的定位信息
+	Locate_information* get_task_locate_information_ex();
+
+	//设置保存文件的使能标志位
+	void set_task_save_flag(bool task_save_flag);
+	//设置保存路径
+	void set_task_save_path(std::string task_save_path);
+	//设置 保存标志位和路径
+	void set_task_save_flag_and_path(bool task_save_flag, std::string task_save_path);
+	//设置 三维点云容器的锁
+	void set_task_cloud_lock(std::mutex* p_task_cloud_lock);
+	//设置 三维点云容器的智能指针
+	void set_task_point_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map);
+	//设置 所有雷达的三维点云是否聚集的标志位
+	void set_cloud_aggregation_flag(bool cloud_aggregation_flag);
+	//设置测量结果 整车的定位信息
+	void set_task_locate_information(Locate_information task_locate_information);
+
+protected:
+
+	//任务执行中间文件的保存使能标志位，//默认不保存，false
+	bool		 								m_task_save_flag;
+	//任务执行中间文件的保存路径，任务输入
+	std::string                     			m_task_save_path;
+
+	//三维点云的数据保护锁，任务输入,
+	std::mutex*                     			mp_task_cloud_lock;
+	//三维点云的智能指针map，任务输入//这里只是智能指针，实际内存由任务发送方管理管理，初始化时必须保证内存有效。
+	std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>*		mp_task_point_cloud_map;
+	//所有雷达的三维点云是否聚集的标志位,
+	bool 										m_cloud_aggregation_flag;
+	//m_cloud_aggregation_flag==true :: mp_task_point_cloud_map里面只有一个cloud
+	//m_cloud_aggregation_flag==false:: mp_task_point_cloud_map里面有很多个cloud, 每个雷达的数据都是独立的.
+
+	//测量结果 整车的定位信息,注:这个不需要外部导入,构造的时候就会初始化为0;
+	Locate_information							m_task_locate_information;
+};
+
+
+#endif //LOCATE_TASK_H

locate/locate_parameter.proto

@@ -0,0 +1,50 @@
+syntax = "proto2";
+package Measure;
+
+message Area3d
+{
+    optional double  x_min=1 [default=0.0];
+    optional double  x_max=2 [default=12000];
+    optional double  y_min=3 [default=4500];
+    optional double  y_max=4 [default=11000];
+    optional double  z_min=5 [default=60];
+    optional double  z_max=6 [default=3000];
+}
+
+message YoloParameter
+{
+    optional string cfg=1 [default=""];
+    optional string weights=2 [default=""];
+    optional double min_x=3 [default=0.0];
+    optional double max_x=4 [default=12000.0];
+    optional double min_y=5 [default=4500.0];
+    optional double max_y=6 [default=11000.];
+    optional double freq=7 [default=25.];
+}
+
+message PointSiftParameter
+{
+    optional int64          point_size=1 [default=8192];
+    optional int64          cls_num=2 [default=3];
+    optional double         freq=3 [default=50];
+    optional Area3d         area=4;
+    optional string         graph=5 [default="seg_model_last.ckpt.meta"];
+    optional string         cpkt=6 [default="seg_model_last.ckpt"];
+}
+
+message cnnParameter
+{
+    optional double length=1 [default=224];
+    optional double width=2 [default=224];
+    optional double height=3 [default=96];
+    optional double freq=4 [default=25];
+    optional double nclass=5 [default=3];
+    optional string weights_file=6 [default="frozen_model.pb"];
+}
+
+message LocateParameter{
+    optional    Area3d                  area=1;
+    optional    cnnParameter            net_3dcnn_parameter=2;
+    optional    PointSiftParameter        seg_parameter=3;
+    optional    YoloParameter               yolo_parameter=4;
+}

locate/pointSIFT_API.h

@@ -46,7 +46,7 @@ protected:
 	std::mutex		m_mutex;
 	std::string		m_error;
 	bool			m_bInit;
-	int				m_point_num;
-	int				m_cls_num;
+	int				m_point_num;		//点云的数量,默认8192
+	int				m_cls_num;			//拆分点云的类别数,默认2个(只有车轮和车身,  通过box限定范围,可以直接剔除地面和杂物)
 	void*			m_sess;
 };

locate/point_sift_segmentation.cpp

@@ -2,6 +2,9 @@
 #include <glog/logging.h>
 #include <fstream>
 
+#include <algorithm>
+#include <pcl/filters//voxel_grid.h>
+#include <pcl/filters/passthrough.h>
 
 #include <time.h>
 #include <sys/time.h>
@@ -9,13 +12,13 @@
 using namespace std;
 using namespace chrono;
 
-void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& pCloud)
+void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& cloud)
 {
 	std::ofstream os;
 	os.open(txt, std::ios::out);
-	for (int i = 0; i < pCloud.points.size(); i++)
+	for (int i = 0; i < cloud.points.size(); i++)
 	{
-		pcl::PointXYZRGB point = pCloud.points[i];
+		pcl::PointXYZRGB point = cloud.points[i];
 		char buf[255];
 		memset(buf, 0, 255);
 		sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
@@ -24,8 +27,28 @@ void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& pClo
 	os.close();
 }
 
+void save_rgb_cloud_txt(std::string txt, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
+{
+	std::ofstream os;
+	os.open(txt, std::ios::out);
+	for (int j = 0; j < cloud_vector.size(); ++j)
+	{
+		for (int i = 0; i < cloud_vector[j]->points.size(); i++)
+		{
+			pcl::PointXYZRGB point = cloud_vector[j]->points[i];
+			char buf[255];
+			memset(buf, 0, 255);
+			sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
+			os.write(buf, strlen(buf));
+		}
+	}
+	os.close();
+}
+
+
+
 Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float freq, pcl::PointXYZ minp, pcl::PointXYZ maxp)
-	:PointSifter(point_size, cls)
+:PointSifter(point_size, cls)
 {
 	m_point_num = point_size;
 	m_cls_num = cls;
@@ -33,33 +56,33 @@ Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float
 	m_minp = minp;
 	m_maxp = maxp;
 }
-
-Error_manager Point_sift_segmentation::set_region(pcl::PointXYZ minp, pcl::PointXYZ maxp)
+Point_sift_segmentation::Point_sift_segmentation(int point_size,int cls,float freq, Cloud_box cloud_box)
+:PointSifter(point_size, cls)
 {
-    m_minp = minp;
-    m_maxp = maxp;
-    if(m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y)
-    {
-        return Error_manager(LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,NORMAL,
-            "Point sift set region invalid :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y ");
-    }
-    return SUCCESS;
+	m_point_num = point_size;
+	m_cls_num = cls;
+	m_freq = freq;
+	m_cloud_box_limit = cloud_box;
 }
 
 Point_sift_segmentation::~Point_sift_segmentation()
 {
 }
 
+
+
+
+
 Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
 {
 	if (!Load(graph, cpkt))
 	{
 		std::string error_string="pointSIFT Init ERROR:";
 		error_string+=LastError();
-		return Error_manager(LOCATER_SIFT_INIT_FAILED,NORMAL,error_string);
+		return Error_manager(LOCATER_SIFT_INIT_FAILED,MINOR_ERROR,error_string);
 	}
 
-    //创建空数据,第一次初始化后空跑
+	//创建空数据,第一次初始化后空跑
 	float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
 	float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));
 
@@ -69,9 +92,9 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
 	{
 		free(cloud_data);
 		free(output);
-        std::string error_string="pointSIFT int first predict ERROR:";
-        error_string+=LastError();
-		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,NORMAL,error_string);
+		std::string error_string="pointSIFT int first predict ERROR:";
+		error_string+=LastError();
+		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,error_string);
 	}
 	else
 	{
@@ -88,119 +111,97 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
 	return SUCCESS;
 }
 
-#include <algorithm>
-bool Point_sift_segmentation::Create_data(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float* output)
+
+
+//分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
+//cloud:输入点云
+//clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
+Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+													std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
+													bool save_flag, std::string save_dir)
 {
-	if (cloud->size() == 0)
-		return false;
-	////������άС����
-	pcl::getMinMax3D(*cloud, m_minp, m_maxp);
-	pcl::PointXYZ center;
-	center.x = (m_minp.x + m_maxp.x) / 2.0;
-	center.y = (m_minp.y + m_maxp.y) / 2.0;
-	
-	int l = int((m_maxp.x - m_minp.x) / m_freq) + 1;
-	int w = int((m_maxp.y - m_minp.y) / m_freq) + 1;
-	int h = int((m_maxp.z - m_minp.z) / m_freq) + 1;
-
-	float* grid = (float*)malloc(l*w*h*sizeof(float));
-	memset(grid, 0, l*w*h*sizeof(float));
-	for (int i = 0; i < cloud->size(); ++i)
-	{
-		pcl::PointXYZ point = cloud->points[i];
-		int idx = (point.x - m_minp.x) / m_freq;
-		int idy = (point.y - m_minp.y) / m_freq;
-		int idz = (point.z - m_minp.z) / m_freq;
-		if (idx < 0 || idy < 0 || idz < 0)
-			continue;
-		*(grid + idx + idy*l + idz*l*w) = i+1;
-	}
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_grid(new pcl::PointCloud<pcl::PointXYZ>);
-	for (int i = 0; i < l*w*h; ++i)
+	if(p_cloud_in.get()==NULL)
 	{
-		if (*(grid + i) > 0)
-		{
-			int id = *(grid + i);
-			if (id <= cloud->size())
-			{
-				pcl::PointXYZ point = cloud->points[id-1];
-				cloud_grid->push_back(point);
-			}
-		}
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
 	}
-	free(grid);
-	////ɸѡ�� m_point_num����
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_select(new pcl::PointCloud<pcl::PointXYZ>);
-	if (cloud_grid->size()>m_point_num)
+	if(p_cloud_in->size()<=0)
 	{
-		//����˳�� ȡǰm_point_num ����
-		std::random_shuffle(cloud_grid->points.begin(), cloud_grid->points.end());	//����˳��
-		cloud_select->points.assign(cloud_grid->points.begin(), cloud_grid->points.begin() + m_point_num);
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
 	}
-	else if (cloud_grid->size() < m_point_num)
+
+	Error_manager t_error;
+
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
+	//第一步
+	// 使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
+	t_error = filter_cloud_with_voxel_grid(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
 	{
-		int add = m_point_num - cloud_grid->size();
-		if (add > cloud_grid->size())
-			return false;
-		std::random_shuffle(cloud_grid->points.begin(), cloud_grid->points.end());	//����˳��
-		cloud_select->points.assign(cloud_grid->points.begin(), cloud_grid->points.begin() + add);
-		cloud_select->points.insert(cloud_select->points.begin(), cloud_grid->points.begin(), cloud_grid->points.end());
+		return t_error;
 	}
-	else
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
+
+	//第二步
+	// 使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
+	t_error = filter_cloud_with_box(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
 	{
-		cloud_select->points.assign(cloud_grid->points.begin(), cloud_grid->points.end());
+		return t_error;
 	}
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
 
-	if (cloud_select->points.size() != m_point_num)
+	//第三步 /
+	// /通过输入点云,更新区域范围, 范围不一定是box的边界,可能比box要小.
+	t_error = update_region(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
 	{
-//		LOG(ERROR) << "\tpointSIFT input select cloud is not " << m_point_num;
-		return false;
+		return t_error;
 	}
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
 
-	for (int i = 0; i < m_point_num; ++i)
+	//第四步
+	// 抽稀点云,	//自己写的算法, 重新生成新的点云.
+	t_error = filter_cloud_with_my_grid(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
 	{
-		pcl::PointXYZ point = cloud_select->points[i];
-		*(output + i * 3) = (point.x-center.x) / 1000.0;
-		*(output + i * 3+1) = (point.y-center.y) / 1000.0;
-		*(output + i * 3+2) = point.z / 1000.0;
+		return t_error;
 	}
-	return true;
-}
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
 
-Error_manager Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg, std::string save_dir)
-{
-    if(cloud->size()==0)
-    {
-        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,NORMAL,"PointSift input cloud empty ");
-    }
-	float* data = (float*)malloc(m_point_num * 3 * sizeof(float));
-	float* output = (float*)malloc(m_point_num*m_cls_num*sizeof(float));
-	memset(data, 0, m_point_num * 3 * sizeof(float));
-	memset(output, 0, m_point_num*m_cls_num * sizeof(float));
-	//LOG(INFO) << "cloud size:" << cloud->size()<<"  /  "<<m_point_num;
-	if (!Create_data(cloud, data))
-	{
-		free(data);
-		free(output);
-		return Error_manager(LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,NORMAL,"pointSIFT Create input data ERROR");
+	//第五步
+	pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_select(new pcl::PointCloud<pcl::PointXYZ>);
+	//从点云中选出固定数量的随机点, 默认8192个点
+	t_error = filter_cloud_with_select(p_cloud_in, p_cloud_select);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	std::cout << "p_cloud_select = "<<p_cloud_select->size() << std::endl;
+
+	//第六步
+	//把pcl点云转化为float, 因为TensorFlow算法只能识别float
+	//提前分配内存, 后续记得回收....
+	float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float));				//8192*3 float, 8192个三维点
+	float* p_data_type = (float*)malloc(m_point_num*m_cls_num*sizeof(float));		//8192*2 float, 8192个类别百分比(车轮和车身2类)
+	memset(p_data_point, 0, m_point_num * 3 * sizeof(float));
+	memset(p_data_type, 0, m_point_num*m_cls_num * sizeof(float));
+	//将点云数据转换到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;
 	}
 
-//	for (int i = 0; i < m_point_num; ++i)
-//	{
-//		pcl::PointXYZ point = cloud->points[i];
-//		*(data + i * 3) = point.x;
-//		*(data + i * 3+1) = point.y ;
-//		*(data + i * 3+2) = point.z ;
-//	}
-
+	//第七步
+	//tensonflow预测点云, 计算出每个三维点的类别百分比,
 	auto start = system_clock::now();
-
-	if (!Predict(data, output))
+	if (!Predict(p_data_point, p_data_type))
 	{
-		free(data);
-		free(output);
-		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,NORMAL,"pointSIFT predict ERROR");
+		free(p_data_point);
+		free(p_data_type);
+		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,"pointSIFT predict ERROR");
 	}
 	auto end   = system_clock::now();
 	auto duration = duration_cast<microseconds>(end - start);
@@ -209,217 +210,394 @@ Error_manager Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr c
 	cout <<  double(duration.count()) << " "
 		 <<   microseconds::period::num << " " << microseconds::period::den   << "秒" << endl;
 
-	////������
-	RecoveryCloud(output, data, cloud_seg);
-	if (!FilterObs(cloud_seg,save_dir))
+	//第八步
+	//恢复点云,并填充到cloud_vector
+	t_error = recovery_float_to_cloud(p_data_type, p_data_point, cloud_vector);
+	if ( t_error != Error_code::SUCCESS )
 	{
-		free(data);
-		free(output);
-		return Error_manager(LOCATER_SIFT_FILTE_OBS_FAILED,NORMAL,"FilterObs failed");
-	}
-	free(output);
-	free(data);
-	//确保有车辆点云及轮胎存在,车辆类别 2,轮胎类别0
-	int segmentation_class_size=cloud_seg.size();
-	if(segmentation_class_size<3)
-    {
-	    return Error_manager(LOCATER_SIFT_PREDICT_NO_CAR_POINT,NORMAL,"PointSift detect no car point");
-    }
-	///保存中间文件
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seg(new pcl::PointCloud<pcl::PointXYZRGB>);
-	for (int i = 0; i < segmentation_class_size; ++i)
-	{
-		seg->operator +=(*cloud_seg[i]);
-	}
-	std::cout << "huli_test seg_size = " << seg->points.size() << std::endl;
-	static int count = 0;
-	count = (count + 1) % m_cls_num;
-	char buf[64] = { 0 };
-	sprintf(buf, "%s/SIFT_%d.txt", save_dir.c_str(), count);
-	save_rgb_cloud_txt(buf, *seg);
+		free(p_data_point);
+		free(p_data_type);
+		return t_error;
+	}
 
-	return SUCCESS;
+	//第九步
+	//保存点云数据
+	if ( save_flag )
+	{
+		t_error = save_cloud(cloud_vector,save_dir);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			free(p_data_point);
+			free(p_data_type);
+			return t_error;
+		}
+	}
+
+
+	//第十步
+	free(p_data_point);
+	free(p_data_type);
+
+	return Error_code::SUCCESS;
 }
 
-bool Point_sift_segmentation::RecoveryCloud(float* output, float* cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg)
+
+
+//使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
+Error_manager Point_sift_segmentation::filter_cloud_with_voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
 {
-	pcl::PointXYZ center;
-	center.x = (m_minp.x + m_maxp.x) / 2.0;
-	center.y = (m_minp.y + m_maxp.y) / 2.0;
+	if(p_cloud.get()==0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	//体素网格 下采样
+	pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
+	vgfilter.setInputCloud(p_cloud);
+	vgfilter.setLeafSize(0.02f, 0.02f, 0.02f);
+	vgfilter.filter(*p_cloud);
+	return Error_code::SUCCESS;
+}
+//使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
+Error_manager Point_sift_segmentation::filter_cloud_with_box(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+	if(p_cloud.get()==0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	//限制 x y
+	pcl::PassThrough<pcl::PointXYZ> passX;
+	pcl::PassThrough<pcl::PointXYZ> passY;
+	pcl::PassThrough<pcl::PointXYZ> passZ;
+	passX.setInputCloud(p_cloud);
+	passX.setFilterFieldName("x");
+	passX.setFilterLimits(m_cloud_box_limit.x_min, m_cloud_box_limit.x_max);
+	passX.filter(*p_cloud);
+
+	passY.setInputCloud(p_cloud);
+	passY.setFilterFieldName("y");
+	passY.setFilterLimits(m_cloud_box_limit.y_min, m_cloud_box_limit.y_max);
+	passY.filter(*p_cloud);
+
+	passZ.setInputCloud(p_cloud);
+	passZ.setFilterFieldName("z");
+	passZ.setFilterLimits(m_cloud_box_limit.z_min, m_cloud_box_limit.z_max);
+	passZ.filter(*p_cloud);
+	return Error_code::SUCCESS;
+}
+//通过输入点云,更新边界区域范围
+Error_manager Point_sift_segmentation::update_region(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+	pcl::PointXYZ min_point,max_point;
+	pcl::getMinMax3D(*p_cloud,min_point,max_point);
 
-	for (int k = 0; k < m_cls_num; ++k)
+	if(max_point.x<=min_point.x || max_point.y<=min_point.y)
 	{
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
-		cloud_seg.push_back(cloud_rgb);
+		return Error_manager(LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,MINOR_ERROR,
+							 "Point sift update_region errror :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y ");
 	}
-	for (int i = 0; i < m_point_num; ++i)
+	m_minp = min_point;
+	m_maxp = max_point;
+	return Error_code::SUCCESS;
+}
+//抽稀点云,	//自己写的算法, 重新生成新的点云.
+Error_manager Point_sift_segmentation::filter_cloud_with_my_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+
+	if(p_cloud.get()==NULL)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	Error_manager t_error;
+
+	//可以按照输入点云的数量, 然后动态修改 m_freq , 让其在抽稀之后,点云数量接近8192
+	//以后在写.
+
+	//按照m_freq的分辨率, 将点云拆分为很多网格
+	//网格大小 m_freq 的3次方, 网格总数 grid_number
+	int t_grid_length = int((m_maxp.x - m_minp.x) / m_freq) + 1;
+	int t_grid_width = int((m_maxp.y - m_minp.y) / m_freq) + 1;
+	int t_grid_height = int((m_maxp.z - m_minp.z) / m_freq) + 1;
+	int t_grid_number = t_grid_length * t_grid_width * t_grid_height;
+//	std::cout << " t_grid_length = "<<t_grid_length << " t_grid_width = "<<t_grid_width<< " t_grid_height = "<<t_grid_height<< std::endl;
+//	std::cout << "---------------------------"<< std::endl;
+
+	//创建网格->三维点的map, 每个网格里面只保留一个点,
+	map<int, pcl::PointXYZ> grid_point_map;
+	//遍历输入点云, 将每个点的索引编号写入对应的网格.
+	for (int i = 0; i < p_cloud->size(); ++i)
 	{
-		pcl::PointXYZ point;
-		point.x = *(cloud + i * 3)+center.x;
-		point.y = *(cloud + i * 3 + 1)+center.y;
-		point.z = *(cloud + i * 3 + 2);
-		if (point.x > m_maxp.x || point.x<m_minp.x
-			|| point.y>m_maxp.y || point.y<m_minp.y
-			|| point.z>m_maxp.z || point.z < m_minp.z)
+		//找到对应的网格.
+		pcl::PointXYZ point = p_cloud->points[i];
+		int id_x = (point.x - m_minp.x) / m_freq;
+		int id_y = (point.y - m_minp.y) / m_freq;
+		int id_z = (point.z - m_minp.z) / m_freq;
+
+//		std::cout << "------------point:"<< point << std::endl;
+//		std::cout << "------------m_minp:"<< m_minp << std::endl;
+//		std::cout << "------------m_freq:"<< m_freq << std::endl;
+//		std::cout << " id_x = "<<id_x << " id_y = "<<id_y<< " id_y = "<<id_y<< std::endl;
+
+		if (id_x < 0 || id_x >= t_grid_length || id_y < 0 || id_y >= t_grid_width || id_z < 0 || id_z >= t_grid_height)
 		{
+			//无效点.不要.
 			continue;
 		}
-		float* prob = output + i*m_cls_num;
-		int cls = 0;
-		float max = prob[0];
-		for (int j = 1; j < m_cls_num; j++)
+		grid_point_map[id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width] = point;
+		//注:如果有多个点属于同一个网格, 那个后者就会覆盖前者, 最终保留一个点.
+	}
+
+	//检查map
+	if ( grid_point_map.size() >0 )
+	{
+		//清空点云, 然后将map里面的点 重新加入p_cloud
+		p_cloud->clear();
+		for ( auto iter = grid_point_map.begin(); iter != grid_point_map.end(); iter++  )
 		{
-			if (prob[j] > max)
-			{
-				max = prob[j];
-				cls = j;
-			}
+			p_cloud->push_back(iter->second);
 		}
-		int r = 255, g = 255, b = 255;
-		if (cls == 1)
+	}
+	else
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_GRID_ERROR, Error_level::MINOR_ERROR,
+							 " filter_cloud_with_my_grid  error ");
+	}
+
+/*
+	//创建网格的数据, 内存大小为t_grid_number, 储存内容为是否存在点云的标记位.
+	bool* tp_grid = (bool*)malloc(t_grid_number*sizeof(bool));
+	memset(tp_grid, 0, t_grid_number*sizeof(bool));
+	//遍历输入点云, 将每个点映射到输入网格, 每个网格最多保留一个点,多余的删除.
+	for(pcl::PointCloud<pcl::PointXYZ>::iterator iter = p_cloud->begin(); iter !=p_cloud->end(); )
+	{
+		//找到对应的网格.
+		int id_x = (iter->x - m_minp.x) / m_freq;
+		int id_y = (iter->y - m_minp.y) / m_freq;
+		int id_z = (iter->z - m_minp.z) / m_freq;
+		if (id_x < 0 || id_x >= t_grid_length || id_y < 0 || id_y >= t_grid_width || id_z < 0 || id_z >= t_grid_height)
 		{
-			r = 0;
-			b = 0;
+			//无效点, 直接删除
+			iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
 		}
-		if (cls == 2)
+		else if(*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) == true)
 		{
-			b = 0;
-			g = 0;
+			//标记位为true,就表示这个网格已经有点了, 那么直接删除后者,保留前者
+			iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
 		}
-		if (cls < m_cls_num)
+		else
 		{
-			pcl::PointXYZRGB point_rgb;
-			point_rgb.x = point.x;
-			point_rgb.y = point.y;
-			point_rgb.z = point.z;
-			point_rgb.r = r;
-			point_rgb.g = g;
-			point_rgb.b = b;
-			cloud_seg[cls]->push_back(point_rgb);
+			*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) = true;
+			iter++;
 		}
 	}
-		
-	return true;
+	free(tp_grid);
+	*/
+
+	return Error_code::SUCCESS;
 }
 
+//从点云中选出固定数量的随机点, 默认8192个点
+Error_manager Point_sift_segmentation::filter_cloud_with_select(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
+{
+	if(p_cloud_in.get()==NULL || p_cloud_out.get()==NULL)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud_in->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	Error_manager t_error;
+
+	//从输入点云 p_cloud_in 里面选出 m_point_num 个点, 然后加入 p_cloud_out
+	//输入个数大于8192
+	if (p_cloud_in->size() > m_point_num)
+	{
+		//将点云随机打乱
+		std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
+		//选出输入点云前面8192个点.
+		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + m_point_num);
+	}
+	//输入个数小鱼8192
+	else if (p_cloud_in->size() < m_point_num)
+	{
+		int add = m_point_num - p_cloud_in->size();
+		//注意了, 输入点云不能少于4096, 否则点云分布太少, 会严重影响到TensorFlow的图像识别.
+		if (add > p_cloud_in->size())
+		{
+			return Error_manager(Error_code::LOCATER_SIFT_CLOUD_VERY_LITTLE, Error_level::MINOR_ERROR,
+								" filter_cloud_with_select  p_cloud_in->size() very little ");
+		}
+		//将点云随机打乱
+		std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
+		//选出输入点云8192个点, 不够的在加一次,
+		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + add);
+		p_cloud_out->points.insert(p_cloud_out->points.begin(), p_cloud_in->points.begin(), p_cloud_in->points.end());
+	}
+	//输入个数等于8192
+	else
+	{
+		//此时就不需要打乱了, 直接全部复制.
+		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.end());
+	}
+
+	if (p_cloud_out->points.size() != m_point_num)
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_SELECT_ERROR, Error_level::MINOR_ERROR,
+							" Point_sift_segmentation::filter_cloud_with_select  error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//将点云数据转换到float*
+Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, float* p_data_out)
+{
+	if(p_cloud_in.get()==NULL)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud_in->size() != m_point_num)
+	{
+		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+							" Point_sift_segmentation::translate_cloud_to_float p_cloud_in->size() error ");
+	}
+
+	if ( p_data_out == NULL )
+	{
+	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+	    					" translate_cloud_to_float  p_data_out POINTER_IS_NULL ");
+	}
+
+	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+2) = point.z ;
+		//注:三维点云的数据 不用做缩放和平移,
+		// 这个在雷达扫描时就进行了标定和转化, 雷达传输过来的坐标就已经是公共坐标系了, (单位米)
+	}
+	return Error_code::SUCCESS;
+}
 
-bool Point_sift_segmentation::FilterObs(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg,std::string save_dir)
+//恢复点云, 将float*转换成点云
+//p_data_type:输入点云对应的类别,大小为  点数*类别
+//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)
 {
-	/*if (cloud_seg.size() != m_cls_num)
-	{
-		LOG(ERROR) << "\t cloud_seg.size() != m_cls_num";
-		return false;
-	}*/
-	
-	const int obs_id = m_cls_num - 1;
-	const int target_id = 1;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr obs_cloud = cloud_seg[obs_id];
-
-	if(cloud_seg.size()>0) {
-        std::string sift_in = save_dir + "/c0.txt";
-        save_rgb_cloud_txt(sift_in, *cloud_seg[0]);
-    }
-    if(cloud_seg.size()>1) {
-        std::string sift_in = save_dir + "/c1.txt";
-        save_rgb_cloud_txt(sift_in, *cloud_seg[1]);
-    }
-
-    if(cloud_seg.size()>2) {
-        std::string sift_in = save_dir + "/c2.txt";
-        save_rgb_cloud_txt(sift_in, *cloud_seg[2]);
-    }
-
-    return true;
-
-
-	/*if (obs_cloud->size() > 100)
-	{
-		////ŷʽ����
-		pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree_upground(new pcl::search::KdTree<pcl::PointXYZRGB>);
-		tree_upground->setInputCloud(obs_cloud);
-
-		std::vector<pcl::PointIndices> cluster_indices_upground;
-		pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
-		ec.setClusterTolerance(100); // 10cm
-		ec.setMinClusterSize(30);
-		ec.setMaxClusterSize(10000);
-		ec.setSearchMethod(tree_upground);
-		ec.setInputCloud(obs_cloud);
-		ec.extract(cluster_indices_upground);
-
-		std::vector<pcl::PointCloud<pcl::PointXYZRGB>> clusters_obs;
-		for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_upground.begin(); it != cluster_indices_upground.end(); ++it)
+	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)
 		{
-			pcl::PointCloud<pcl::PointXYZRGB> cloud_cluster;
-			for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
-				cloud_cluster.push_back(obs_cloud->points[*pit]); //*
-			cloud_cluster.width = cloud_cluster.size();
-			cloud_cluster.height = 1;
-			cloud_cluster.is_dense = true;
-			clusters_obs.push_back(cloud_cluster);
+			continue;
 		}
-		/////  �� obs ���������
-		std::vector<std::vector<cv::Point2f> > contours;
-		for (int k = 0; k < clusters_obs.size(); ++k)
+
+		//当前点 的类别百分比的指针
+		float* tp_type_percent = p_data_type + i*m_cls_num;
+		//当前点 的类别, 初始化为0
+		int t_cls = 0;
+		//当前点 的类别, 百分比的最大值
+		float t_type_percent_max = tp_type_percent[0];
+		//循环比较, 找出百分比最大的类别
+		for (int j = 1; j < m_cls_num; j++) //前面已经把第0个用来初始化了, 后续从第下一个开始比较
 		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected_XY(new pcl::PointCloud<pcl::PointXYZRGB>);
-			pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
-			coefficients->values.resize(4);
-			coefficients->values[0] = 0.;
-			coefficients->values[1] = 0.;
-			coefficients->values[2] = 1.0;
-			coefficients->values[3] = 0.;
-
-			// Create the filtering object
-			pcl::ProjectInliers<pcl::PointXYZRGB> proj;
-			proj.setModelType(pcl::SACMODEL_PLANE);
-			proj.setInputCloud(clusters_obs[k].makeShared());
-			proj.setModelCoefficients(coefficients);
-			proj.filter(*cloud_projected_XY);
-
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_convexhull(new pcl::PointCloud<pcl::PointXYZRGB>);
-			pcl::ConvexHull<pcl::PointXYZRGB> cconvexhull;
-			cconvexhull.setInputCloud(cloud_projected_XY);
-			cconvexhull.setDimension(2);
-			cconvexhull.reconstruct(*cloud_convexhull);
-
-			if (cloud_convexhull->size() > 3)
+			if (tp_type_percent[j] > t_type_percent_max)
 			{
-				std::vector<cv::Point2f> contour;
-				for (int j = 0; j < cloud_convexhull->size(); ++j)
-				{
-					pcl::PointXYZRGB point = cloud_convexhull->points[j];
-					cv::Point2f  pt(point.x, point.y);
-					contour.push_back(pt);
-				}
-				contours.push_back(contour);
+				t_type_percent_max = tp_type_percent[j];
+				t_cls = j;
 			}
 		}
-		//// ��Ŀ������
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZRGB>);
-		for (int i = 0; i < cloud_seg[target_id]->size(); ++i)
+
+		//根据点的类别, 添加颜色, 并且分别加入到各自的点云里面
+		switch ( t_cls )
 		{
-			pcl::PointXYZRGB point = cloud_seg[target_id]->points[i];
-			cv::Point2f  pt(point.x, point.y);
-			bool valid = true;
-			for (int n = 0; n < contours.size(); ++n)
-			{
-				if (cv::pointPolygonTest(contours[n], pt, true)>-300.)
-				{
-					valid = false;
-					break;
-				}
-			}
-			if (valid)
-			{
-				cloud_target->push_back(point);
-			}
+		    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_seg[target_id] = cloud_target;
-		char buf[255] = { 0 };
-		static int count = 0;
-		sprintf(buf, "%s/target_%d.txt", save_dir.c_str(), count++%m_cls_num);
-		save_rgb_cloud_txt(buf, *cloud_target);
+		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 true;*/
+
+
+	return Error_code::SUCCESS;
+}
+
+//保存点云数据
+Error_manager Point_sift_segmentation::save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,std::string save_dir)
+{
+	if(cloud_vector.size()>0) {
+		std::string sift_in = save_dir + "/SIFT_cls_0.txt";
+		save_rgb_cloud_txt(sift_in, *cloud_vector[0]);
+	}
+	if(cloud_vector.size()>1) {
+		std::string sift_in = save_dir + "/SIFT_cls_1.txt";
+		save_rgb_cloud_txt(sift_in, *cloud_vector[1]);
+	}
+
+	if(cloud_vector.size()>2) {
+		std::string sift_in = save_dir + "/SIFT_cls_2.txt";
+		save_rgb_cloud_txt(sift_in, *cloud_vector[2]);
+	}
+
+	std::string sift_in = save_dir + "/SIFT_TOTAL.txt";
+	save_rgb_cloud_txt(sift_in, cloud_vector);
+
+	return Error_code::SUCCESS;
 }

locate/point_sift_segmentation.h

@@ -6,6 +6,17 @@
 #include <pcl/point_types.h>
 #include <pcl/common/common.h>
 
+//Cloud_box,点云的三维方框,
+typedef struct Cloud_box
+{
+	double x_min;
+	double x_max;
+	double y_min;
+	double y_max;
+	double z_min;
+	double z_max;
+}Cloud_box;
+
 /*
  * 封装PointSift点云分割网络,输入点云,分割出汽车,检验结果
  * 功能包括将pcl点云数据格式转换成原始数据格式float*
@@ -15,34 +26,51 @@ class Point_sift_segmentation : public PointSifter
 {
 public:
 	Point_sift_segmentation(int point_size,int cls,float freq,pcl::PointXYZ minp,pcl::PointXYZ maxp);
-    //设置要识别的点云区域
-    Error_manager set_region(pcl::PointXYZ minp, pcl::PointXYZ maxp);
+	Point_sift_segmentation(int point_size,int cls,float freq, Cloud_box cloud_box);
+	virtual ~Point_sift_segmentation();
+
     //初始化网络, 加载网络权重
 	virtual Error_manager init(std::string graph,std::string cpkt);
+
 	//分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
-	//cloud:输入点云
-	//clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)
-	virtual Error_manager seg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-	    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& clouds, std::string save_dir);
+	//cloud:输入点云, (有内存)
+	//cloud_vector:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
+	virtual Error_manager segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+							  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
+							  bool save_flag, std::string save_dir);
+
 
-	virtual ~Point_sift_segmentation();
-protected:
-    //将点云数据转换到float*
-    //cloud:输入点云
-    //output:转换结果保存地址,函数内不负责内存的分配与释放
-	bool	Create_data(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float* output);
-    //将float*转换成点云
-    //output:点云对应的类别,大小为  点数*类别
-    //cloud:点云数据(xyz)
-    //cloud_seg::输入带颜色的点云
-	bool    RecoveryCloud(float* output, float* cloud, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg);
-
-	bool	FilterObs(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_seg, std::string save_dir);
 protected:
-	float		        m_freq;
-    pcl::PointXYZ		m_minp;
-    pcl::PointXYZ		m_maxp;
 
+
+	//使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
+	Error_manager filter_cloud_with_voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud);
+	//使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
+	Error_manager filter_cloud_with_box(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud);
+	//通过输入点云,更新边界区域范围
+	Error_manager update_region(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud);
+	//抽稀点云,	//自己写的算法, 重新生成新的点云.
+	Error_manager filter_cloud_with_my_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud);
+	//从点云中选出固定数量的随机点, 默认8192个点
+	//p_cloud_in::输入点云, 有内存,且size>0
+	//p_cloud_out::输出点云, 需要提前分配内存. size==0
+	Error_manager filter_cloud_with_select(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out);
+	//将点云数据转换到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
+	Error_manager recovery_float_to_cloud(float* 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;		//限制点云的方框, 只有在有效区域内的点才能参与计算.
 };
 #endif
 

locate/tf_wheel_3Dcnn_api.h

@@ -0,0 +1,30 @@
+/*
+ * 3dcnn 识别api
+ */
+#ifndef __tf_wheel_3Dcnn__H
+#define __tf_wheel_3Dcnn__H
+#include <string>
+
+#ifndef LIB_3DCNN_API
+#ifdef LIB_EXPORTS
+#if defined(_MSC_VER)
+#define LIB_3DCNN_API __declspec(dllexport)
+#else
+#define LIB_3DCNN_API __attribute__((visibility("default")))
+#endif
+#else
+#if defined(_MSC_VER)
+#define LIB_3DCNN_API
+#else
+#define LIB_3DCNN_API
+#endif
+#endif
+#endif
+//初始化3dcnn网络参数,长宽高,权重文件
+LIB_3DCNN_API int		 tf_wheel_3dcnn_init(std::string model_file,int length, int width, int height);
+//3dcnn网络预测
+LIB_3DCNN_API bool	 tf_wheel_3dcnn_predict(float* data,float* pOut);
+//关闭网络
+LIB_3DCNN_API int		 tf_wheel_3dcnn_close();
+
+#endif

main.cpp

@@ -6,10 +6,13 @@
 #include "./laser/laser_manager.h"
 #include "./laser/Laser.h"
 #include "./laser/LivoxLaser.h"
-#include "./locate/point_sift_segmentation.h"
+
+#include "./locate/locate_manager.h"
 #include <iostream>
 #include <fstream>
 
+#include "./tool/communication_socket_base.h"
+
 #include "./error_code/error_code.h"
 #include "LogFiles.h"
 #include <string.h>
@@ -28,33 +31,33 @@ using google::protobuf::Message;
 #define O_RDONLY	     00
 
 #define LIVOX_NUMBER	     2
-/*
-//读取protobuf 配置文件
-//file:文件
-//parameter:要读取的配置
-bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
-{
-	int fd = open(file.c_str(), O_RDONLY);
-	if (fd == -1) return false;
-	FileInputStream* input = new FileInputStream(fd);
-	bool success = google::protobuf::TextFormat::Parse(input, &parameter);
-	delete input;
-	close(fd);
-	return success;
-}
-*/
 
 
 int main(int argc,char* argv[])
 {
+	Communication_socket_base csb;
+	std::vector<std::string> connect_string_vector;
+	connect_string_vector.push_back("tcp://192.168.2.166:9001");
+	csb.communication_init("tcp://192.168.2.166:9000", connect_string_vector);
+//	csb.communication_init();
+
+	char ch ;
+	std::cin >> ch ;
+	return 0;
+
+	LOG(INFO) << " --- main start --- " ;
+
 
 	Error_manager t_error;
 	Error_manager t_result ;
 
-	std::cout << "huli 101 main start!" << std::endl;
 
 
+	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();
 
 	Point_sift_segmentation*                mp_point_sift;
 	int point_size = 8192;
@@ -90,13 +93,16 @@ int main(int argc,char* argv[])
 		}
 		std::cout << "  start scan ------------" << std::endl;
 
-		int n = 2;
+		int n = 1;
 		while(n)
 		{
 			n--;
-			pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+			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);
@@ -107,8 +113,8 @@ int main(int argc,char* argv[])
 
 			Laser_manager_task * laser_manager_task = new Laser_manager_task;
 			laser_manager_task->task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
-										  1000,true,t_save_path,&cloud_lock,scan_cloud,
-										  true,select_laser_id_vector	);
+										  true,t_save_path,&cloud_lock,&point_cloud_map,false,
+										  1000,false,select_laser_id_vector	);
 
 
 
@@ -117,7 +123,7 @@ int main(int argc,char* argv[])
 
 			if ( t_error != Error_code::SUCCESS )
 			{
-				return -1;
+				std::cout << "huli Laser_manager:::::" << t_error.to_string() << std::endl;
 			}
 			else
 			{
@@ -132,15 +138,29 @@ int main(int argc,char* argv[])
 													" laser_manager_task is_over_time ");
 						laser_manager_task->set_task_error_manager(t_error);
 			        }
-					std::this_thread::yield();
+			        else
+			        {
+						std::this_thread::yield();
+			        }
+
 			    }
+			    std::cout << "huli task_error::::"  << laser_manager_task->get_task_error_manager().to_string() << std::endl;
+
 			}
 			delete(laser_manager_task);
-			std::cout << t_error.to_string() << std::endl;
 
 
+			std::cout << "huli laser result---------------------" << t_error.to_string() << std::endl;
+			std::cout << "huli zczxczxczx---------------------" << t_error.to_string() << std::endl;
+			std::cout << "huli zczxczxczx---------------------" << t_error.to_string() << std::endl;
+			std::cout << "huli zczxczxczx---------------------" << t_error.to_string() << std::endl;
 
 
+
+/*
+			pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud ;
+			scan_cloud =  point_cloud_map[0];
+
 			//locate
 			pcl::getMinMax3D(*scan_cloud,minp,maxp);
 			std::cout << "huli 112" << std::endl;
@@ -151,24 +171,59 @@ int main(int argc,char* argv[])
 			std::cout << t_error.to_string() << std::endl;
 
 			std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
-			t_error = mp_point_sift->seg(scan_cloud, segmentation_clouds, "../data/locate_data");
+			t_error = mp_point_sift->segmentation(scan_cloud, segmentation_clouds, true, "../data/locate_data");
 			std::cout << t_error.to_string() << std::endl;
-		}
+			*/
 
-		cout << "huli: 601 :" << t_error.to_string() << endl;
-		//发送任务单给雷达
-		std::this_thread::sleep_for(std::chrono::seconds(5));
+			Locate_manager_task * locate_manager_task = new Locate_manager_task;
+			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 )
+			{
+				std::cout << "huli Locate_manager:::::" << t_error.to_string() << std::endl;
+			}
+			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::yield();
+					}
 
+				}
 
+				std::cout << "huli task_error:::::"  << locate_manager_task->get_task_error_manager().to_string() << std::endl;
+
+			}
+			delete(locate_manager_task);
+			std::cout << "huli locate result:::::" << t_error.to_string() << std::endl;
 
+		}
 
+		cout << "huli: 601 :" << t_error.to_string() << endl;
+		std::this_thread::sleep_for(std::chrono::seconds(2));
 
 	}
 
 	Laser_manager::get_instance_pointer()->laser_manager_uninit();
+	Locate_manager::get_instance_pointer()->Locate_manager_uninit();
 
 	cout << "huli 1234 main end" << endl;
+	std::this_thread::sleep_for(std::chrono::seconds(3));
+
 
 	return 0;
 }

setting/laser.prototxt

@@ -22,3 +22,4 @@ laser_parameters
     mat_r22:1
     mat_r23:0
 }
+

setting/locate.prototxt

@@ -0,0 +1,47 @@
+area{
+    x_min:-1227.5
+    x_max:1802.9
+    y_min:-2789.8
+    y_max:3777.19
+    z_min:0
+    z_max:1800
+}
+
+net_3dcnn_parameter
+{
+    length:0.224
+    width:0.224
+    height:0.096
+    freq:0.025
+    nclass:3
+    weights_file:"./setting/3dcnn_model.pb"
+}
+
+seg_parameter
+{
+    point_size:8192
+    cls_num:2
+    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
+    }
+    graph:"../model_param/seg_model_404500.ckpt.meta"
+    cpkt:"../model_param/seg_model_404500.ckpt"
+}
+
+yolo_parameter
+{
+    cfg:"./setting/yolov3-spot2.cfg"
+    weights:"./setting/yolov3-spot2_12000.weights"
+    min_x:-1227.5
+    max_x:10802.9
+    min_y:-2789.8
+    max_y:3777.19
+    freq:25.
+}

task/task_base.cpp

@@ -7,6 +7,10 @@
 
 Task_Base::Task_Base()
 {
+	static unsigned int t_task_id = 0;
+	m_task_id = t_task_id;
+	t_task_id++;
+
     m_task_type = UNKNOW_TASK;
 	m_task_statu = TASK_CREATED;
 	mp_tast_receiver = NULL;
@@ -39,7 +43,7 @@ bool Task_Base::is_over_time()
 //判断是否结束, TASK_OVER  TASK_ERROR TASK_DEAD 都算结束
 bool Task_Base::is_task_end()
 {
-	if(m_task_statu == TASK_OVER || m_task_statu == TASK_ERROR || m_task_statu == TASK_ERROR)
+	if(m_task_statu == TASK_OVER || m_task_statu == TASK_ERROR || m_task_statu == TASK_DEAD)
 	{
 		return true;
 	}
@@ -52,7 +56,11 @@ bool Task_Base::is_task_end()
 
 
 
-
+//获取 任务单id
+unsigned int  Task_Base::get_task_id()
+{
+	return m_task_id;
+}
 
 //获取任务类型
 Task_type Task_Base::get_task_type()

task/task_base.h

@@ -20,7 +20,7 @@ enum Task_type
 	UNKNOW_TASK             =0,				//未知任务单//初始化,默认值
 	LASER_MANGER_SCAN_TASK  =1,             //雷达管理模块的扫描任务,
 	LASER_BASE_SCAN_TASK    =2,             //单个雷达的扫描任务,
-    LOCATE_TASK             =3,             //测量任务
+    LOCATE_MANGER_TASK		=3,             //测量任务
     PLC_TASK                =4,             //上传PLC任务
 
     WJ_TASK,
@@ -61,6 +61,12 @@ public:
 	bool is_task_end();
 
 public:
+
+	//获取 任务单id
+	unsigned int  get_task_id();
+	//设置 任务单id
+//	void  set_task_id(unsigned int task_id) = delete;
+
     //获取 任务类型
     Task_type   get_task_type();
 	//设置 任务类型
@@ -95,6 +101,7 @@ public:
 
 
 protected:
+	unsigned int				m_task_id;						//任务id, 每次新建任务, 自动+1, 用于多任务的管理
     Task_type                   m_task_type;					//任务类型，不允许中途修改
     Task_statu                  m_task_statu;					//任务状态
     std::string					m_task_statu_information;		//任务状态说明

task/task_command_manager.cpp

@@ -4,7 +4,7 @@
 #include "task_command_manager.h"
 #include "../laser/Laser.h"
 #include "../laser/laser_manager.h"
-
+#include "../locate/locate_manager.h"
 
 //对外的接口函数，所有的任务发送方，都必须使用该函数。
 //execute_task在内部解析了Task_Base里面的Task_type，然后转发给具体某个模块的实例对象。
@@ -22,26 +22,33 @@ Error_manager Task_command_manager::execute_task(Task_Base* p_task_base)
 		case LASER_MANGER_SCAN_TASK:
 			if ( tp_tast_receiver != NULL )
 			{
-				((Laser_manager*)tp_tast_receiver)->execute_task(p_task_base);
+				t_error = ((Laser_manager*)tp_tast_receiver)->execute_task(p_task_base);
 			}
 			else
 			{
-				Laser_manager::get_instance_references().execute_task(p_task_base);
+				t_error = Laser_manager::get_instance_references().execute_task(p_task_base);
 			}
 			break;
 		case LASER_BASE_SCAN_TASK:
 			if ( tp_tast_receiver != NULL )
 			{
-				((Laser_base*)tp_tast_receiver)->execute_task(p_task_base);
+				t_error = ((Laser_base*)tp_tast_receiver)->execute_task(p_task_base);
 			}
 			else
 			{
-				return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
-									" fun error ");
+				return Error_manager(Error_code::TASK_NO_RECEIVER, Error_level::MINOR_ERROR,
+									" Task_command_manager::execute_task have not receiver  ");
 	        }
 	        break;
-		case LOCATE_TASK:
-			;
+		case LOCATE_MANGER_TASK:
+			if ( tp_tast_receiver != NULL )
+			{
+				t_error = ((Locate_manager*)tp_tast_receiver)->execute_task(p_task_base);
+			}
+			else
+			{
+				t_error = Locate_manager::get_instance_references().execute_task(p_task_base);
+			}
 			break;
 		case PLC_TASK:
 			;
@@ -50,7 +57,8 @@ Error_manager Task_command_manager::execute_task(Task_Base* p_task_base)
 			;
 			break;
 	    default:
-
+			t_error.error_manager_reset(Error_code::TASK_TYPE_IS_UNKNOW, Error_level::MINOR_ERROR,
+										" p_task_base->get_task_type() is  UNKNOW_TASK ");
 	        break;
 	}
 

tool/binary_buf.cpp

@@ -45,6 +45,8 @@ Binary_buf::~Binary_buf()
 		mp_buf = NULL;
 	}
 	m_length = 0;
+
+//	std::cout << "Binary_buf::~Binary_buf()" << std::endl;
 }
 
 
@@ -67,6 +69,26 @@ Binary_buf::Binary_buf(const char* p_buf, int len)
 	}
 }
 
+
+//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+Binary_buf::Binary_buf(char* p_buf, int len)
+{
+	mp_buf = NULL;
+	m_length = 0;
+
+	if ( p_buf != NULL)
+	{
+		if (len <= 0)
+		{
+			len = strlen(p_buf);
+		}
+
+		mp_buf = (char*)malloc(len);
+		memcpy(mp_buf, p_buf, len);
+		m_length = len;
+	}
+}
+
 //重载=，深拷贝，
 Binary_buf& Binary_buf::operator=(const Binary_buf& other)
 {

tool/binary_buf.h

@@ -15,6 +15,7 @@
 
 #ifndef LIDARMEASURE_BINARY_BUF_H
 #define LIDARMEASURE_BINARY_BUF_H
+#include <iostream>
 
 
 //雷达消息的类型
@@ -42,6 +43,8 @@ public:
 
 	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
 	Binary_buf(const char* p_buf, int len = 0);
+	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
+	Binary_buf(char* p_buf, int len = 0);
 	//重载=，深拷贝，
 	Binary_buf& operator=(const Binary_buf& other);
 	//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'

tool/communication_socket_base.cpp

@@ -0,0 +1,464 @@
+
+
+
+#include "communication_socket_base.h"
+
+Communication_socket_base::Communication_socket_base()
+{
+	m_communication_statu = COMMUNICATION_UNKNOW;
+
+	mp_receive_data_thread = NULL;
+	mp_analysis_data_thread = NULL;
+	mp_send_data_thread = NULL;
+	mp_encapsulate_data_thread = NULL;
+}
+
+Communication_socket_base::~Communication_socket_base()
+{
+	communication_uninit();
+}
+
+//初始化 通信 模块。如下三选一
+Error_manager Communication_socket_base::communication_init()
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+
+	Error_manager t_error;
+	int t_socket_result;
+
+	//m_socket 自己作为一个服务器, 绑定一个端口
+	t_socket_result = m_socket.bind("tcp://192.168.2.166:9000");
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_BIND_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.bind error ");
+	}
+	//m_socket 和远端通信, 连接远端服务器的端口
+	t_socket_result = m_socket.connect("tcp://192.168.2.166:9001");
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_CONNECT_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.connect error ");
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;
+}
+
+//初始化
+Error_manager Communication_socket_base::communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector)
+{
+	LOG(INFO) << " ---Communication_socket_base::communication_init() run--- "<< this;
+	Error_manager t_error;
+
+	t_error = communication_bind(bind_string);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	t_error = communication_connect(connect_string_vector);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+
+	//启动通信, run thread
+	communication_run();
+
+	return Error_code::SUCCESS;}
+//bind
+Error_manager Communication_socket_base::communication_bind(std::string bind_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+
+	//m_socket 自己作为一个服务器, 绑定一个端口
+	t_socket_result = m_socket.bind(bind_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_BIND_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.bind error ");
+	}
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::vector<std::string>& connect_string_vector)
+{
+	Error_manager t_error;
+	for (auto iter = connect_string_vector.begin(); iter != connect_string_vector.end(); ++iter)
+	{
+		t_error = communication_connect(*iter);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			return t_error;
+		}
+	}
+	return Error_code::SUCCESS;
+}
+//connect
+Error_manager Communication_socket_base::communication_connect(std::string connect_string)
+{
+	Error_manager t_error;
+	int t_socket_result;
+	//m_socket 和远端通信, 连接远端服务器的端口
+	t_socket_result = m_socket.connect(connect_string);
+	if ( t_socket_result <0 )
+	{
+		return Error_manager(Error_code::COMMUNICATION_CONNECT_ERROR, Error_level::MINOR_ERROR,
+							 " m_socket.connect error ");
+	}
+	return Error_code::SUCCESS;
+}
+//启动通信, run thread
+Error_manager Communication_socket_base::communication_run()
+{
+	m_communication_statu = COMMUNICATION_READY;
+	//启动4个线程。
+	//接受线程默认循环, 内部的nn_recv进行等待, 超时1ms
+	m_receive_condition.reset(false, true, false);
+	mp_receive_data_thread = new std::thread(&Communication_socket_base::receive_data_thread, this);
+	//解析线程默认等待, 需要接受线程去唤醒, 超时1ms, 超时后主动遍历m_receive_data_list
+	m_analysis_data_condition.reset(false, false, false);
+	mp_analysis_data_thread = new std::thread(&Communication_socket_base::analysis_data_thread, this);
+	//发送线程默认循环, 内部的wait_and_pop进行等待,
+	m_send_data_condition.reset(false, true, false);
+	mp_send_data_thread = new std::thread(&Communication_socket_base::send_data_thread, this);
+	//封装线程默认等待, ...., 超时1ms, 超时后主动 封装心跳和状态信息,
+	m_encapsulate_data_condition.reset(false, false, false);
+	mp_encapsulate_data_thread = new std::thread(&Communication_socket_base::encapsulate_data_thread, this);
+
+	return Error_code::SUCCESS;
+}
+
+
+//反初始化 通信 模块。
+Error_manager Communication_socket_base::communication_uninit()
+{
+	//终止list，防止 wait_and_pop 阻塞线程。
+	m_receive_data_list.termination_list();
+	m_send_data_list.termination_list();
+
+	//杀死4个线程，强制退出
+	if (mp_receive_data_thread)
+	{
+		m_receive_condition.kill_all();
+	}
+	if (mp_analysis_data_thread)
+	{
+		m_analysis_data_condition.kill_all();
+	}
+	if (mp_send_data_thread)
+	{
+		m_send_data_condition.kill_all();
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		m_encapsulate_data_condition.kill_all();
+	}
+	//回收4个线程的资源
+	if (mp_receive_data_thread)
+	{
+		mp_receive_data_thread->join();
+		delete mp_receive_data_thread;
+		mp_receive_data_thread = NULL;
+	}
+	if (mp_analysis_data_thread)
+	{
+		mp_analysis_data_thread->join();
+		delete mp_analysis_data_thread;
+		mp_analysis_data_thread = 0;
+	}
+	if (mp_send_data_thread)
+	{
+		mp_send_data_thread->join();
+		delete mp_send_data_thread;
+		mp_send_data_thread = NULL;
+	}
+	if (mp_encapsulate_data_thread)
+	{
+		mp_encapsulate_data_thread->join();
+		delete mp_encapsulate_data_thread;
+		mp_encapsulate_data_thread = NULL;
+	}
+
+	//清空list
+	m_receive_data_list.clear_and_delete();
+	m_send_data_list.clear_and_delete();
+
+	m_communication_statu = COMMUNICATION_UNKNOW;
+	m_socket.close();
+
+	return Error_code::SUCCESS;
+}
+
+
+
+
+
+
+
+//mp_receive_data_thread 接受线程执行函数，
+//receive_data_thread 内部线程负责接受消息
+void Communication_socket_base::receive_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::receive_data_thread start "<< this;
+
+	//通信接受线程, 负责接受socket消息, 并存入 m_receive_data_list
+	while (m_receive_condition.is_alive())
+	{
+		m_receive_condition.wait();
+		if ( m_receive_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			std::unique_lock<std::mutex> lk(m_mutex);
+			//flags为1, 非阻塞接受消息, 如果接收到消息, 那么接受数据长度大于0
+			nnxx::message t_msg = m_socket.recv(1);
+			if ( t_msg.size()>0 )
+			{
+				Binary_buf * tp_binary_buf = new Binary_buf( (char*)(t_msg.data()), t_msg.size() );
+//				std::cout << tp_binary_buf->get_buf() << std::endl;
+				bool is_push = m_receive_data_list.push(tp_binary_buf);
+//				if ( is_push == false )
+//				{
+//					return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//										 " m_receive_data_list.push error ");
+//				}
+
+				//唤醒解析线程一次,
+				m_analysis_data_condition.notify_all(false, true);
+			}
+
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::receive_data_thread end "<< this;
+	return;
+}
+
+//mp_analysis_data_thread 解析线程执行函数，
+//analysis_data_thread 内部线程负责解析消息
+void Communication_socket_base::analysis_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread start "<< this;
+
+	//通信解析线程, 负责巡检m_receive_data_list, 并解析和处理消息
+	while (m_analysis_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_analysis_data_condition.wait_for_millisecond(1000);
+		if ( m_analysis_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果解析线程被主动唤醒, 那么就表示 收到新的消息, 那就遍历整个链表
+			if ( t_pass_flag )
+			{
+				analysis_receive_list();
+			}
+				//如果解析线程超时通过, 那么就定时处理链表残留的消息,
+			else
+			{
+				analysis_receive_list();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::analysis_data_thread end "<< this;
+	return;
+}
+
+//循环接受链表, 解析消息,
+Error_manager Communication_socket_base::analysis_receive_list()
+{
+	Error_manager t_error;
+	if ( m_receive_data_list.m_termination_flag )
+	{
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::analysis_receive_list error ");
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_receive_data_list.m_mutex);
+		for (auto iter = m_receive_data_list.m_data_list.begin(); iter != m_receive_data_list.m_data_list.end(); )
+		{
+			Binary_buf* tp_buf = **iter;
+			if ( tp_buf == NULL )
+			{
+				iter = m_receive_data_list.m_data_list.erase(iter);
+				//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+			}
+			else
+			{
+				//检查消息是否可以被解析
+				t_error = check_msg(tp_buf);
+				if ( t_error == SUCCESS)
+				{
+					//处理消息
+					t_error = excute_msg(tp_buf);
+//				if ( t_error )
+//				{
+//					//执行结果不管
+//				}
+//				else
+//				{
+//					//执行结果不管
+//				}
+					delete(tp_buf);
+					tp_buf = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+				}
+				else if( t_error == COMMUNICATION_ANALYSIS_TIME_OUT )
+				{
+					//超时了就直接删除
+					delete(tp_buf);
+					tp_buf = NULL;
+					iter = m_receive_data_list.m_data_list.erase(iter);
+					//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+				}
+				else //if( t_error == COMMUNICATION_EXCUTER_IS_BUSY)
+				{
+					//处理器正忙, 那就不做处理, 直接处理下一个
+					//注:这条消息就被保留了下来, wait_for_millisecond 超时通过之后, 会循环检查残留的消息.
+					iter++;
+				}
+			}
+		}
+	}
+	return Error_code::SUCCESS;
+}
+
+//检查消息是否可以被解析
+Error_manager Communication_socket_base::check_msg(Binary_buf* p_buf)
+{
+	//检查对应模块的状态, 判断是否可以处理这条消息
+	//同时也要判断是否超时, 超时返回 COMMUNICATION_ANALYSIS_TIME_OUT
+	//如果处理器正在忙别的, 那么返回 COMMUNICATION_EXCUTER_IS_BUSY
+
+	//......................
+
+	std::cout << "Communication_socket_base::check_msg  p_buf =  " << p_buf->get_buf() << std::endl;
+	std::cout << "Communication_socket_base::check_msg   size =  " << p_buf->get_length() << std::endl;
+
+//	return Error_code::COMMUNICATION_ANALYSIS_TIME_OUT;
+//	return Error_code::COMMUNICATION_EXCUTER_IS_BUSY;
+	return Error_code::SUCCESS;
+}
+
+//处理消息
+Error_manager Communication_socket_base::excute_msg(Binary_buf* p_buf)
+{
+	//先将 p_buf 转化为 对应的格式, 不能一直使用 p_buf, 和这个是要销毁的
+	//然后处理这个消息, 就是调用对应模块的 excute 接口函数
+	//执行结果不管, 如果需要答复, 那么对应模块 在自己内部 封装一条消息发送即可.
+
+	std::cout << "Communication_socket_base::excute_msg  p_buf =  " << p_buf->get_buf() << std::endl;
+	std::cout << "Communication_socket_base::excute_msg   size =  " << p_buf->get_length() << std::endl;
+	return Error_code::SUCCESS;
+}
+
+//mp_send_data_thread 发送线程执行函数，
+//send_data_thread 内部线程负责发送消息
+void Communication_socket_base::send_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::send_data_thread start "<< this;
+
+	//通信发送线程, 负责巡检m_send_data_list, 并发送消息
+	while (m_send_data_condition.is_alive())
+	{
+		m_send_data_condition.wait();
+		if ( m_send_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+
+			Binary_buf* tp_buf = NULL;
+			//这里 wait_and_pop 会使用链表内部的 m_data_cond 条件变量来控制等待,
+			//封装线程使用push的时候, 会唤醒线程并通过等待, 此时 m_send_data_condition 是一直通过的.
+			//如果需要退出, 那么就要 m_send_data_list.termination_list();	和 m_send_data_condition.kill_all();
+			bool is_pop = m_send_data_list.wait_and_pop(tp_buf);
+			if ( is_pop )
+			{
+				if ( tp_buf != NULL )
+				{
+					std::unique_lock<std::mutex> lk(m_mutex);
+					m_socket.send(tp_buf->get_buf(), tp_buf->get_length(), 0);
+					delete(tp_buf);
+					tp_buf = NULL;
+				}
+			}
+			else
+			{
+				//没有取出, 那么应该就是 m_termination_flag 结束了
+//				return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+//									 " Communication_socket_base::send_data_thread() error ");
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::send_data_thread end "<< this;
+	return;
+}
+
+//mp_encapsulate_data_thread 封装线程执行函数，
+//encapsulate_data_thread 内部线程负责封装消息
+void Communication_socket_base::encapsulate_data_thread()
+{
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread start "<< this;
+
+	//通信封装线程, 负责定时封装消息, 并存入 m_send_data_list
+	while (m_encapsulate_data_condition.is_alive())
+	{
+		bool t_pass_flag = m_encapsulate_data_condition.wait_for_millisecond(1000);
+		if ( m_encapsulate_data_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//如果封装线程被主动唤醒, 那么就表示 需要主动发送消息,
+			if ( t_pass_flag )
+			{
+				//主动发送消息,
+			}
+				//如果封装线程超时通过, 那么就定时封装心跳和状态信息
+			else
+			{
+				encapsulate_send_data();
+			}
+		}
+	}
+
+	LOG(INFO) << " Communication_socket_base::encapsulate_data_thread end "<< this;
+	return;
+}
+
+
+//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_send_data()
+{
+	char buf[256] = {0};
+	static unsigned int t_heartbeat = 0;
+	sprintf(buf, "Communication_socket_base, heartbeat = %d\0\0\0", t_heartbeat);
+	t_heartbeat++;
+
+	Binary_buf* tp_buf = new Binary_buf(buf, strlen(buf)+1);//+1是为了保证发送了结束符, 方便打印
+	bool is_push = m_send_data_list.push(tp_buf);
+	if ( is_push == false )
+	{
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//封装消息, 需要子类重载
+Error_manager Communication_socket_base::encapsulate_msg(Binary_buf* p_buf)
+{
+	Binary_buf * tp_buf = new Binary_buf(*p_buf);
+	bool is_push = m_send_data_list.push(p_buf);
+	if ( is_push == false )
+	{
+		return Error_manager(Error_code::CONTAINER_IS_TERMINATE, Error_level::MINOR_ERROR,
+							 " Communication_socket_base::encapsulate_msg error ");
+	}
+	return Error_code::SUCCESS;
+}

tool/communication_socket_base.h

@@ -0,0 +1,145 @@
+
+
+/*
+ * communication_socket_base 通信模块的基类,
+ * 用户从这个基类继承, 初始化之后, 便可以自动进行通信
+ * 重载解析消息和封装消息,
+ *
+ *Thread_safe_list<Binary_buf*> 使用 Binary_buf , 而不是string
+ * 主要是为了支持直接发送数字0
+ * 
+ * 
+ * */
+
+#ifndef __COMMUNICATION_SOCKET_BASE__HH__
+#define __COMMUNICATION_SOCKET_BASE__HH__
+
+
+#include <nnxx/message>
+#include <nnxx/socket>
+
+#include <glog/logging.h>
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_list.h"
+#include "../tool/thread_condition.h"
+
+class Communication_socket_base
+{
+	//通信状态
+	enum Communication_statu
+	{
+		COMMUNICATION_UNKNOW		=0,	        //通信状态 未知
+		COMMUNICATION_READY			=1,			//通信状态 正常
+
+		COMMUNICATION_FAULT			=3,         //通信状态 错误
+	};
+
+public:
+	Communication_socket_base();
+	Communication_socket_base(const Communication_socket_base& other)= delete;
+	Communication_socket_base& operator =(const Communication_socket_base& other)= delete;
+	~Communication_socket_base();
+public://API functions
+	//初始化 通信 模块。如下三选一
+	virtual Error_manager communication_init();
+	//初始化 通信 模块。从文件读取
+//	Error_manager communication_init_from_protobuf(std::string prototxt_path);
+	//初始化 通信 模块。从protobuf读取
+//	Error_manager communication_init_from_protobuf(Laser_proto::Laser_parameter_all& laser_parameters);
+
+	//初始化
+	virtual Error_manager communication_init(std::string bind_string, std::vector<std::string>& connect_string_vector);
+	//bind
+	virtual Error_manager communication_bind(std::string bind_string);
+	//connect
+	virtual Error_manager communication_connect(std::vector<std::string>& connect_string_vector);
+	//connect
+	virtual Error_manager communication_connect(std::string connect_string);
+	//启动通信, run thread
+	virtual Error_manager communication_run();
+
+	//反初始化 通信 模块。
+	virtual Error_manager communication_uninit();
+	
+	
+public://get or set member variable
+
+protected:
+	//mp_receive_data_thread 接受线程执行函数，
+	//receive_data_thread 内部线程负责接受消息
+	void receive_data_thread();
+
+	//mp_analysis_data_thread 解析线程执行函数，
+	//analysis_data_thread 内部线程负责解析消息
+	void analysis_data_thread();
+
+	//遍历接受链表, 解析消息,
+	Error_manager analysis_receive_list();
+
+	//检查消息是否可以被解析, 需要子类重载
+	virtual Error_manager check_msg(Binary_buf* p_buf);
+
+	//处理消息, 需要子类重载
+	virtual Error_manager excute_msg(Binary_buf* p_buf);
+
+	//mp_send_data_thread 发送线程执行函数，
+	//send_data_thread 内部线程负责发送消息
+	void send_data_thread();
+
+	//mp_encapsulate_data_thread 封装线程执行函数，
+	//encapsulate_data_thread 内部线程负责封装消息
+	void encapsulate_data_thread();
+
+	//定时封装发送消息, 一般为心跳和状态信息, 需要子类重载
+	virtual Error_manager encapsulate_send_data();
+
+	//封装消息, 需要子类重载
+	virtual Error_manager encapsulate_msg(Binary_buf* p_buf);
+
+protected://member variable
+
+	//通用的网络编程接口, 默认使用总线模式, (网状结构)
+	nnxx::socket 						m_socket { nnxx::SP, nnxx::BUS };
+	std::mutex 							m_mutex;				//m_socket的锁
+
+	//通信状态
+	Communication_statu 				m_communication_statu;			//通信状态
+
+	//接受模块,
+	Thread_safe_list<Binary_buf*>		m_receive_data_list; 			//接受的list容器
+	std::thread*						mp_receive_data_thread;    		//接受的线程指针
+	Thread_condition					m_receive_condition;			//接受的条件变量
+	std::thread*						mp_analysis_data_thread;    	//解析的线程指针
+	Thread_condition					m_analysis_data_condition;		//解析的条件变量
+
+
+	//发送模块,
+	Thread_safe_list<Binary_buf*>		m_send_data_list;				//发送的list容器
+	std::thread*						mp_send_data_thread;    		//发送的线程指针
+	Thread_condition					m_send_data_condition;			//发送的条件变量
+	std::thread*						mp_encapsulate_data_thread;    	//封装的线程指针
+	Thread_condition					m_encapsulate_data_condition;	//封装的条件变量
+	
+
+
+private:
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //__COMMUNICATION_SOCKET_BASE__HH__

tool/pcl_cloud_with_lock.h

@@ -0,0 +1,30 @@
+
+
+
+#ifndef PCL_CLOUD_WITH_LOCK_H
+#define PCL_CLOUD_WITH_LOCK_H
+
+
+class Pcl_cloud_with_lock
+{
+public:
+	Pcl_cloud_with_lock();
+	Pcl_cloud_with_lock(const Pcl_cloud_with_lock& other);
+	~Pcl_cloud_with_lock();
+public://API functions
+
+public://get or set member variable
+
+
+protected://member variable
+//	//三维点云的数据保护锁，
+//	std::mutex*                     mp_task_cloud_lock;
+//	//三维点云容器的智能指针，这里不直接分配内存,
+//	pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+
+private:
+
+};
+
+
+#endif //PCL_CLOUD_WITH_LOCK_H

tool/thread_condition.cpp

@@ -30,6 +30,7 @@ Thread_condition::Thread_condition()
 	m_kill_flag = false;
 	m_pass_ever = false;
 	m_pass_once = false;
+	m_working_flag = false;
 }
 Thread_condition::~Thread_condition()
 {
@@ -44,6 +45,10 @@ bool Thread_condition::wait()
 	m_condition_variable.wait(loc,std::bind(is_pass_wait,this));
 	bool t_pass = is_pass_wait(this);
 	m_pass_once = false;
+
+	//只要前面通过了, 那就进入工作状态
+	m_working_flag = true;
+
 	return t_pass;
 }
 //等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
@@ -55,6 +60,10 @@ bool Thread_condition::wait_for_millisecond(unsigned int millisecond)
 	m_condition_variable.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(is_pass_wait, this));
 	bool t_pass = is_pass_wait(this);
 	m_pass_once = false;
+
+	//只要前面通过了, 那就进入工作状态 , 超时通过也算通过
+	m_working_flag = true;
+
 	return t_pass;
 }
 
@@ -98,6 +107,18 @@ bool Thread_condition::is_alive()
 }
 
 
+//判断是否等待, 外部线程通过这个函数来查询this线程的工作状态,
+bool Thread_condition::is_waiting()
+{
+	return !m_working_flag;
+}
+//判断是否工作, 外部线程通过这个函数来查询this线程的工作状态,
+bool Thread_condition::is_working()
+{
+	return m_working_flag;
+}
+
+
 bool Thread_condition::get_kill_flag()
 {
 	return m_kill_flag;
@@ -139,6 +160,16 @@ bool Thread_condition::is_pass_wait(Thread_condition * other)
 		throw (other);
 		return false;
 	}
-	return (other->m_kill_flag || other->m_pass_ever || other->m_pass_once);
+
+	bool result = (other->m_kill_flag || other->m_pass_ever || other->m_pass_once);
+
+	//如果不能通过等待, 那么线程状态改为等待中,
+	if ( !result )
+	{
+		other->m_working_flag = false;
+	}
+
+
+	return result;
 }
 

tool/thread_condition.h

@@ -21,6 +21,10 @@
 	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
 
  注：notify唤醒线程之后，wait里面的判断函数会重新判断。
+
+
+ 最下面有使用样例,
+
  */
 
 #ifndef LIDARMEASURE_THREAD_CONDITION_H
@@ -72,6 +76,11 @@ public:
 	//判断是否存活，只有活着才能继续支持子线程从功能函数，否则需要强制退出函数并结束子线程
 	bool is_alive();
 
+	//判断是否等待, 外部线程通过这个函数来查询this线程的工作状态,
+	bool is_waiting();
+	//判断是否工作, 外部线程通过这个函数来查询this线程的工作状态,
+	bool is_working();
+
 public:
 
 	bool get_kill_flag();
@@ -91,6 +100,8 @@ protected:
 	std::atomic<bool> 		m_pass_ever;			//线程能否直接通过等待，对后面的线程也生效。
 	std::atomic<bool> 		m_pass_once;			//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
 
+	std::atomic<bool> 		m_working_flag;			//线程是否进入工作的标志位, false:表示线程进行进入wait等待, true:表示线程仍然在运行中,
+
 	std::mutex 				m_mutex;				//线程的锁
 	std::condition_variable m_condition_variable;	//线程的条件变量
 
@@ -112,3 +123,61 @@ bool Thread_condition::wait_for_ex(const std::chrono::duration<_Rep, _Period>& t
 }
 
 #endif //LIDARMEASURE_THREAD_CONDITION_H
+
+
+
+
+
+/*
+//使用样例:
+std::thread*						mp_thread_receive;    		//接受缓存的线程指针
+Thread_condition					m_condition_receive;		//接受缓存的条件变量
+
+void thread_receive()
+{
+	while (m_condition_receive.is_alive())
+	{
+		m_condition_receive.wait();
+		if ( m_condition_receive.is_alive() )
+		{
+			//do everything
+
+		}
+	}
+
+	return;
+}
+
+//main函数的主线程
+int main(int argc,char* argv[])
+{
+ 	//线程创建之后, 默认等待
+	m_condition_receive.reset(false, false, false);
+	mp_thread_receive = new std::thread(& thread_receive );
+
+
+	//唤醒所有线程, 然后线程可以一直通过wait等待, 线程进入无限制的循环工作.
+	m_condition_receive.notify_all(true);
+
+	//暂停所有线程, 然后线程还是继续工作, 直到下一次循环, 进入wait等待
+	m_condition_receive.notify_all(false);
+
+	//如果线程单次循环运行时间较长, 需要等待线程完全停止, 才能读写公共的内存,
+	if ( m_condition_receive.is_waiting() )
+	{
+	    // 读写公共的内存,
+	}
+
+	//唤醒一个线程, 然后线程循环一次, 然后下次循环进入等待
+	m_condition_receive.notify_all(false, true);
+
+
+	//杀死线程,
+	m_condition_receive.kill_all();
+
+	//在线程join结束之后, 就可以可以回收线程内存资源
+	mp_thread_receive->join();
+	delete mp_thread_receive;
+	mp_thread_receive = NULL;
+}
+*/

tool/thread_safe_list.cpp

@@ -0,0 +1,6 @@
+
+
+
+#include "thread_safe_queue.h"
+
+

tool/thread_safe_list.h

@@ -0,0 +1,354 @@
+
+
+/*
+ * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+
+（2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
+
+通过std::move的使用（前提是我们实现的类型T定义了移动构造函数和移动赋值函数），能利用移动语义带来的性能优势。
+
+使用shared_ptr<T>返回元素，用户无需释放元素的内存。
+
+
+原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
+ 增加了一些功能函数，
+ 补充了注释说明
+
+ termination_list
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
+
+ thread_safe_queue  就是在 Thread_safe_queue 的基础上修改的,
+
+ * */
+
+#ifndef __THREAD_SAFE_LIST_H__
+#define __THREAD_SAFE_LIST_H__
+
+#include <list>
+
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+
+
+template<class T>
+class Thread_safe_list
+{
+public:
+	Thread_safe_list();
+	Thread_safe_list(const Thread_safe_list& other);
+	~Thread_safe_list();
+
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	bool wait_and_pop(T& value);
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	bool try_pop(T& value);
+	//等待并弹出数据，成功弹出则返回true
+	// 队列为空则无限等待，termination终止队列，则返回false
+	std::shared_ptr<T> wait_and_pop();
+	//尝试弹出数据，成功弹出则返回true
+	//队列为空 或者 termination终止队列，返回false
+	std::shared_ptr<T> try_pop();
+	//插入一项，并唤醒一个线程，
+	//如果成功插入，则返回true，  失败则返回false
+	//注：只能唤醒一个线程，防止多线程误判empty()
+	bool push(T new_value);
+	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+	bool clear();
+	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+	bool clear_and_delete();
+
+public:
+	//判空
+	bool empty();
+	//获取队列大小
+	size_t size();
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+	void termination_list();
+	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_list();
+	//获取退出状态
+	bool get_termination_flag();
+	//判断是否可以直接通过wait，  m_data_list不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+
+public:
+	std::mutex 						m_mutex;				//队列的锁
+	std::list<std::shared_ptr<T>> 	m_data_list;			//队列数据，使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+
+private:
+
+
+};
+
+
+
+
+
+
+
+
+template<class T>
+Thread_safe_list<T>::Thread_safe_list()
+{
+	m_termination_flag = false;
+}
+template<class T>
+Thread_safe_list<T>::Thread_safe_list(const Thread_safe_list& other)
+{
+	std::unique_lock<std::mutex> lock_this(m_mutex);
+	std::unique_lock<std::mutex> lock_other(other.m_mutex);
+	m_data_list = other.data_list;
+	m_termination_flag = other.m_termination_flag;
+}
+template<class T>
+Thread_safe_list<T>::~Thread_safe_list()
+{
+	//析构时，终止队列，让线程通过等待，方便线程推出。
+	termination_list();
+}
+
+//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+bool Thread_safe_list<T>::wait_and_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_list.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return false;
+		}
+		else
+		{
+//			value = move(*m_data_list.front());
+			value = (*m_data_list.front());
+
+			m_data_list.pop_front();
+			return true;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+bool Thread_safe_list<T>::try_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_list.empty())
+		{
+			return false;
+		}
+		else
+		{
+//			value = move(*m_data_list.front());
+			value = (*m_data_list.front());
+
+			m_data_list.pop();
+			return true;
+		}
+	}
+}
+
+
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_list<T>::wait_and_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_list.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_list.front();
+			m_data_list.pop();
+			return res;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_list<T>::try_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_list.empty())
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_list.front();
+			m_data_list.pop();
+			return res;
+		}
+	}
+}
+//插入一项，并唤醒一个线程，
+//如果成功插入，则返回true，  失败则返回false
+//注：只能唤醒一个线程，防止多线程误判empty()
+template<class T>
+bool Thread_safe_list<T>::push(T new_value)
+{
+	if (m_termination_flag)
+	{
+		return false;
+	}
+	else
+	{
+//		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
+		std::shared_ptr<T> data(std::make_shared<T>((new_value)));
+		std::unique_lock<std::mutex> lk(m_mutex);
+		m_data_list.push_back(data);
+		m_data_cond.notify_one();
+		return true;
+	}
+}
+//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
+template<class T>
+bool Thread_safe_list<T>::clear()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_list.empty())
+	{
+		m_data_list.pop_front();
+	}
+	return true;
+
+}
+
+//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
+template<class T>
+bool Thread_safe_list<T>::clear_and_delete()
+{
+
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_list.empty())
+	{
+		T res = NULL;
+//		res = move(*m_data_list.front());
+		res = (*m_data_list.front());
+
+		m_data_list.pop_front();
+		if(res != NULL)
+		{
+			delete(res);
+
+		}
+	}
+	return true;
+}
+
+
+
+//判空
+template<class T>
+bool Thread_safe_list<T>::empty()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_list.empty();
+}
+//获取队列大小
+template<class T>
+size_t Thread_safe_list<T>::size()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_list.size();
+}
+//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+template<class T>
+void Thread_safe_list<T>::termination_list()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = true;
+	m_data_cond.notify_all();
+}
+//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
+template<class T>
+void Thread_safe_list<T>::wake_list()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = false;
+	m_data_cond.notify_all();
+}
+//获取退出状态
+template<class T>
+bool Thread_safe_list<T>::get_termination_flag()
+{
+	return m_termination_flag;
+}
+//判断是否可以直接通过wait，  m_data_list不为空或者m_termination终止时都可以通过等待。
+template<class T>
+bool Thread_safe_list<T>::is_pass()
+{
+	return (!m_data_list.empty() || m_termination_flag);
+}
+
+
+
+
+
+
+#endif //__THREAD_SAFE_LIST_H__

tool/thread_safe_queue.h

@@ -270,18 +270,18 @@ bool Thread_safe_queue<T>::clear()
 template<class T>
 bool Thread_safe_queue<T>::clear_and_delete()
 {
-		std::unique_lock<std::mutex> lk(m_mutex);
-		while (!m_data_queue.empty())
+	std::unique_lock<std::mutex> lk(m_mutex);
+	while (!m_data_queue.empty())
+	{
+		T res = NULL;
+		res = move(*m_data_queue.front());
+		m_data_queue.pop();
+		if(res != NULL)
 		{
-			T res = NULL;
-			res = move(*m_data_queue.front());
-			m_data_queue.pop();
-			if(res != NULL)
-			{
-				delete(res);
+			delete(res);
 
-			}
 		}
+	}
 	return true;
 
 }