hl20200511，优化雷达模块的架构

CMakeLists.txt

@@ -22,7 +22,7 @@ include_directories(
         laser
 #        plc
         src
-#        Locate
+        Locate
 
         error_code
         tool
@@ -34,7 +34,7 @@ link_directories("/usr/local/lib")
 
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/laser LASER_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc PLC_SRC )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/Locate LOCATE_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/locate LOCATE_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/terminor TERMINOR_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
@@ -51,7 +51,7 @@ add_executable(LidarMeasure ./main.cpp ./error_code/error_code.cpp
 #        ${TERMINOR_SRC}
         ${LOCATE_SRC}
         ${TASK_MANAGER_SRC}
-#        ${TOOL_SRC}
+        ${TOOL_SRC}
         ./tool/binary_buf.cpp
         ./tool/thread_condition.h ./tool/thread_condition.cpp
         ./tool/thread_safe_queue.h ./tool/thread_safe_queue.cpp
@@ -67,9 +67,10 @@ target_link_libraries(LidarMeasure
         ${GLOG_LIBRARIES}
         ${PCL_LIBRARIES}
         ${PROTOBUF_LIBRARIES}
-        #ipopt libtensorflow_cc.so
+#        ipopt
+        libtensorflow_cc.so
         #tf_3dcnn_api.so
-        #pointSIFT_API.so
+        pointSIFT_API.so
         #dark.so
         /usr/local/lib/libglog.a
         ##/usr/local/lib/libmodbus.so

error_code/error_code.h

@@ -65,7 +65,7 @@ enum Error_code
 //    错误码的规范，
 //    错误码是int型，32位，十六进制。
 //    例如0x12345678
-//    12表示功能模块，例如:laser雷达模块               框架制定
+//    12表示功能模块，例如:laser雷达模块               	框架制定
 //    34表示文件名称，例如:laser_livox.cpp             框架制定
 //    56表示具体的类，例如:class laser_livox           个人制定
 //    78表示类的函数，例如:laser_livox::start();       个人制定
@@ -74,7 +74,6 @@ enum Error_code
 
 //    laser扫描模块
     LASER_ERROR_BASE                = 0x01000000,
-
 //    laser_base基类
 	LASER_BASE_ERROR_BASE			= 0x01010000,
     LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达任务输入参数错误
@@ -82,9 +81,9 @@ enum Error_code
 	LASER_START_FAILED,
 	LASER_CHECK_FAILED				,
 	LASER_STATUS_ERROR,								//雷达状态错误
-
-
+	//laser livox
     LASER_LIVOX_SKD_INIT_FAILED,
+	LASER_TASK_OVER_TIME,							//雷达模块，任务超时
 
 
 //    laser_livox.cpp的错误码
@@ -92,6 +91,15 @@ enum Error_code
     LIVOX_START_FAILE               = 0x01020101,
 	LIVOX_TASK_TYPE_ERROR,							//livox任务类型错误
 
+	//laser_manager 雷达管理模块
+	LASER_MANAGER_ERROR_BASE						= 0x01030000,
+	LASER_MANAGER_READ_PROTOBUF_ERROR,				//雷达管理模块，读取参数错误
+	LASER_MANAGER_IS_NOT_READY,						//雷达管理模块，不在准备状态
+	LASER_MANAGER_LASER_INDEX_ERRPR,				//雷达管理模块，雷达索引错误，编号错误。
+	LASER_MANAGER_TASK_OVER_TIME,					//雷达管理模块，任务超时
+
+
+
 
      //PLC error code  ...
     PLC_ERROR_BASE                  = 0x02010000,
@@ -215,6 +223,9 @@ enum Error_code
 
 
 
+    //task module,  error from 0x10010000-0x1001FFFF
+	TASK_MODULE_ERROR_BASE 							= 0x10010000,
+	TASK_TYPE_IS_UNKNOW,
 };
 
 //错误等级，用来做故障处理

laser/Laser.cpp

@@ -145,7 +145,7 @@ Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
 	}
 }
 //检查雷达状态,是否正常运行
-Error_manager Laser_base::check_laser()
+Error_manager Laser_base::check_status()
 {
 	if ( is_ready() )
 	{
@@ -160,15 +160,17 @@ Error_manager Laser_base::start_scan()
 	LOG(INFO) << " Laser_base::start_scan "<< this;
 	if ( is_ready() )
 	{
+		//重置数据
+		m_points_count=0;
+		m_queue_laser_data.clear_and_delete();
+		m_last_data.clear();
+
 		//启动雷达扫描
 		m_laser_scan_flag=true;				//启动雷达扫描
 		m_laser_statu = LASER_BUSY;			//雷达状态 繁忙
 		m_condition_receive.notify_all(true);		//唤醒接受线程
 		m_condition_transform.notify_all(true);	//唤醒转化线程
-		//重置数据
-		m_points_count=0;
-		m_queue_laser_data.clear_and_delete();
-		m_last_data.clear();
+
 		return Error_code::SUCCESS;
 	}
 	else
@@ -204,7 +206,7 @@ Error_manager Laser_base::end_task()
 	if(mp_laser_task !=NULL)
 	{
 		//判断任务单的错误等级，
-		if ( mp_laser_task->get_task_error_manager().get_error_level() > Error_level::MINOR_ERROR)
+		if ( mp_laser_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
 		{
 			if ( m_laser_statu == LASER_BUSY )
 			{
@@ -224,7 +226,14 @@ Error_manager Laser_base::end_task()
 	return Error_code::SUCCESS;
 }
 
-
+//取消任务单，
+Error_manager Laser_base::cancel_task()
+{
+	end_task();
+	//强制改为TASK_OVER，不管它当前在做什么。
+	mp_laser_task->set_task_statu(TASK_DEAD);
+	return Error_code::SUCCESS;
+}
 
 //设置保存文件的路径，并打开文件，
 Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
@@ -419,7 +428,7 @@ void Laser_base::thread_transform()
 							if ( t_error != Error_code::SUCCESS )
 							{
 								mp_laser_task->get_task_error_manager().compare_and_cover_error(t_error);
-								stop_scan();
+								stop_scan();  //因错误而停止。
 							}
 						}
 					}
@@ -428,8 +437,7 @@ void Laser_base::thread_transform()
 					//思科雷达停止扫描。
 					if (t_buf_type == BUF_STOP)
 					{
-						//注：这个只是思科的雷达停止处，不同的雷达在不同的地方调用 stop_scan
-						stop_scan();
+						stop_scan();//注：这个只是思科的雷达停止处，不同的雷达在不同的地方调用 stop_scan
 					}
 				}
 			}

laser/Laser.h

@@ -63,13 +63,15 @@ public:
 	//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
 	virtual Error_manager execute_task(Task_Base* p_laser_task);
 	//检查雷达状态,是否正常运行
-	virtual Error_manager check_laser();
+	virtual Error_manager check_status();
 	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
 	virtual Error_manager start_scan();
 	//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
 	virtual Error_manager stop_scan();
 	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
 	virtual Error_manager end_task();
+	//取消任务单，
+	virtual Error_manager cancel_task();
 
 public:
 	//设置保存文件的路径，并打开文件，
@@ -117,7 +119,7 @@ protected:
 protected:
 	//为了保证多线程的数据安全，修改共享数据必须加锁。  atomic 和 安全队列 可以不加锁进行读写
 	//建议：判断标志位使用 atomic， 容器要封装并使用 安全容器，
-	std::mutex          				m_laser_lock;            	//雷达数据锁
+//	std::mutex          				m_laser_lock;            	//雷达数据锁
 
 	std::atomic<int>					m_laser_id;                 //雷达设备id
 	Laser_proto::laser_parameter  		m_laser_param;				//雷达的配置参数

laser/LivoxLaser.cpp

@@ -57,13 +57,13 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 	//检查指针
 	if (p_laser_task == NULL) {
 		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-							 "Laser_base::porform_task failed, POINTER_IS_NULL");
+							 "Laser_base::execute_task failed, POINTER_IS_NULL");
 	}
 	//检查任务类型，
-	if (p_laser_task->get_task_type() != LASER_TASK)
+	if (p_laser_task->get_task_type() != LASER_BASE_SCAN_TASK)
 	{
 		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
-							 "laser task type error ！= LASER_TASK");
+							 "laser task type error ！= LASER_BASE_SCAN_TASK");
 	}
 
 	//接受任务，并将任务的状态改为TASK_SIGNED已签收
@@ -77,8 +77,8 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
 											Error_level::MINOR_ERROR,
 											"execute_task mp_task_point_cloud is null");
-		//将任务的状态改为 TASK_OVER 结束任务
-		mp_laser_task->set_task_statu(TASK_OVER);
+		//将任务的状态改为 TASK_ERROR 结束错误
+		mp_laser_task->set_task_statu(TASK_ERROR);
 		//返回错误码
 		mp_laser_task->set_task_error_manager(t_result);
 		return t_result;
@@ -115,15 +115,19 @@ Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 	//返回错误码
 	if (t_result != Error_code::SUCCESS)
 	{
+		//将任务的状态改为 TASK_ERROR 结束错误
+		mp_laser_task->set_task_statu(TASK_ERROR);
+		//返回错误码
 		mp_laser_task->set_task_error_manager(t_result);
+		return t_result;
 	}
 	return t_result;
 }
 
 //检查雷达状态,是否正常运行
-Error_manager CLivoxLaser::check_laser()
+Error_manager CLivoxLaser::check_status()
 {
-	return Laser_base::check_laser();
+	return Laser_base::check_status();
 }
 //雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
 Error_manager CLivoxLaser::start_scan()
@@ -235,6 +239,8 @@ void CLivoxLaser::InitLivox()
 			SetBroadcastCallback(OnDeviceBroadcast);
 			SetDeviceStateUpdateCallback(OnDeviceChange);
 			g_init = true;
+
+
 		}
 	}
 }
@@ -315,6 +321,27 @@ void CLivoxLaser::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
 			}
 			g_devices[handle].device_state = kDeviceStateSampling;
 		}
+
+		CLivoxLaser* tp_livox = g_all_laser[handle];
+		if (tp_livox == NULL)
+		{
+			if (g_handle_sn.find(handle) != g_handle_sn.end())
+			{
+				std::string sn = g_handle_sn[handle];
+				if (g_sn_laser.find(sn) != g_sn_laser.end())
+				{
+					tp_livox = g_sn_laser[sn];
+					g_all_laser[handle] = tp_livox;
+					if (tp_livox)
+						tp_livox->UpdataHandle();
+				}
+			}
+		}
+
+		//获取欧拉角
+		uint8_t temp =  LidarGetExtrinsicParameter(handle, lidar_get_extrinsic_parameter_callback, NULL);
+//		uint8_t temp =  LidarSetExtrinsicParameter( handle,	&(tp_livox->m_set_extrinsic_parameter), common_command_callback, NULL);
+
 	}
 }
 void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
@@ -346,6 +373,8 @@ void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
 		else
 			g_sn_handle.insert(std::make_pair(sn,handle));
 
+
+
 	}
 
 }
@@ -420,3 +449,73 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 		std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
 	}
 }
+
+//获取欧拉角。使用 LidarGetExtrinsicParameter 来设置回调函数。
+void CLivoxLaser::lidar_get_extrinsic_parameter_callback(uint8_t status,
+											uint8_t handle,
+											LidarGetExtrinsicParameterResponse *response,
+											void *client_data)
+{
+	std::cout << "-----------------lidar_get_extrinsic_parameter_callback-------------------" << std::endl;
+
+	//m_extrinsic_parameter;		//欧拉角，(roll, pitch, yaw, x, y, z)
+	std::cout << "roll = " << response->roll << std::endl;
+	std::cout << "pitch = " << response->pitch << std::endl;
+	std::cout << "yaw = " << response->yaw << std::endl;
+
+	std::cout << "x = " << response->x << std::endl;
+	std::cout << "y = " << response->y << std::endl;
+	std::cout << "z = " << response->z << std::endl;
+
+	CLivoxLaser* tp_livox = g_all_laser[handle];
+	if (tp_livox == NULL)
+	{
+		if (g_handle_sn.find(handle) != g_handle_sn.end())
+		{
+			std::string sn = g_handle_sn[handle];
+			if (g_sn_laser.find(sn) != g_sn_laser.end())
+			{
+				tp_livox = g_sn_laser[sn];
+				g_all_laser[handle] = tp_livox;
+				if (tp_livox)
+					tp_livox->UpdataHandle();
+			}
+		}
+	}
+	tp_livox->m_get_extrinsic_parameter = *response;
+
+	double t_pitch = tp_livox->m_get_extrinsic_parameter.pitch;
+	double t_roll = tp_livox->m_get_extrinsic_parameter.roll;
+	double t_yaw = tp_livox->m_get_extrinsic_parameter.yaw;
+	double t_x = tp_livox->m_get_extrinsic_parameter.x;
+	double t_y = tp_livox->m_get_extrinsic_parameter.y;
+	double t_z = tp_livox->m_get_extrinsic_parameter.z;
+
+	//欧拉角转化为3*4矩阵
+	t_roll = t_roll * M_PI / 180.0;
+	t_pitch = t_pitch * M_PI / 180.0;
+	t_yaw = t_yaw * M_PI / 180.0;
+	double rotate[12] = {
+	std::cos(t_pitch) * std::cos(t_yaw),
+	std::sin(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) - std::cos(t_roll) * std::sin(t_yaw),
+	std::cos(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) + std::sin(t_roll) * std::sin(t_yaw),
+	t_x,
+	std::cos(t_pitch) * std::sin(t_yaw),
+	std::sin(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) + std::cos(t_roll) * std::cos(t_yaw),
+	std::cos(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) - std::sin(t_roll) * std::cos(t_yaw),
+	t_y,
+	-std::sin(t_pitch),
+	std::sin(t_roll) * std::cos(t_pitch),
+	std::cos(t_roll) * std::cos(t_pitch),
+	t_z
+	};
+
+	tp_livox->set_laser_matrix(rotate, 12);
+
+}
+
+void CLivoxLaser::common_command_callback(uint8_t status, uint8_t handle, uint8_t response, void *client_data)
+{
+	std::cout << "-----------------common_command_callback-------------------" << std::endl;
+
+}

laser/LivoxLaser.h

@@ -45,7 +45,7 @@ public:
 	//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
 	virtual Error_manager execute_task(Task_Base* p_laser_task);
 	//检查雷达状态,是否正常运行
-	virtual Error_manager check_laser();
+	virtual Error_manager check_status();
 	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
 	virtual Error_manager start_scan();
 	//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
@@ -76,6 +76,14 @@ protected:
 	static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
 	static void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data);
 
+	//获取欧拉角。使用 LidarGetExtrinsicParameter 来设置回调函数。
+	static void lidar_get_extrinsic_parameter_callback(uint8_t status,
+											   uint8_t handle,
+											   LidarGetExtrinsicParameterResponse *response,
+											   void *client_data);
+
+	static void common_command_callback(uint8_t status, uint8_t handle, uint8_t response, void *client_data);
+
 protected:
 	uint8_t									m_handle;
 	unsigned int							m_frame_maxnum;
@@ -90,6 +98,8 @@ protected:
 	static unsigned int						g_count[kMaxLidarCount];
 
 
+	LidarGetExtrinsicParameterResponse 		m_get_extrinsic_parameter;		//欧拉角，(roll, pitch, yaw, x, y, z)
+	LidarSetExtrinsicParameterRequest		m_set_extrinsic_parameter;		//欧拉角，(roll, pitch, yaw, x, y, z)
 };
 
 #endif

laser/LivoxMid100Laser.cpp

@@ -79,10 +79,10 @@ Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
 							 "Laser_base::porform_task failed, POINTER_IS_NULL");
 	}
 	//检查任务类型，
-	if (p_laser_task->get_task_type() != LASER_TASK)
+	if (p_laser_task->get_task_type() != LASER_BASE_SCAN_TASK)
 	{
 		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
-							 "laser task type error ！= LASER_TASK");
+							 "laser task type error ！= LASER_BASE_SCAN_TASK");
 	}
 
 	//接受任务，并将任务的状态改为TASK_SIGNED已签收
@@ -142,9 +142,9 @@ Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
 }
 
 //检查雷达状态,是否正常运行
-Error_manager CLivoxMid100Laser::check_laser()
+Error_manager CLivoxMid100Laser::check_status()
 {
-	return CLivoxLaser::check_laser();
+	return CLivoxLaser::check_status();
 }
 
 //雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。

laser/LivoxMid100Laser.h

@@ -19,7 +19,7 @@ public:
 	//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
 	virtual Error_manager execute_task(Task_Base* p_laser_task);
 	//检查雷达状态,是否正常运行
-	virtual Error_manager check_laser();
+	virtual Error_manager check_status();
 	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
 	virtual Error_manager start_scan();
 	//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。

laser/laser_manager.cpp

@@ -0,0 +1,406 @@
+
+
+
+#include "./laser_manager.h"
+
+#include "../tool/proto_tool.h"
+#include "../laser/laser_parameter.pb.h"
+
+#include <livox_sdk.h>
+
+Laser_manager::Laser_manager()
+{
+	m_laser_manager_status = LASER_MANAGER_UNKNOW;
+	mp_laser_manager_thread = NULL;
+
+	mp_laser_manager_task = NULL;
+	m_laser_number = 0;
+}
+Laser_manager::~Laser_manager()
+{
+	laser_manager_uninit();
+}
+
+//初始化 雷达 管理模块。如下三选一
+Error_manager Laser_manager::laser_manager_init()
+{
+	return  laser_manager_init_from_protobuf(LASER_PARAMETER_ALL_PATH);
+}
+//初始化 雷达 管理模块。从文件读取
+Error_manager Laser_manager::laser_manager_init_from_protobuf(std::string prototxt_path)
+{
+	Laser_proto::Laser_parameter_all laser_parameters;
+
+	if(!  proto_tool::read_proto_param(prototxt_path,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);
+}
+//初始化 雷达 管理模块。从protobuf读取
+Error_manager Laser_manager::laser_manager_init_from_protobuf(Laser_proto::Laser_parameter_all& laser_parameters)
+{
+	m_laser_manager_status = LASER_MANAGER_READY;
+	m_laser_number = laser_parameters.laser_parameters_size();
+	m_laser_manager_working_flag = false;
+
+	//创建大疆雷达
+	int laser_cout=laser_parameters.laser_parameters_size();
+	m_laser_vector.resize(laser_cout);
+	for(int i=0;i<laser_parameters.laser_parameters_size();++i)
+	{
+		m_laser_vector[i]=LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(),i,
+													  laser_parameters.laser_parameters(i));
+		if(m_laser_vector[i]!=NULL)
+		{
+			if(m_laser_vector[i]->connect_laser()!=SUCCESS)
+			{
+				char description[255]={0};
+				sprintf(description,"Laser %d connect failed...",i);
+				return Error_manager(LASER_CONNECT_FAILED,NORMAL,description);
+			}
+		}
+	}
+	//查询是否有livox雷达,初始化库
+	for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
+	{
+		std::string type = laser_parameters.laser_parameters(i).type();
+		if (type.find("Livox") != type.npos)
+		{
+			if (Start() == false)
+			{
+				Uninit();
+				return Error_manager(LASER_LIVOX_SKD_INIT_FAILED,NORMAL,"Livox laser init failed...");
+			}
+			break;
+		}
+	}
+
+	//启动雷达管理模块的内部线程。默认wait。
+	m_laser_manager_condition.reset(false, false, false);
+	mp_laser_manager_thread = new std::thread(&Laser_manager::thread_work, this);
+
+	return Error_code::SUCCESS;
+}
+//反初始化 雷达 管理模块。
+Error_manager Laser_manager::laser_manager_uninit()
+{
+
+	//关闭线程
+	if (mp_laser_manager_thread)
+	{
+		m_laser_manager_condition.kill_all();
+	}
+	//回收线程的资源
+	if (mp_laser_manager_thread)
+	{
+		mp_laser_manager_thread->join();
+		delete mp_laser_manager_thread;
+		mp_laser_manager_thread = NULL;
+	}
+
+	//回收雷达的内存
+	for (int i = 0; i < m_laser_number; ++i)
+	{
+		delete(m_laser_vector[i]);
+	}
+	//回收下发雷达任务单
+	for (map<int, Laser_task*>::iterator iter  = m_laser_task_map.begin(); iter != m_laser_task_map.end(); ++iter)
+	{
+		delete(iter->second);
+		m_laser_task_map.erase(iter);
+	}
+}
+
+
+
+//对外的接口函数，负责接受并处理任务单，
+//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+Error_manager Laser_manager::execute_task(Task_Base* p_laser_task)
+{
+	LOG(INFO) << " Laser_manager::execute_task start  "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//检查指针
+	if (p_laser_task == NULL) {
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Laser_manager::execute_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型，
+	if (p_laser_task->get_task_type() != LASER_MANGER_SCAN_TASK)
+	{
+		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 "laser task type error ！= LASER_MANGER_SCAN_TASK ");
+	}
+
+	//接受任务，并将任务的状态改为TASK_SIGNED已签收
+	mp_laser_manager_task = (Laser_manager_task *) p_laser_task;
+	mp_laser_manager_task->set_task_statu(TASK_SIGNED);
+
+	//检查消息内容是否正确，
+	//检查三维点云指针
+	if (mp_laser_manager_task->get_task_point_cloud().get() == NULL)
+	{
+		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
+									 Error_level::MINOR_ERROR,
+									 "execute_task mp_task_point_cloud is null");
+		//将任务的状态改为 TASK_ERROR 结束错误
+		mp_laser_manager_task->set_task_statu(TASK_ERROR);
+		//返回错误码
+		mp_laser_manager_task->set_task_error_manager(t_result);
+		return t_result;
+	}
+	else
+	{
+		if ( m_laser_manager_status != LASER_MANAGER_READY )
+		{
+			t_result.error_manager_reset(Error_code::LASER_MANAGER_IS_NOT_READY,
+										 Error_level::MINOR_ERROR,
+										 "execute_task m_laser_manager_status != LASER_MANAGER_READY");
+			//将任务的状态改为 TASK_ERROR 结束错误
+			mp_laser_manager_task->set_task_statu(TASK_ERROR);
+			//返回错误码
+			mp_laser_manager_task->set_task_error_manager(t_result);
+			return t_result;
+		}
+		else
+		{
+			//启动雷达管理模块，的核心工作线程
+			m_laser_manager_status = LASER_MANAGER_ISSUED_TASK;
+			m_laser_manager_working_flag = true;
+			m_laser_manager_condition.notify_all(true);
+			//通知 thread_work 子线程启动。
+
+			//将任务的状态改为 TASK_WORKING 处理中
+			mp_laser_manager_task->set_task_statu(TASK_WORKING);
+
+		}
+
+	}
+	//返回错误码
+	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 t_result;
+	}
+	return t_result;
+
+}
+
+//检查雷达状态,是否正常运行
+Error_manager Laser_manager::check_status()
+{
+
+}
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager Laser_manager::end_task()
+{
+	LOG(INFO) << " Laser_manager::end_task "<< this;
+
+	m_laser_manager_working_flag=false;
+	m_laser_manager_condition.notify_all(false);
+
+	laser_task_map_clear_and_delete();
+
+	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
+	if(mp_laser_manager_task !=NULL)
+	{
+		//判断任务单的错误等级，
+		if ( mp_laser_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
+		{
+			if ( m_laser_manager_status == LASER_MANAGER_ISSUED_TASK  || m_laser_manager_status == LASER_MANAGER_WAIT_REPLY)
+			{
+				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR，则恢复待机，可以接受下一次的任务单
+				m_laser_manager_status = LASER_MANAGER_READY;
+			}
+
+		}
+		else
+		{
+			//故障等级为 MAJOR_ERROR CRITICAL_ERROR，则雷达故障，不可以再接受任务单。
+			m_laser_manager_status = LASER_MANAGER_FAULT;
+		}
+		//强制改为TASK_OVER，不管它当前在做什么。
+		mp_laser_manager_task->set_task_statu(TASK_OVER);
+	}
+	std::cout << "------Laser_manager::end_task()------."<<mp_laser_manager_task->get_task_error_manager().to_string() << std::endl;
+	return Error_code::SUCCESS;
+}
+
+//释放下发的任务单
+Error_manager Laser_manager::laser_task_map_clear_and_delete()
+{
+	for ( map<int, Laser_task*>::iterator map_iter2 = m_laser_task_map.begin(); map_iter2 != m_laser_task_map.end(); )
+	{
+		//销毁下发的任务单。
+		delete(map_iter2->second);
+		map_iter2 = m_laser_task_map.erase(map_iter2);
+		//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
+		//for循环不用 ++map_iter2
+	}
+	return Error_code::SUCCESS;
+}
+
+//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+Error_manager Laser_manager::issued_task(int laser_index)
+{
+	Error_manager t_error;
+
+	if ( laser_index < 0 || laser_index >= m_laser_number )
+	{
+	    return Error_manager(Error_code::LASER_MANAGER_LASER_INDEX_ERRPR, Error_level::MINOR_ERROR,
+	    					" issued_task laser_index  error ");
+	}
+
+	//创建雷达扫描任务,
+	// 这里为下发雷达任务分配内存，后续要记得回收。
+	Laser_task* t_laser_task=new Laser_task();
+	mp_laser_manager_task->trans_to_laser_task(t_laser_task,laser_index,m_laser_vector[laser_index],
+	TASK_CREATED,std::chrono::milliseconds(7000));
+
+	//保存雷达扫描任务 到 m_laser_task_map
+	map<int, Laser_task*>::iterator map_iter = m_laser_task_map.find(laser_index);
+	if ( map_iter != m_laser_task_map.end() )
+	{
+		//如果重复，则清除原有的，用新的覆盖。
+		delete(map_iter->second);
+		m_laser_task_map[laser_index] = t_laser_task;
+	}
+	else
+	{
+		m_laser_task_map[laser_index] = t_laser_task;
+	}
+
+	//发送任务单给雷达
+//	t_error=m_laser_vector[laser_index]->execute_task(t_laser_task);
+	task_command_manager::get_instance_references().execute_task(t_laser_task);
+
+	if(t_error!=SUCCESS)
+	{
+		;
+		//这里的故障处理很复杂，以后再写。
+		//部分成功，部分失败。
+	}
+
+	return t_error;
+
+}
+
+//mp_laser_manager_thread 线程执行函数，
+//thread_work 内部线程负责分发扫描任务给下面的雷达，并且回收汇总雷达的数据
+void Laser_manager::thread_work()
+{
+	LOG(INFO) << " mp_laser_manager_thread start "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//雷达管理的独立线程，负责控制管理所有的雷达。
+	while (m_laser_manager_condition.is_alive())
+	{
+		m_laser_manager_condition.wait();
+		if ( m_laser_manager_condition.is_alive() )
+		{
+			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)
+			{
+				//分发扫描任务给下面的雷达
+				if ( mp_laser_manager_task->get_select_all_laser_flag() )
+				{
+					//下发任务给所有的雷达
+					for (int i = 0; i < m_laser_number; ++i)
+					{
+						//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+						t_error = issued_task(i);
+						if ( t_error != SUCCESS)
+						{
+							t_result.compare_and_cover_error(t_error);
+						}
+					}
+				}
+				else
+				{
+					//下发任务给选中的雷达
+					std::vector<int> & t_select_laser_id_vector = mp_laser_manager_task->get_select_laser_id_vector();
+					for (int i = 0; i < t_select_laser_id_vector.size(); ++i)
+					{
+						//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+						t_error = issued_task(t_select_laser_id_vector[i]);
+						if ( t_error != SUCCESS)
+						{
+							t_result.compare_and_cover_error(t_error);
+						}
+					}
+				}
+
+				//下发任务之后，修改状态为等待答复。
+				m_laser_manager_status = LASER_MANAGER_WAIT_REPLY;
+			}
+			//等待答复
+			else if(m_laser_manager_status == LASER_MANAGER_WAIT_REPLY)
+			{
+				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->get_task_statu() == TASK_OVER
+						 || map_iter->second->get_task_statu() == TASK_ERROR
+						 || map_iter->second->get_task_statu() == TASK_DEAD) )
+					{
+						//等待任务继续完成，等1ms
+						std::this_thread::sleep_for(std::chrono::milliseconds(1));
+						break;
+					}
+				}
+
+				//如果下发的任务全部结束，那么汇总所有的结果, 并销毁下发的任务单。
+				if(map_iter == m_laser_task_map.end() )
+				{
+					for ( map<int, Laser_task*>::iterator map_iter2 = m_laser_task_map.begin(); map_iter2 != m_laser_task_map.end(); )
+					{
+						//数据汇总，
+						//由于 pcl::PointCloud<pcl::PointXYZ>::Ptr mp_task_point_cloud; 是所有任务单共用的，所以不用处理了。自动就在一起。
+
+						//错误汇总
+						t_result.compare_and_cover_error(map_iter2->second->get_task_error_manager());
+
+						//销毁下发的任务单。
+						delete(map_iter2->second);
+						map_iter2 = m_laser_task_map.erase(map_iter2);
+						//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
+						//for循环不用 ++map_iter2
+					}
+
+					mp_laser_manager_task->set_task_error_manager(t_result);
+					//结束任务
+					end_task();
+				}
+			}
+
+
+		}
+	}
+	LOG(INFO) << " mp_laser_manager_thread end :"<<this;
+	return;
+}
+
+
+

laser/laser_manager.h

@@ -0,0 +1,116 @@
+
+/*
+ * Laser_manager 是雷达模块的总管理类。
+ * Laser_manager 作为一个单例，全局可以使用。 get_instance 系列的函数可获取唯一的实例。
+ * 它作为一个独立的模块，不和外部有直接的联系。
+ * 对外只有一个接口函数 execute_task 用以执行 任务单 Laser_manager_task
+ *
+ * Laser_manager模块内部包含了所有的雷达指针。
+ * 还自带一个线程，用来控制雷达。
+ *
+ *
+ * */
+
+
+
+#ifndef LASER_MANAGER_H
+#define LASER_MANAGER_H
+
+#include "../error_code/error_code.h"
+#include "../tool/singleton.h"
+#include "../tool/thread_condition.h"
+
+#include "../laser/Laser.h"
+#include "../laser/laser_manager_task.h"
+
+
+#define LASER_PARAMETER_ALL_PATH "../setting/laser.prototxt"
+
+class Laser_manager:public Singleton<Laser_manager>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<Laser_manager>;
+
+public:
+	//雷达管理模块的工作状态
+	enum Laser_manager_status
+	{//default LASER_MANAGER_UNKNOW = 0
+	    LASER_MANAGER_UNKNOW               	= 0,    //未知
+	    LASER_MANAGER_READY               	= 1,    //准备，待机
+		LASER_MANAGER_ISSUED_TASK			= 2,	//工作下发任务
+		LASER_MANAGER_WAIT_REPLY			= 3,	//工作等待答复
+		LASER_MANAGER_FAULT					= 4,	//故障
+	};
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	Laser_manager(const Laser_manager&)=delete;
+	Laser_manager& operator =(const Laser_manager&)= delete;
+	~Laser_manager();
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	Laser_manager();
+
+
+public: //API 对外接口
+	//初始化 雷达 管理模块。如下三选一
+	Error_manager laser_manager_init();
+	//初始化 雷达 管理模块。从文件读取
+	Error_manager laser_manager_init_from_protobuf(std::string prototxt_path);
+	//初始化 雷达 管理模块。从protobuf读取
+	Error_manager laser_manager_init_from_protobuf(Laser_proto::Laser_parameter_all& laser_parameters);
+	//反初始化 雷达 管理模块。
+	Error_manager laser_manager_uninit();
+
+
+	//对外的接口函数，负责接受并处理任务单，
+	//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+	Error_manager execute_task(Task_Base* p_laser_task);
+	//检查雷达状态,是否正常运行
+	Error_manager check_status();
+	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+	Error_manager end_task();
+
+	//释放下发的任务单
+	Error_manager laser_task_map_clear_and_delete();
+
+public://get or set member variable
+
+
+protected:
+	//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+	Error_manager issued_task(int laser_index);
+
+	//mp_laser_manager_thread 线程执行函数，
+	//thread_work 内部线程负责分发扫描任务给下面的雷达，并且回收汇总雷达的数据
+	void thread_work();
+
+
+
+
+
+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来销毁
+
+	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;		//发布信息的条件变量
+
+
+	//雷达管理的任务单指针，实际内存由发送方控制管理，//接受任务后，指向新的任务单
+	Laser_manager_task *  						mp_laser_manager_task;        		//雷达管理任务单的指针，内存由发送方管理。
+
+private:
+
+};
+
+
+
+
+#endif //LASER_MANAGER_H

laser/laser_manager_task.cpp

@@ -0,0 +1,256 @@
+
+/*
+ * Laser_manager_task 是雷达管理的任务单。
+ * 外部通过它对 Laser_manager 下发任务。
+ *
+//用法：上级直接调用 Laser_manager 的接口函数 execute_task ，并将 Laser_manager_task 类作为参数传递
+// Laser_manager 类则按照 Laser_manager_task 的功能码和指定的参数，执行相应的功能
+//并将结果填充到 Laser_manager_task ，返回给上级。
+ *
+ *
+ * */
+
+#include "laser_manager_task.h"
+
+
+//构造函数，构造函数锁定任务类型为 LASER_MANGER_SCAN_TASK ，后续不允许更改
+Laser_manager_task::Laser_manager_task()
+{
+//构造函数锁定任务类型为 LASER_MANGER_SCAN_TASK，后续不允许更改
+	m_task_type = LASER_MANGER_SCAN_TASK;
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_frame_maxnum = 0;
+	m_task_save_flag = false;//默认不保存，false
+	m_task_save_path.clear();
+	mp_task_point_cloud = NULL;
+	mp_task_cloud_lock=NULL;
+
+	m_select_all_laser_flag = true;
+	m_select_laser_id_vector.clear();
+}
+
+//析构函数
+Laser_manager_task::~Laser_manager_task()
+{
+
+}
+
+//初始化任务单，必须初始化之后才可以使用，（必选参数）
+// input:p_task_cloud_lock 三维点云的数据保护锁
+// 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)
+{
+	if(p_task_point_cloud.get() == 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_frame_maxnum = 0;
+	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;
+
+	m_select_all_laser_flag = true;
+	m_select_laser_id_vector.clear();
+
+	return Error_code::SUCCESS;
+}
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_frame_maxnum 点云的采集帧数最大值
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_point_cloud 三维点云容器的智能指针
+//    input:m_is_select_all_laser 是否选取所有的雷达
+//    input:select_laser_number_vector 选取的指定雷达id
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+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,
+											bool select_all_laser_flag,
+											std::vector<int> select_laser_id_vector
+)
+{
+	if(p_task_point_cloud.get() == 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_frame_maxnum = task_frame_maxnum;
+	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;
+
+	m_select_all_laser_flag = select_all_laser_flag;
+	m_select_laser_id_vector = select_laser_id_vector;
+
+	return Error_code::SUCCESS;
+}
+
+
+
+//push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
+Error_manager Laser_manager_task::task_push_point(pcl::PointXYZ point_xyz)
+{
+	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)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Laser_task::task_push_point p_task_point_cloud is null");
+	}
+	//加锁，并添加三维点。
+	mp_task_cloud_lock->lock();
+	mp_task_point_cloud->push_back(point_xyz);
+	mp_task_cloud_lock->unlock();
+
+	return SUCCESS;
+}
+
+//把 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
+	);
+	return Error_code::SUCCESS;
+}
+
+
+
+//获取 点云的采集帧数最大值
+unsigned int Laser_manager_task::get_task_frame_maxnum()
+{
+	return m_task_frame_maxnum;
+}
+//获取采集的点云保存文件的使能标志位
+bool Laser_manager_task::get_task_save_flag()
+{
+	return m_task_save_flag;
+}
+//获取采集的点云保存路径
+std::string Laser_manager_task::get_task_save_path()
+{
+	return m_task_save_path;
+}
+//获取 三维点云容器的智能指针
+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()
+{
+	return mp_task_point_cloud;
+}
+
+
+
+//设置 点云的采集帧数最大值
+void Laser_manager_task::set_task_frame_maxnum(unsigned int task_frame_maxnum)
+{
+	m_task_frame_maxnum = task_frame_maxnum;
+
+}
+//设置采集的点云保存文件的使能标志位
+void Laser_manager_task::set_task_save_flag(bool task_save_flag)
+{
+	m_task_save_flag=task_save_flag;
+}
+//设置采集的点云保存路径
+void Laser_manager_task::set_task_save_path(std::string task_save_path)
+{
+	m_task_save_path=task_save_path;
+}
+//设置采集的点云保存标志位和路径
+void Laser_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;
+}
+
+//获取 是否选取所有的雷达
+bool Laser_manager_task::get_select_all_laser_flag()
+{
+	return m_select_all_laser_flag;
+}
+//设置 是否选取所有的雷达
+void Laser_manager_task::set_select_all_laser_flag( bool select_all_laser_flag)
+{
+	m_select_all_laser_flag = select_all_laser_flag;
+}
+//获取 被选中的雷达编号。
+std::vector<int>& Laser_manager_task::get_select_laser_id_vector()
+{
+	return m_select_laser_id_vector;
+}
+//设置 被选中的雷达编号。
+void Laser_manager_task::set_select_laser_id_vector(std::vector<int>& select_laser_id_vector)
+{
+	m_select_laser_id_vector = select_laser_id_vector;
+}
+
+
+
+
+
+
+

laser/laser_manager_task.h

@@ -0,0 +1,155 @@
+
+/*
+ * Laser_manager_task 是雷达管理的任务单。
+ * 外部通过它对 Laser_manager 下发任务。
+ *
+//用法：上级直接调用 Laser_manager 的接口函数 execute_task ，并将 Laser_manager_task 类作为参数传递
+// Laser_manager 类则按照 Laser_manager_task 的功能码和指定的参数，执行相应的功能
+//并将结果填充到 Laser_manager_task ，返回给上级。
+ *
+ *
+ * */
+
+
+
+
+#ifndef LASER_MANAGER_TASK_H
+#define LASER_MANAGER_TASK_H
+
+#include "Point2D.h"
+#include "Point3D.h"
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include "../error_code/error_code.h"
+
+#include <vector>
+#include <mutex>
+
+#include "../task/task_command_manager.h"
+#include "../laser/laser_task_command.h"
+#include "../laser/Laser.h"
+
+
+class Laser_manager_task:public Task_Base
+{
+public://API functions
+	//构造函数，构造函数锁定任务类型为 LASER_MANGER_SCAN_TASK ，后续不允许更改
+	Laser_manager_task();
+	Laser_manager_task(const Laser_manager_task& other) = delete;
+	~Laser_manager_task();
+
+	//初始化任务单，必须初始化之后才可以使用，（必选参数）
+	// input:p_task_cloud_lock 三维点云的数据保护锁
+	// 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);
+
+	//初始化任务单，必须初始化之后才可以使用，（可选参数）
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	//    input:task_frame_maxnum 点云的采集帧数最大值
+	//    input:task_save_flag 是否保存
+	//    input:task_save_path 保存路径
+	//    input:p_task_cloud_lock 三维点云的数据保护锁
+	//    output:p_task_point_cloud 三维点云容器的智能指针
+	//    input:m_is_select_all_laser 是否选取所有的雷达
+	//    input:select_laser_number_vector 选取的指定雷达id
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager 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,
+							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 超时时间，一般要比上级任务的时间短一些
+	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,
+									  std::chrono::milliseconds task_over_time = std::chrono::milliseconds(TASK_OVER_TIME_DEFAULT));
+
+public://get or set member variable
+	//获取 点云的采集帧数最大值
+	unsigned int get_task_frame_maxnum();
+	//获取采集的点云保存文件的使能标志位
+	bool get_task_save_flag();
+	//获取采集的点云保存路径
+	std::string get_task_save_path();
+	//获取 三维点云容器的智能指针
+	std::mutex*  get_task_cloud_lock();
+	//获取 三维点云容器的智能指针
+	pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
+
+
+
+	//设置 点云的采集帧数最大值
+	void set_task_frame_maxnum(unsigned int task_frame_maxnum);
+	//设置采集的点云保存文件的使能标志位
+	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);
+
+
+
+	//获取 是否选取所有的雷达
+	bool get_select_all_laser_flag();
+	//设置 是否选取所有的雷达
+	void set_select_all_laser_flag( bool select_all_laser_flag);
+	//获取 被选中的雷达编号。
+	std::vector<int>& get_select_laser_id_vector();
+	//设置 被选中的雷达编号。
+	void set_select_laser_id_vector(std::vector<int>& select_laser_id_vector);
+
+protected://member variable
+	//点云的采集帧数最大值，任务输入
+	unsigned int                    m_task_frame_maxnum;
+
+	//雷达保存文件的使能标志位，//默认不保存，false
+	bool		 					m_task_save_flag;
+	//点云保存文件的路径，任务输入
+	std::string                     m_task_save_path;
+
+	//三维点云的数据保护锁，任务输入
+	std::mutex*                     mp_task_cloud_lock;
+	//采集结果，三维点云容器的智能指针，任务输出
+	//这里只是智能指针，实际内存由 任务发送方管理，初始化时必须保证内存有效。
+	pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+
+
+	bool 							m_select_all_laser_flag;					//是否选取所有的雷达。默认为true
+	std::vector<int>				m_select_laser_id_vector;				//被选中的雷达编号。
+	//注：当 m_is_select_all_laser 为true时，m_select_laser_number_vector不用写。
+	//注：当 m_is_select_all_laser 为false时，m_select_laser_number_vector 必须填写，不能为空。否则报错。
+
+
+
+private:
+
+};
+
+
+
+
+
+
+
+
+
+#endif //LASER_MANAGER_TASK_H

laser/laser_task_command.cpp

@@ -10,8 +10,8 @@
 //构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
 Laser_task::Laser_task()
 {
-    //构造函数锁定任务类型为LASER_TASK，后续不允许更改
-    m_task_type = LASER_TASK;
+    //构造函数锁定任务类型为 LASER_BASE_SCAN_TASK，后续不允许更改
+    m_task_type = LASER_BASE_SCAN_TASK;
     m_task_statu = TASK_CREATED;
     m_task_statu_information.clear();
 	m_task_error_manager.error_manager_clear_all();
@@ -29,80 +29,90 @@ Laser_task::~Laser_task()
 }
 
 //初始化任务单，必须初始化之后才可以使用，（必选参数）
-// input:task_statu 任务状态
-// output:p_task_point_cloud 三维点云容器的智能指针
 // input:p_task_cloud_lock 三维点云的数据保护锁
+// output:p_task_point_cloud 三维点云容器的智能指针
 //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-Error_manager Laser_task::task_init(Task_statu task_statu,
-                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                                    std::mutex* p_task_cloud_lock)
-{
-    if(p_task_point_cloud.get() == NULL)
-    {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::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 task input lock is null");
-    }
-
-    m_task_statu = task_statu;
-    m_task_statu_information.clear();
+Error_manager Laser_task::task_init(std::mutex* p_task_cloud_lock,
+											pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
+{
+	if(p_task_point_cloud.get() == 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_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
+	m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
 	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_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
 
-    return Error_code::SUCCESS;
+	return Error_code::SUCCESS;
 }
 
 //初始化任务单，必须初始化之后才可以使用，（可选参数）
-//    input:task_statu任务状态
-//    input:task_statu_information状态说明
-//    input:task_frame_maxnum点云的采集帧数最大值
-//    output:p_task_point_cloud三维点云容器的智能指针
-// input:p_task_cloud_lock 三维点云的数据保护锁
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_frame_maxnum 点云的采集帧数最大值
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_point_cloud 三维点云容器的智能指针
 //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
 Error_manager Laser_task::task_init(Task_statu task_statu,
-                                    std::string & task_statu_information,
-                                    unsigned int task_frame_maxnum,
-                                    pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                                    std::mutex* p_task_cloud_lock,
-                                    bool task_save_flag ,
-									std::string task_save_path )
-{
-    if(p_task_point_cloud.get() == NULL)
-    {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::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 task input lock is null");
-    }
+											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
+)
+{
+	if(p_task_point_cloud.get() == 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_statu = task_statu;
-    m_task_statu_information = task_statu_information;
+	if ( task_frame_maxnum == 0 )
+	{
+		m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
+	}
+	else
+	{
+		m_task_frame_maxnum = task_frame_maxnum;
+	}
 
-    if(task_frame_maxnum == 0)
-    {
-        m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
-    }
-    else
-    {
-        m_task_frame_maxnum = task_frame_maxnum;
-    }
 	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;
-    m_task_error_manager.error_manager_clear_all();
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
 
-    return Error_code::SUCCESS;
+	return Error_code::SUCCESS;
 }
 
 //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
@@ -144,6 +154,11 @@ std::string Laser_task::get_task_save_path()
     return m_task_save_path;
 }
 //获取 三维点云容器的智能指针
+std::mutex*  Laser_task::get_task_cloud_lock()
+{
+	return mp_task_cloud_lock;
+}
+//获取 三维点云容器的智能指针
 pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_task::get_task_point_cloud()
 {
     return mp_task_point_cloud;
@@ -152,16 +167,6 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_task::get_task_point_cloud()
 
 
 
-//设置 任务状态
-void Laser_task::set_task_statu(Task_statu task_statu)
-{
-    m_task_statu = task_statu;
-}
-//设置 任务状态说明
-void Laser_task::set_task_statu_information(std::string & task_statu_information)
-{
-    m_task_statu_information = task_statu_information;
-}
 //设置 点云的采集帧数最大值
 void Laser_task::set_task_frame_maxnum(unsigned int task_frame_maxnum)
 {
@@ -184,18 +189,6 @@ void Laser_task::set_task_save_flag_and_path(bool task_save_flag, std::string ta
 	m_task_save_flag=task_save_flag;
 	m_task_save_path=task_save_path;
 }
-//设置 三维点云容器的智能指针
-void Laser_task::set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
-{
-    mp_task_point_cloud = p_task_point_cloud;
-}
-//设置 错误码
-void Laser_task::set_task_error_manager(Error_manager & error_manager)
-{
-    m_task_error_manager = error_manager;
-}
-
-
 
 
 

laser/laser_task_command.h

@@ -33,29 +33,34 @@ public:
     //析构函数
     ~Laser_task();
 
-    //初始化任务单，必须初始化之后才可以使用，（必选参数）
-    // input:task_statu 任务状态
-    // output:p_task_point_cloud 三维点云容器的智能指针
-    // input:p_task_cloud_lock 三维点云的数据保护锁
-    //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-    Error_manager task_init(Task_statu task_statu,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock);
-
-    //初始化任务单，必须初始化之后才可以使用，（可选参数）
-    //    input:task_statu任务状态
-    //    input:task_statu_information状态说明
-    //    input:task_frame_maxnum点云的采集帧数最大值
-    //    output:p_task_point_cloud 三维点云容器的智能指针
-    // input:p_task_cloud_lock 三维点云的数据保护锁
-    //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-    Error_manager task_init(Task_statu task_statu,
-                            std::string & task_statu_information,
-                            unsigned int task_frame_maxnum,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock,
-							bool task_save_flag = false,
-							std::string task_save_path = "");
+	//初始化任务单，必须初始化之后才可以使用，（必选参数）
+	// input:p_task_cloud_lock 三维点云的数据保护锁
+	// 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);
+
+	//初始化任务单，必须初始化之后才可以使用，（可选参数）
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	//    input:task_frame_maxnum 点云的采集帧数最大值
+	//    input:task_save_flag 是否保存
+	//    input:task_save_path 保存路径
+	//    input:p_task_cloud_lock 三维点云的数据保护锁
+	//    output:p_task_point_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager 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
+	);
 
     //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
     Error_manager task_push_point(pcl::PointXYZ point_xyz);
@@ -68,16 +73,15 @@ public:
 	bool get_task_save_flag();
     //获取采集的点云保存路径
     std::string get_task_save_path();
+	//获取 三维点云容器的智能指针
+	std::mutex*  get_task_cloud_lock();
     //获取 三维点云容器的智能指针
     pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
 
 
 
 
-    //设置 任务状态
-    void set_task_statu(Task_statu task_statu);
-    //设置 任务状态说明
-    void set_task_statu_information(std::string & task_statu_information);
+
     //设置 点云的采集帧数最大值
     void set_task_frame_maxnum(unsigned int task_frame_maxnum);
 	//设置采集的点云保存文件的使能标志位
@@ -86,10 +90,8 @@ public:
     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_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
-    //设置 错误码
-    void set_task_error_manager(Error_manager & error_manager);
+
+
 
 
 protected:
@@ -104,7 +106,7 @@ protected:
     //三维点云的数据保护锁，任务输入
     std::mutex*                     mp_task_cloud_lock;
     //采集结果，三维点云容器的智能指针，任务输出
-    //这里只是指针，实际内存由应用层管理，初始化时必须保证内存有效。
+    //这里只是智能指针，实际内存由任务发送方管理管理，初始化时必须保证内存有效。
     pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
 
 

locate/YoloDetector.cpp

@@ -1,207 +0,0 @@
-#include "YoloDetector.h"
-#include <glog/logging.h>
-
-void YoloDetector::free_img(image_t m)
-{
-    if (m.data)
-    {
-        free(m.data);
-    }
-}
-
-Error_manager YoloDetector::set_region(float minx, float maxx, float miny, float maxy,float freq)
-{
-    if(m_maxx<=m_minx || m_maxy<=m_miny || m_freq<=0)
-    {
-        char description[255]={0};
-        sprintf(description,"set yolo parameter l:%.3f  r:%.3f  t:%.3f  b:%.3f",minx,maxx,miny,maxy);
-        return Error_manager(LOCATER_YOLO_PARAMETER_INVALID,NORMAL,description);
-    }
-    m_minx = minx;
-    m_maxx = maxx;
-    m_miny = miny;
-    m_maxy = maxy;
-    m_freq = freq;
-    return SUCCESS;
-}
-
-YoloDetector::YoloDetector(std::string cfg, std::string weights, float minx, float maxx, float miny, float maxy,float freq)
-{
-#if YOLO_API_USED
-	m_pDetector = new Detector(cfg, weights);
-	m_minx = minx;
-	m_maxx = maxx;
-	m_miny = miny;
-	m_maxy = maxy;
-	m_freq = freq;
-	//LOG(INFO) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
-#endif
-}
-YoloDetector::~YoloDetector()
-{
-#if YOLO_API_USED
-	if (m_pDetector)
-	{
-		delete m_pDetector;
-		m_pDetector = NULL;
-	}
-#endif
-}
-
-Error_manager YoloDetector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox>& boxes, std::string save_dir)
-{
-	boxes.clear();
-#if YOLO_API_USED
-	/*int w = int((m_maxx - m_minx) / m_freq);
-	int h = int((m_maxy - m_miny) / m_freq);*/
-	int w = 416;
-	int h = 208;
-	
-
-	cv::Mat img = cv::Mat::zeros(cv::Size(w, h * 2), CV_8UC1);
-	for (int i = 0; i < cloud_in->size(); ++i)
-	{
-		pcl::PointXYZ point = cloud_in->points[i];
-
-		float x = point.x;
-		float y = point.y;
-		float z = point.z;
-		if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.&&z>50.)
-		{
-			int c = int((x - m_minx) / (m_maxx-m_minx)*w);
-			int r = int((y - m_miny) / (m_maxy-m_miny)*h);
-			int gray = int(z / 2000. * 256);
-			if (gray > img.at<uchar>(r, c))
-				img.at<uchar>(r, c) = gray;
-		}
-
-	}
-
-	if (m_pDetector)
-	{
-		boxes.clear();
-		cv::cvtColor(img,img,CV_GRAY2RGB);
-        std::shared_ptr<image_t> img_t=mat_to_image_resize(img);
-        std::vector<bbox_t> rects=m_pDetector->detect(*img_t, 0.5);
-        //free_img(*img_t);
-		cv::Mat rgb=img.clone();
-		float ext = 0.0;
-		
-		for (int i = 0; i < rects.size(); ++i)
-		{
-			/*bbox_t box = rects[i];
-			bbox_t box_t = box;
-			float len = std::min(box.w*m_freq, box.h*m_freq);
-			box_t.x = int(box.x*m_freq + m_minx - ext*len);
-			box_t.y = int(box.y*m_freq + m_miny - ext*len);
-			box_t.w = int(box.w*m_freq*(1.0+2*ext));
-			box_t.h = int(box.h*m_freq*(1.0+2*ext));*/
-
-			bbox_t box = rects[i];
-			YoloBox box_y;
-            box_y.x = (float(box.x) / float(w)*(m_maxx - m_minx) + m_minx);
-            box_y.y = (float(box.y) /float(h)*(m_maxy - m_miny) + m_miny);
-            box_y.w = (float(box.w)/float(w)*(m_maxx - m_minx));
-            box_y.h = (float(box.h)/float(h)*(m_maxy - m_miny));
-
-			//LOG(WARNING) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
-			//LOG(WARNING) << "box " << i << " x:" << box.x << " ,y:" << box.y << " ,w:" << box.w << " ,h:" << box.h;
-			//LOG(INFO) << "yolo box " << i << " x:" << box_y.x << " ,y:" << box_y.y << " ,w:" << box_y.w << " ,h:" << box_y.h;
-
-			boxes.push_back(box_y);
-			//����boxes
-
-			cv::rectangle(rgb, cv::Point(box.x, box.y),
-				cv::Point(box.x + box.w, box.y + box.h), cv::Scalar(0, 255, 0), 1);
-
-		}
-
-		char num[32] = { 0 };
-		sprintf(num, " size=%d", rects.size());
-		cv::putText(rgb, num, cv::Point(50, 50), 1, 1, cv::Scalar(0, 255, 0));
-		cv::imwrite(save_dir + "/yolo.jpg", rgb);
-		if (boxes.size() > 0)
-			return SUCCESS;
-		else
-			return Error_manager(LOCATER_YOLO_DETECT_NO_TARGET,NORMAL,"yolo detect no car");
-
-	}
-#endif
-	return Error_manager(LOCATER_YOLO_DETECT_FAILED,NORMAL,"YOLO detect failed");
-}
-
-
-Error_manager YoloDetector::detect(cv::Mat& l, cv::Mat& r, std::vector<YoloBox>& boxes,std::string save_dir)
-{
-#if YOLO_API_USED
-	int w = int((m_maxx - m_minx) / m_freq);
-	int h = int((m_maxy - m_miny) / m_freq);
-
-	cv::Mat img = cv::Mat::zeros(cv::Size(w, h*2), CV_8UC1);
-	for (int i = 0; i < l.rows; ++i)
-	{
-		for (int j = 0; j < l.cols; ++j)
-		{
-			cv::Vec3f vec_l = l.at<cv::Vec3f>(i, j);
-			cv::Vec3f vec_r = r.at<cv::Vec3f>(i, j);
-	
-			float x = vec_l[0];
-			float y = vec_l[1];
-			float z = vec_l[2];
-			if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.)
-			{
-				int c = int((x - m_minx) / m_freq);
-				int r = int((y - m_miny) / m_freq);
-				int gray = int(z / 2000. * 256);
-				if (gray > img.at<uchar>(r, c))
-					img.at<uchar>(r, c) = gray;
-			}
-
-			x = vec_r[0];
-			y = vec_r[1];
-			z = vec_r[2];
-			if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.)
-			{
-				int c = int((x - m_minx) / m_freq);
-				int r = int((y - m_miny) / m_freq);
-				int gray = int(z / 2000. * 256);
-				if (gray > img.at<uchar>(r, c))
-					img.at<uchar>(r, c) = gray;
-			}
-		}
-	}
-
-	if (m_pDetector)
-	{
-		boxes.clear();
-		std::shared_ptr<image_t> img_t=mat_to_image_resize(img);
-		std::vector<bbox_t> rects=m_pDetector->detect(*img_t, 0.5);
-		free_img(*img_t);
-		cv::Mat rgb;
-		cv::cvtColor(img, rgb, CV_GRAY2RGB);
-		for (int i = 0; i < rects.size(); ++i)
-		{
-			bbox_t box = rects[i];
-            YoloBox box_t;
-			box_t.x = (box.x*m_freq+m_minx);
-			box_t.y = (box.y*m_freq+m_miny);
-			box_t.w = (box.w*m_freq);
-			box_t.h = (box.h*m_freq);
-			boxes.push_back(box_t);
-			//����boxes
-			
-			cv::rectangle(rgb, cv::Point(box.x, box.y ),
-				cv::Point(box.x+box.w, box.y + box.h), cv::Scalar(0, 255, 0), 1);
-			
-		}
-		char num[32] = { 0 };
-		sprintf(num, " size=%d", rects.size());
-		cv::putText(rgb, num, cv::Point(50, 50), 1, 1, cv::Scalar(0, 255, 0));
-		cv::imwrite(save_dir + "/yolo.jpg", rgb(cv::Rect(0,0,rgb.cols,rgb.rows/2)));
-		return SUCCESS;
-
-	}
-#endif
-	return Error_manager(LOCATER_YOLO_DETECT_FAILED,NORMAL,"yolo api not define,(no code)");
-
-}

locate/YoloDetector.h

@@ -1,105 +0,0 @@
-#ifndef YOLO_DETECTOR__HH
-#define YOLO_DETECTOR__HH
-
-#include "opencv/highgui.h"
-#include "opencv2/opencv.hpp"
-#include <pcl/point_types.h>
-#include <pcl/common/common.h>
-#include "yolo_v2_class.hpp"
-#include "../error_code/error_code.h"
-
-#define YOLO_API_USED	1
-
-typedef struct YOLO_BOX
-{
-    float x, y, w, h;
-}YoloBox;
-
-/*
- * yolo v2网络识别类, 输入图像大小为416*416.
- */
-class YoloDetector
-{
-public:
-	YoloDetector(std::string cfg,std::string weights,float minx,float maxx,float miny,float maxy,float freq);
-	//设置输入点云的边界,识别的时候根据边界投影到图像
-	//输入边界的长款比必须是1:1,否则会出现图像扭曲
-	//freq:表示多长一个像素点
-    Error_manager set_region(float minx, float maxx, float miny, float maxy,float freq);
-	virtual ~YoloDetector();
-    //识别函数, 将点云数据转换成图像格式保存,该函数已经过期(第一版算法,点云由两个雷达扫描)
-    //l:左边雷达扫描数据; r:右边雷达扫描数据
-    //save_dir:中间数据保存路径
-	Error_manager detect(cv::Mat& l, cv::Mat& r, std::vector<YoloBox>& boxes,std::string save_dir);
-	//将点云投影成图像, 识别车辆外接矩形框
-	Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox>& boxes, std::string save_dir);
-protected:
-    //释放图像内存
-    void free_img(image_t img);
-    //opencv图像类型转换成 image_t 格式,并调整大小
-    std::shared_ptr<image_t> mat_to_image_resize(cv::Mat mat) const
-    {
-        if (mat.data == NULL) return std::shared_ptr<image_t>(NULL);
-        cv::Mat det_mat;
-        cv::resize(mat, det_mat, cv::Size(m_pDetector->get_net_width(), m_pDetector->get_net_height()));
-        return mat_to_image(det_mat);
-    }
-
-    static std::shared_ptr<image_t> mat_to_image(cv::Mat img_src)
-    {
-        cv::Mat img;
-        cv::cvtColor(img_src, img, cv::COLOR_RGB2BGR);
-        std::shared_ptr<image_t> image_ptr(new image_t, [](image_t *img) { if(img->data) free(img->data); delete img; });
-        std::shared_ptr<IplImage> ipl_small = std::make_shared<IplImage>(img);
-        *image_ptr = ipl_to_image(ipl_small.get());
-        return image_ptr;
-    }
-    static image_t ipl_to_image(IplImage* src)
-    {
-        unsigned char *data = (unsigned char *)src->imageData;
-        int h = src->height;
-        int w = src->width;
-        int c = src->nChannels;
-        int step = src->widthStep;
-        image_t out = make_image_custom(w, h, c);
-        int count = 0;
-
-        for (int k = 0; k < c; ++k) {
-            for (int i = 0; i < h; ++i) {
-                int i_step = i*step;
-                for (int j = 0; j < w; ++j) {
-                    out.data[count++] = data[i_step + j*c + k] / 255.;
-                }
-            }
-        }
-
-        return out;
-    }
-
-    static image_t make_empty_image(int w, int h, int c)
-    {
-        image_t out;
-        out.data = 0;
-        out.h = h;
-        out.w = w;
-        out.c = c;
-        return out;
-    }
-
-    static image_t make_image_custom(int w, int h, int c)
-    {
-        image_t out = make_empty_image(w, h, c);
-        out.data = (float *)calloc(h*w*c, sizeof(float));
-        return out;
-    }
-
-protected:
-	Detector*	m_pDetector;
-	float m_minx;
-	float m_maxx;
-	float m_miny;
-	float m_maxy;
-	float m_freq;
-};
-#endif
-

locate/cnn3d_segmentation.cpp

@@ -1,721 +0,0 @@
-#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));
-}
-
-double IIU(std::vector<cv::Point>& poly1, std::vector<cv::Point>& poly2, float max_x, float max_y)
-{
-	int width = int(max_x) + 100;
-	int height = int(max_y) + 100;
-	//��ͼ1
-	cv::Mat src1 = cv::Mat::zeros(cv::Size(width, height), CV_8UC1);
-	cv::Mat src2 = cv::Mat::zeros(cv::Size(width, height), CV_8UC1);
-	std::vector<std::vector<cv::Point> > polys1;
-	std::vector<std::vector<cv::Point> > polys2;
-	polys1.push_back(poly1);
-	polys2.push_back(poly2);
-	cv::fillPoly(src1, polys1, cv::Scalar(1));
-	cv::fillPoly(src2, polys2, cv::Scalar(1));
-
-	cv::Mat I = src1&src2;
-	//cv::Mat U = src1 | src2;
-	cv::Scalar S_I = cv::sum(I);
-	cv::Scalar S_U = cv::sum(src1);
-
-	if (S_U[0] == 0) return 0;
-
-	return double(S_I[0]) / double(S_U[0]);
-
-}
-
-float* Cnn3d_segmentation::generate_tensor(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float min_x, float max_x,
-	float min_y, float max_y, float min_z, float max_z)
-{
-	int size = m_lenth*m_width*m_height;
-	float* data = (float*)malloc(size*sizeof(float));
-	memset(data, 0, size*sizeof(float));
-	for (int i = 0; i < cloud->size(); ++i)
-	{
-		pcl::PointXYZ point = cloud->points[i];
-		int new_x = (point.x - min_x) / (max_x - min_x)*m_lenth;
-		int new_y = (point.y - min_y) / (max_y - min_y)*m_width;
-		int new_z = (point.z - min_z) / (max_z - min_z)*m_height;
-		if (new_x < 0 || new_x >= m_lenth || new_y < 0 || new_y >= m_width || new_z < 0 || new_z >= m_height)
-		{
-			continue;
-		}
-		*(data + new_x*(m_width*m_height) + new_y*m_height + new_z) = 1.0;
-	}
-	return data;
-}
-
-std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> Cnn3d_segmentation::decodeCloud(pcl::PointCloud<pcl::PointXYZ>& cloud,
-		float* data, float min_x, float max_x, float min_y, float max_y, float min_z, float max_z)
-{
-	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
-	for (int i = 0; i < m_nClass - 1; ++i)
-		clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
-	for (int i = 0; i < cloud.size(); ++i)
-	{
-		pcl::PointXYZ point = cloud.points[i];
-		int new_x = (point.x - min_x) / (max_x - min_x)*m_lenth;
-		int new_y = (point.y - min_y) / (max_y - min_y)*m_width;
-		int new_z = (point.z - min_z) / (max_z - min_z)*m_height;
-		if (new_x < 0 || new_x >= m_lenth || new_y < 0 || new_y >= m_width || new_z < 0 || new_z >= m_height)
-		{
-			continue;
-		}
-		int max_index = 0;
-		float max_prob = *(data + new_x*(m_width*m_height*m_nClass) + new_y*m_height*m_nClass + new_z*m_nClass + 0);
-
-		for (int j = 0; j < m_nClass; ++j)
-		{
-			if (*(data + new_x*(m_width*m_height*m_nClass) + new_y*m_height*m_nClass + new_z*m_nClass + j) > max_prob)
-			{
-				max_prob = *(data + new_x*(m_width*m_height*m_nClass) + new_y*m_height*m_nClass + new_z*m_nClass + j);
-				max_index = j;
-			}
-		}
-
-		pcl::PointXYZRGB point_color;
-		point_color.x = point.x;
-		point_color.y = point.y;
-		point_color.z = point.z;
-		point_color.r = 255;
-		point_color.g = 255;
-		point_color.b = 255;
-		switch (max_index)
-		{
-		case 1:
-			point_color.r = 0;
-			point_color.g = 255;
-			point_color.b = 0;
-			clouds[0]->push_back(point_color);
-			break;
-		case 2:
-			point_color.r = 255;
-			point_color.g = 0;
-			point_color.b = 0;
-			clouds[1]->push_back(point_color);
-			break;
-		default:break;
-		}
-
-	}
-	return clouds;
-}
-
-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,NORMAL,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,NORMAL,"first locate 3dcnn failed");
-	}
-	free(data);
-	free(pout);
-	return SUCCESS;*/
-}
-
-std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> Cnn3d_segmentation::kmeans(
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,std::string file_path)
-{
-    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> kmens_out_clouds;
-    for(int i=0;i<4;++i)
-    {
-        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-        kmens_out_clouds.push_back(t_cloud);
-    }
-
-    //开始分割
-	std::vector<cv::Point2f> cv_points;
-	for (int i = 0; i < cloud->size(); ++i)
-	{
-		cv::Point2f point(cloud->points[i].x, cloud->points[i].y);
-		cv_points.push_back(point);
-	}
-	cv::RotatedRect rect = cv::minAreaRect(cv_points);
-	cv::Point2f corner[4];
-	rect.points(corner);
-
-	std::vector<cv::Point2f> clusters[4];
-	double sumX[4] = { 0.0};
-	double sumY[4] = { 0.0 };
-	for (int i = 0; i < cloud->size(); ++i)
-	{
-		double min_distance = 9999999.9;
-		int cluste_index = 0;
-		cv::Point2f point(cloud->points[i].x, cloud->points[i].y);
-		for (int j = 0; j < 4; ++j)
-		{
-			double dis=distance(point, corner[j]);
-			if (dis < min_distance)
-			{
-				min_distance = dis;
-				cluste_index = j;
-			}
-		}
-		clusters[cluste_index].push_back(point);
-		sumX[cluste_index] += point.x;
-		sumY[cluste_index] += point.y;
-
-        pcl::PointXYZ t_point3d;
-        t_point3d.x=cloud->points[i].x;
-        t_point3d.y=cloud->points[i].y;
-        t_point3d.z=cloud->points[i].z;
-        if(cluste_index>=0&&cluste_index<4)
-        {
-            kmens_out_clouds[cluste_index]->push_back(t_point3d);
-        }
-
-		if (cluste_index == 0)
-		{
-			cloud->points[i].r = 255;
-			cloud->points[i].g = 0;
-			cloud->points[i].b = 0;
-		}
-		else if (cluste_index == 1)
-		{
-			cloud->points[i].r = 0;
-			cloud->points[i].g = 255;
-			cloud->points[i].b = 0;
-		}
-		else if (cluste_index == 2)
-		{
-			cloud->points[i].r = 0;
-			cloud->points[i].g = 0;
-			cloud->points[i].b = 255;
-		}
-		else
-		{
-
-			cloud->points[i].r = 255;
-			cloud->points[i].g = 255;
-			cloud->points[i].b = 255;
-		}
-	}
-	std::vector<cv::Point2f> rets;
-	for (int i = 0; i < 4; ++i)
-	{
-		if (clusters[i].size() == 0)
-		{
-			continue;
-		}
-		float avg_x = sumX[i] / float(clusters[i].size());
-		float avg_y = sumY[i] / float(clusters[i].size());
-		rets.push_back(cv::Point2f(avg_x, avg_y));
-
-	}
-	std::string save_file=file_path+"/cnn3d.txt";
-	save_rgb_cloud_txt(save_file, cloud);
-	return kmens_out_clouds;
-
-
-	/*int clusterCount = 4;
-	int sampleCount = cloud->size();
-
-	cv::Mat points(sampleCount, 1, CV_32FC2), labels;   //��������������ʵ����Ϊ2ͨ������������Ԫ������ΪPoint2f
-	for (int i = 0; i < sampleCount; ++i)
-	{
-		points.at<cv::Point2f>(i) = cv::Point2f(cloud->points[i].x, cloud->points[i].y);
-	}
-
-	clusterCount = MIN(clusterCount, sampleCount);
-	cv::Mat centers(clusterCount, 1, points.type());    //�����洢���������ĵ�
-	cv::RNG rng(12345); //�����������
-
-
-	cv::kmeans(points, clusterCount, labels,
-		cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 500, 15.0),
-		5, cv::KMEANS_PP_CENTERS, centers);
-
-
-
-	for (int i = 0; i < sampleCount; i++)
-	{
-		int clusterIdx = labels.at<int>(i);
-		pcl::PointXYZ point;
-		point.x=cloud->points[i].x;
-        point.y=cloud->points[i].y;
-        point.z=cloud->points[i].z;
-        if(clusterIdx>=0&&clusterIdx<4)
-        {
-            kmens_out_clouds[clusterIdx]->push_back(point);
-        }
-		if (clusterIdx == 0)
-		{
-			cloud->points[i].r = 255;
-			cloud->points[i].g = 0;
-			cloud->points[i].b = 0;
-
-		}
-		else if (clusterIdx == 1)
-		{
-			cloud->points[i].r = 0;
-			cloud->points[i].g = 255;
-			cloud->points[i].b = 0;
-		}
-		else if (clusterIdx == 2)
-		{
-			cloud->points[i].r = 0;
-			cloud->points[i].g = 0;
-			cloud->points[i].b = 255;
-		}
-		else
-		{
-
-			cloud->points[i].r = 255;
-			cloud->points[i].g = 255;
-			cloud->points[i].b = 255;
-		}
-
-	}
-    std::string save_file=file_path+"/cnn3d.txt";
-	save_rgb_cloud_txt(save_file, cloud);*/
-	return kmens_out_clouds;
-}
-
-Error_manager Cnn3d_segmentation::isRect(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,NORMAL,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,NORMAL,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,NORMAL,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,NORMAL,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,NORMAL,description);
-        }
-
-    }
-
-}
-
-void save_cloudT(std::string txt, pcl::PointCloud<pcl::PointXYZ>& pCloud)
-{
-    std::ofstream os;
-    os.open(txt, std::ios::out);
-    for (int i = 0; i < pCloud.points.size(); i++)
-    {
-        pcl::PointXYZ 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::predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-                                          float& center_x,float& center_y,float& wheel_base,
-    float& width,float& angle,std::string cluster_file_path)
-{
-    if(cloud.get()==NULL)
-    {
-        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,NORMAL,"3dcnn predict input cloud uninit");
-    }
-    if(cloud->size()==0)
-    {
-        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,NORMAL,"3dcnn predict input cloud empty");
-    }
-
-	clock_t clock1 = clock();
-
-    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudout(new pcl::PointCloud<pcl::PointXYZRGB>);
-    pcl::copyPointCloud(*cloud,*cloudout);
-
-
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr last_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-	if (cloudout->size() == 0)
-	{
-		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,NORMAL,"tf pred class[wheel].points size ==0");
-	}
-    //离群点过滤
-	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sorfilter(true); // Initializing with true will allow us to extract the removed indices
-	sorfilter.setInputCloud(cloudout);
-	sorfilter.setMeanK(20);
-	sorfilter.setStddevMulThresh(2.0);
-	sorfilter.filter(*cloudout);
-
-	if (cloudout->size() < 4 )
-	{
-		return Error_manager(LOCATER_3DCNN_PREDICT_FAILED,NORMAL,"tf pred wheel points size <4");
-	}
-	//kmeans
-    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> kmeans_out_clouds = kmeans(cloudout, cluster_file_path);
-	//对kmeans结果点云作pca filter
-    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> pca_filter_clouds;
-    for(int i=0;i<4;++i)
-    {
-        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-        pca_filter_clouds.push_back(t_cloud);
-    }
-	for(int i=0;i<kmeans_out_clouds.size();++i)
-    {
-	    pca_minist_axis_filter(kmeans_out_clouds[i],pca_filter_clouds[i]);
-
-        char buf[255]={0};
-        sprintf(buf,"%s/pca_wheel_%d.txt",cluster_file_path.c_str(),i);
-        save_cloudT(buf,*pca_filter_clouds[i]);
-    }
-    //计算pca filter点云中心
-    std::vector<cv::Point2f> centers;
-    for(int i=0;i<pca_filter_clouds.size();++i)
-    {
-        if(pca_filter_clouds[i]->size()!=0) {
-            Eigen::Vector4f centroid;
-            pcl::compute3DCentroid(*pca_filter_clouds[i], centroid);
-            centers.push_back(cv::Point2f(centroid[0],centroid[1]));
-        }
-
-    }
-    //将pca过滤后的轮胎点转换成opencv点,用于计算角度与宽
-	std::vector<cv::Point2f> wheel_points;
-	for(int i=0;i<pca_filter_clouds.size();++i)
-    {
-	    for(int j=0;j<pca_filter_clouds[i]->size();++j)
-	        wheel_points.push_back(cv::Point2f(pca_filter_clouds[i]->points[j].x,
-                                               pca_filter_clouds[i]->points[j].y));
-    }
-	if (centers.size() != 4 && centers.size()!=3)
-	{
-		return Error_manager(LOCATER_3DCNN_KMEANS_FAILED,NORMAL,"3dcnn cluster center size != 4 & 3");
-	}
-	//取四轮中心计算轴长
-	Error_manager code=isRect(centers);
-	if(code!=SUCCESS)
-	{
-		return code;
-	}
-
-    if(centers.size()==3)
-    {
-        wheel_points.push_back(centers[centers.size()-1]);
-    }
-
-    cv::RotatedRect rect_center = minAreaRect_cloud(centers);
-    cv::RotatedRect rect_wheels=minAreaRect_cloud(wheel_points);
-    bool IOU = check_box(rect_center, cloud);
-    if(IOU==false){
-        return Error_manager(LOCATER_3DCNN_IIU_FAILED,NORMAL,"IIU check box failed");
-    }
-	//开始赋值
-    center_x=rect_center.center.x;
-    center_y=rect_center.center.y;
-    wheel_base=std::max(rect_center.size.height,rect_center.size.width);
-    width=std::min(rect_center.size.height,rect_center.size.width);
-    //根据 rect_wheel 计算角度
-    const double C_PI=3.14159265;
-    cv::Point2f vec;
-    cv::Point2f vertice[4];
-    rect_wheels.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;
-    }
-    angle = 180.0 / C_PI * acos(vec.x / sqrt(vec.x * vec.x + vec.y * vec.y));
-
-
-    LOG(INFO)<<"3dcnn rotate rect center :"<<rect_center.center<<"   size : "<<rect_center.size<<"  angle:"<<angle;
-
-	return SUCCESS;
-}
-
-Error_manager Cnn3d_segmentation::pca_minist_axis_filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
-                                                              pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out)
-{
-    if(cloud_in.get()==NULL)
-    {
-        return Error_manager(PARAMETER_ERROR,NORMAL,"pca_minist_axis_filter  input cloud uninit");
-    }
-    if(cloud_in->size()==0)
-    {
-        return Error_manager(PARAMETER_ERROR,NORMAL,"pca_minist_axis_filter  input cloud empty");
-    }
-
-    //pca寻找主轴
-    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
-    feature_extractor.setInputCloud (cloud_in);
-    feature_extractor.compute ();
-
-    std::vector <float> moment_of_inertia;
-    std::vector <float> eccentricity;
-    pcl::PointXYZ min_point_AABB;
-    pcl::PointXYZ max_point_AABB;
-    pcl::PointXYZ min_point_OBB;
-    pcl::PointXYZ max_point_OBB;
-    pcl::PointXYZ 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<cloud_in->size();++i)
-    {
-        pcl::PointXYZ point=cloud_in->points[i];
-        if(fabs(point.x*a+point.y*b+point.z*c+d)/S <40 )
-        {
-            cloud_out->push_back(point);
-        }
-    }
-
-    /*if(cloud_out->size()==0)
-    {
-        return Error_manager(LOCATER_3DCNN_PCA_OUT_CLOUD_EMPTY,NORMAL,"wheel pca filter out cloud empty");
-    }*/
-    return SUCCESS;
-}
-
-bool Cnn3d_segmentation::check_box(cv::RotatedRect& box, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
-{
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected_XY(new pcl::PointCloud<pcl::PointXYZ>);
-	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::PointXYZ> proj;
-	proj.setModelType(pcl::SACMODEL_PLANE);
-	proj.setInputCloud(cloud);
-	proj.setModelCoefficients(coefficients);
-	proj.filter(*cloud_projected_XY);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_convexhull(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::ConvexHull<pcl::PointXYZ> cconvexhull;
-	cconvexhull.setInputCloud(cloud_projected_XY);
-	cconvexhull.setDimension(2);
-	cconvexhull.reconstruct(*cloud_convexhull);
-	//����IOU
-	pcl::PointXYZ minPt, maxPt;
-	pcl::getMinMax3D(*cloud_convexhull, minPt, maxPt);
-	cv::Point2f vertice[4];
-	box.points(vertice);
-	std::vector<cv::Point> poly;
-	poly.push_back(vertice[0]);
-	poly.push_back(vertice[1]);
-	poly.push_back(vertice[2]);
-	poly.push_back(vertice[3]);
-
-	std::vector<cv::Point> poly1;
-	for (int i = 0; i < cloud_convexhull->points.size(); ++i)
-	{
-		pcl::PointXYZ pt = cloud_convexhull->points[i];
-		cv::Point2f point(pt.x, pt.y);
-		poly1.push_back(point);
-	}
-	//double area_percent = IIU(poly, poly1, maxPt.x, maxPt.y);
-	if (/*area_percent < 0.95 ||*/ std::min(box.size.width, box.size.height)>2000.0 || std::min(box.size.width, box.size.height)<1200.0)
-	{
-		return false;
-	}
-	if (std::max(box.size.width, box.size.height) < 1900 || std::max(box.size.width, box.size.height) > 3600)
-	{
-		LOG(ERROR) << "\tlen not in 1900-3400";
-		return false;
-	}
-	return true;
-}

locate/cnn3d_segmentation.h

@@ -1,70 +0,0 @@
-#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"
-
-/*
- * 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);
-
-	//预测
-	//cloud:输入点云
-	//rect:输出旋转矩形框
-	//save_dir:中间文件保存路径
-	virtual Error_manager predict(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-	    float& center_x,float& center_y,
-	    float& wheel_base,float& width,float& angle, std::string save_dir);
-
-protected:
-    //根据设置的参数将点云转换成网络输入数格式
-	float* generate_tensor(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float min_x, float max_x,
-		float min_y, float max_y, float min_z, float max_z);
-    //将识别结果转换成点云
-	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> decodeCloud(pcl::PointCloud<pcl::PointXYZ>& cloud,
-		float* data, float min_x, float max_x, float min_y, float max_y, float min_z, float max_z);
-protected:
-	//判断矩形框是否找到
-	//points:输入轮子中心点, 只能是3或者4个
-	Error_manager isRect(std::vector<cv::Point2f>& points);
-	//kmeans算法聚类,拆分四个轮子,返回四轮中心
-	//cloud:输入点云
-    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> kmeans(
-        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, std::string cluster_file_path);
-	bool check_box(cv::RotatedRect& box, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
-	///IIU判断
-	bool check_IOU(cv::RotatedRect& box, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
-    /*
-     * 对点云作pca分析,然后在最小轴方向过滤, 防止轮胎左右两边出现噪声
-     * cloud_in:输入点云
-     * cloud_out:输出点云
-     */
-    Error_manager pca_minist_axis_filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
-                                                             pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out);
-
-    /*
-     *计算两点距离
-     */
-    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_task.cpp

@@ -1,54 +0,0 @@
-//
-// Created by zx on 2019/12/30.
-//
-
-#include "locate_task.h"
-#include <glog/logging.h>
-Locate_task::Locate_task()
-{
-    m_task_type=LOCATE_TASK;
-}
-Locate_task::~Locate_task()
-{
-
-}
-
-Error_manager Locate_task::set_locate_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
-{
-    if(cloud.get()==0)
-    {
-        return Error_manager(LOCATER_TASK_INPUT_CLOUD_UNINIT,NORMAL,"locate task set input cloud uninit");
-    }
-    if(cloud->size()==0) {
-        return Error_manager(LOCATER_TASK_INIT_CLOUD_EMPTY,NORMAL,"locate task input cloud is empty");
-    }
-    m_locate_cloud_ptr=cloud;
-    return SUCCESS;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr Locate_task::get_locate_cloud()
-{
-    return m_locate_cloud_ptr;
-}
-
-Locate_information Locate_task::get_locate_information()
-{
-    return m_locate_information;
-}
-
-Error_manager Locate_task::set_locate_information(Locate_information information)
-{
-    memcpy(&m_locate_information,&information,sizeof(Locate_information));
-    return SUCCESS;
-}
-
-std::string Locate_task::get_save_path()
-{
-    return m_save_path;
-}
-
-Error_manager Locate_task::set_save_path(std::string path)
-{
-    m_save_path=path;
-    return SUCCESS;
-}

locate/locate_task.h

@@ -1,57 +0,0 @@
-//
-// 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>
-
-/*
- * 测量结果结构体
- */
-typedef struct LOCATE_INFORMATION
-{
-    float locate_x;
-    float locate_y;
-    float locate_theta;
-    float locate_length;
-    float locate_width;
-    float locate_hight;
-    float locate_wheel_base;
-    bool locate_correct;
-}Locate_information;
-
-/*
- * 测量任务类,负责维护测量任务所需的输入参数以及测量的结果
- */
-class Locate_task: public Task_Base
-{
-public:
-    Locate_task();
-    ~Locate_task();
-    //设置测量输入点云
-    Error_manager set_locate_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
-    //获取输入点云
-    pcl::PointCloud<pcl::PointXYZ>::Ptr get_locate_cloud();
-    //获取测量结果
-    Locate_information get_locate_information();
-    //设置测量结果
-    Error_manager set_locate_information(Locate_information information);
-    //获取文件存放路径
-    std::string get_save_path();
-    //设置文件村昂路径
-    Error_manager set_save_path(std::string path);
-protected:
-    //测量算法输入点云
-    pcl::PointCloud<pcl::PointXYZ>::Ptr         m_locate_cloud_ptr;
-    //测量结果
-    Locate_information                          m_locate_information;
-    //测量中间文件存放路径
-    std::string                                 m_save_path;
-};
-
-
-#endif //LOCATE_TASK_H

locate/locate_uml.wsd

@@ -1,252 +0,0 @@
-@startuml
-title 摆扫测量模块类图
-
-class Task_Base
-{
-    +~Task_Base();
-    +virtual Error_manager init();//初始化任务单,初始任务单类型为 UNKONW_TASK
-    +Error_manager update_statu(Task_statu task_statu,std::string statu_information="");//更新任务单
-    +Task_type   get_task_type();//获取任务类型
-    +Task_statu  get_statu();//获取任务单状态
-    +std::string get_statu_information();//获取状态说明
-}
-
-
-class Locate_task
-{
-    +Locate_task();
-    +~Locate_task();
-    //设置测量输入点云()
-    +Error_manager set_locate_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
-    //获取输入点云()
-    +pcl::PointCloud<pcl::PointXYZ>::Ptr get_locate_cloud();
-    //获取测量结果()
-    +Locate_information get_locate_information();
-    //设置测量结果()
-    +Error_manager set_locate_information(Locate_information information);
-    //获取文件存放路径()
-    +std::string get_save_path();
-    //设置文件村昂路径()
-    +Error_manager set_save_path(std::string path);
-
-    -pcl::PointCloud<pcl::PointXYZ>::Ptr         m_locate_cloud_ptr;//测量算法输入点云
-    -Locate_information                          m_locate_information;//测量结果存放结构体
-    -std::string                                 m_save_path;//测量中间文件存放路径
-}
-
-
-
-class Detector 
-{
-    std::shared_ptr<void> detector_gpu_ptr;//yolo测量网络指针
-    std::deque<std::vector<bbox_t>> prev_bbox_vec_deque;
-    const int cur_gpu_id;
-    float nms = .4;
-    bool wait_stream;
-
-    + Detector(std::string cfg_filename, std::string weight_filename, int gpu_id = 0);
-    + ~Detector();
-
-    + std::vector<bbox_t> detect(std::string image_filename, float thresh = 0.2, bool use_mean = false);
-    + std::vector<bbox_t> detect(image_t img, float thresh = 0.2, bool use_mean = false);
-    + image_t load_image(std::string image_filename);
-    + void free_image(image_t m);
-    + int get_net_width() const;
-    + int get_net_height() const;
-    + int get_net_color_depth() const;
-
-    + std::vector<bbox_t> tracking_id(std::vector<bbox_t> cur_bbox_vec, bool const change_history = true,
-                                                int const frames_story = 5, int const max_dist = 40);
-
-    + void *get_cuda_context();
-
-    //+ bool send_json_http(std::vector<bbox_t> cur_bbox_vec, std::vector<std::string> obj_names, int frame_id,
-    //    std::string filename = std::string(), int timeout = 400000, int port = 8070);
-    //重置图像大小,并识别()
-    +std::vector<bbox_t> detect_resized(image_t img, int init_w, int init_h, float thresh = 0.2, bool use_mean = false);
-    
-}
-
-class YoloDetector
-{
-	+YoloDetector(std::string cfg,std::string weights,float minx,float maxx,float miny,float maxy,float freq);
-	+//设置输入点云的边界,识别的时候根据边界投影到图像()
-	+//输入边界的长款比必须是1:1,否则会出现图像扭曲()
-	+//freq:表示多长一个像素点()
-    +Error_manager set_region(float minx, float maxx, float miny, float maxy,float freq);
-	+virtual ~YoloDetector();
-    +//识别函数, 将点云数据转换成图像格式保存,该函数已经过期(第一版算法,点云由两个雷达扫描)
-    +//l:左边雷达扫描数据; r:右边雷达扫描数据()
-    +//save_dir:中间数据保存路径()
-	+Error_manager detect(cv::Mat& l, cv::Mat& r, std::vector<YoloBox>& boxes,std::string save_dir);
-	+//将点云投影成图像, 识别车辆外接矩形框()
-	+Error_manager detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox>& boxes, std::string save_dir);
-
-    -//释放图像内存()
-    -void free_img(image_t img);
-    -//opencv图像类型转换成 image_t 格式,并调整大小()
-    -std::shared_ptr<image_t> mat_to_image_resize(cv::Mat mat) const
-    -static std::shared_ptr<image_t> mat_to_image(cv::Mat img_src)
-    -static image_t ipl_to_image(IplImage* src)
-    -static image_t make_empty_image(int w, int h, int c)
-    -static image_t make_image_custom(int w, int h, int c)
-	-Detector*	m_pDetector;
-	-float m_minx;
-	-float m_maxx;
-	-float m_miny;
-	-float m_maxy;
-	-float m_freq;
-}
-
-class PointSifter
-{
-    +//初始化PointSift()
-    +//point_num:pointsift输入点数()
-    +//cls_num:PointSift输出类别数()
-	+PointSifter(int point_num, int cls_num);
-	+~PointSifter();
-	+//加载网络参数()
-	+//meta:tensorflow网络结构定义文件()
-	+//cpkt:tensorflow网络权重()
-	+bool Load(std::string meta, std::string cpkt);
-	+//预测()
-	+//data:输入数据,大小为  输入点数*3()
-	+//output:输出数据,大小为  输入点数*类别数()
-	+bool Predict(float* data, float* output);
-	+//错误原因()
-	+std::string LastError();
-
-	-PointSifter();
-	-std::mutex		m_mutex;
-	-std::string		m_error;
-	-bool			m_bInit;
-	-int				m_point_num;
-	-int				m_cls_num;
-	-void*			m_sess;
-}
-note right of PointSifter
-    sift识别类,导入类
-    负责pointsift网络的加载识别,
-    内部加锁,保证只有一个识别任务占有
-end note
-
-
-class Point_sift_segmentation 
-{
-	+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);
-    +//初始化网络, 加载网络权重()
-	+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);
-	+virtual ~Point_sift_segmentation();
-
-    -//将点云数据转换到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);
-
-	-float		        m_freq;
-    -pcl::PointXYZ		m_minp;
-    -pcl::PointXYZ		m_maxp;
-
-}
-note bottom of Point_sift_segmentation
- * 封装PointSift点云分割网络,输入点云,分割出汽车,检验结果
- * 功能包括将pcl点云数据格式转换成原始数据格式float*
- * 将识别数据转换成点云数据并用不同颜色区分
-end note
-
-
-class LocateParameter
-{
-    +获取各个配置参数()
-    +序列化配置到文件()
-    +从文件流读取配置()
-}
-note left of LocateParameter
-    测量算法参数配置类
-    格式protobuf,
-    详见locater_parameter.proto文件
-end note
-
-class Locater
-{
-    +ocater();
-    +Locater();
-
-    +/初始化测量算法参数()
-    +Error_manager init(Measure::LocateParameter parameter);
-
-    +/获取测量模块状态(错误码);返回是否能执行任务,以及不能执行任务的原因()
-    +Error_manager get_error();
-
-    +//执行任务,()
-    +//task:测量任务()
-    +//time_out:超时时间,单位秒()
-    +Error_manager execute_task(Task_Base* task,double time_out=5);
-
-    -//测量算法函数()
-    -//cloud_in:输入点云()
-    -//locate_information:测量结果()
-    -//work_dir:中间文件保存路径()
-    -Error_manager locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
-
-    -Locate_information& locate_information, std::string work_dir);
-
-    -//yolo定位车辆外接矩形()
-    -//cloud_in:输入点云,定位算法内部不会修改点云内容()
-    -//boxes:定位到的外接box框()
-    -//work_dir:中间文件输出路径()
-    -Error_manager locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
-    -    std::vector<YoloBox>& boxes, std::string work_dir);
-
-    -//根据yolo识别结果, 利用PointSift从场景中分割车辆点云()
-    -//cloud:输入点云()
-    -//boxes:yolo识别结果()
-    -//cloud_target:车辆点云()
-    -//work_dir:中间文件保存路径()
-    -Error_manager locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<YoloBox> boxes,
-    -                       pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_wheel,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_car,
-    -                       std::string work_dir);
-
-    -//输入汽车点云,识别轮胎,并计算轴长,宽,中心,旋转角()
-    -Error_manager locate_wheel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float& center_x,float& center_y,float& wheel_base,float& width,float& angle,std::string work_dir);
-    
-    -//根据汽车点云计算车高()
-    -//cloud_car:车辆点云()
-    -Error_manager measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height);
-    
-    -// 保存点云成txt到文件()
-    -static void save_cloud_txt(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,std::string save_file);
-
-    -Measure::LocateParameter           m_locate_parameter;
-    -Point_sift_segmentation*                mp_point_sift;
-    -YoloDetector*                           mp_yolo_detector;
-    -Cnn3d_segmentation*                     mp_cnn3d;
-
-    -std::mutex						        m_mutex_lock;
-}
-
-
-Locate_task --|> Task_Base
-YoloDetector --> Detector
-Point_sift_segmentation --|> PointSifter
-
-Locater --> Point_sift_segmentation
-Locater --> YoloDetector
-Locater --> LocateParameter
-
-
-
-@enduml

locate/locater.cpp

@@ -1,382 +0,0 @@
-//
-// Created by zx on 2019/12/30.
-//
-
-#include "locater.h"
-#include <pcl/filters//voxel_grid.h>
-#include <pcl/filters/passthrough.h>
-#include <glog/logging.h>
-Locater::Locater():mp_yolo_detector(0),mp_point_sift(0),mp_cnn3d(0)
-{
-}
-Locater::~Locater()
-{}
-
-Error_manager Locater::get_error()
-{
-    return SUCCESS;
-}
-
-Error_manager Locater::init(Measure::LocateParameter parameter)
-{
-    Error_manager  code;
-    {
-        int point_size = parameter.seg_parameter().point_size();
-        int cls_num = parameter.seg_parameter().cls_num();
-        double freq = parameter.seg_parameter().freq();
-        std::string graph = parameter.seg_parameter().graph();
-        std::string cpkt = parameter.seg_parameter().cpkt();
-        Measure::Area3d area = parameter.seg_parameter().area();
-        pcl::PointXYZ minp(area.x_min(), area.y_min(), area.z_min());
-        pcl::PointXYZ maxp(area.x_max(), area.y_max(), area.z_max());
-
-        LOG(INFO)<<"Creating pointSift net ...";
-        mp_point_sift = new Point_sift_segmentation(point_size,cls_num,freq,minp,maxp);
-
-        std::string	graph_file =  graph;
-        std::string	cpkt_file =  cpkt;
-        LOG(INFO)<<"init  pointSift net graph:"<<graph;
-        code=mp_point_sift->init(graph_file,cpkt_file);
-        if(code!=SUCCESS)
-        {
-            LOG(ERROR)<<code.to_string();
-            return code;
-        }
-    }
-
-    {
-        LOG(INFO)<<"Creating darknet ...";
-        float min_x = parameter.yolo_parameter().min_x();
-        float max_x = parameter.yolo_parameter().max_x();
-        float min_y = parameter.yolo_parameter().min_y();
-        float max_y = parameter.yolo_parameter().max_y();
-        float freq = parameter.yolo_parameter().freq();
-        std::string cfg = parameter.yolo_parameter().cfg();
-        std::string weights = parameter.yolo_parameter().weights();
-        mp_yolo_detector = new YoloDetector(cfg, weights, min_x, max_x, min_y, max_y, freq);
-
-    }
-
-    int l = parameter.net_3dcnn_parameter().length();
-    int w = parameter.net_3dcnn_parameter().width();
-    int h = parameter.net_3dcnn_parameter().height();
-    int nClass = parameter.net_3dcnn_parameter().nclass();
-    int freq = parameter.net_3dcnn_parameter().freq();
-
-    mp_cnn3d = new Cnn3d_segmentation(l, w, h, freq, nClass);
-
-    LOG(INFO)<<"Init 3dcnn ...";
-    std::string	weights = parameter.net_3dcnn_parameter().weights_file();
-    code=mp_cnn3d->init(weights);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    return SUCCESS;
-
-}
-
-Error_manager Locater::execute_task(Task_Base* task,double time_out)
-{
-    LOG(INFO)<<"NEW LOCATE TASK ==============================================================================";
-    Locate_information locate_information;
-    locate_information.locate_correct=false;
-
-    //判断task是否合法
-    if(task==0) return LOCATER_TASK_ERROR;
-    if(task->get_task_type()!=LOCATE_TASK)
-    {
-        task->update_statu(TASK_OVER,"locate task type error");
-        return  Error_manager(LOCATER_TASK_ERROR,NORMAL,"locate Task type error");
-    }
-    Locate_task* locate_task=(Locate_task*)task;
-    locate_task->update_statu(TASK_SIGNED);
-    Error_manager code;
-    ///第一步,获取task中input点云,文件保存路径
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_input=locate_task->get_locate_cloud();
-    std::string save_path=locate_task->get_save_path();
-    if(t_cloud_input.get()==0)
-    {
-        locate_task->update_statu(TASK_OVER,"Task set cloud is uninit");
-        return Error_manager(LOCATER_INPUT_CLOUD_UNINIT,NORMAL,"Task set cloud is uninit");
-    }
-    //第二步,加锁,更新task状态,调用locate,进入测量中
-    std::lock_guard<std::mutex> t_lock(m_mutex_lock);
-    locate_task->update_statu(TASK_WORKING);
-    code=locate(t_cloud_input,locate_information,save_path);
-    locate_task->set_locate_information(locate_information);
-    if(code!=SUCCESS)
-    {
-        locate_task->update_statu(TASK_OVER,"Locate Failed");
-        return code;
-    }
-    locate_task->update_statu(TASK_OVER,"Locate OK");
-    return code;
-
-}
-
-Error_manager Locater::locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
-                       std::vector<YoloBox>& boxes, std::string work_dir)
-{
-
-    if(cloud_in.get()==0)
-    {
-        return Error_manager(LOCATER_YOLO_INPUT_CLOUD_UNINIT,NORMAL,"yolo input cloud uninit");
-    }
-    if(cloud_in->size()==0)
-    {
-        return Error_manager(LOCATER_INPUT_YOLO_CLOUD_EMPTY,NORMAL,"locate_yolo input cloud empty");
-    }
-    if(mp_yolo_detector==0)
-    {
-        return Error_manager(LOCATER_YOLO_UNINIT,NORMAL,"locate_yolo yolo is not init");
-    }
-    //1,根据点云区域,调整yolo边界参数,边界参数必须保证长宽 1:1
-    pcl::PointXYZ min_point,max_point;
-    pcl::getMinMax3D(*cloud_in,min_point,max_point);
-    float yolo_minx=min_point.x;
-    float yolo_maxx=max_point.x;
-    float yolo_miny=min_point.y;
-    float yolo_maxy=max_point.y;
-    float length=yolo_maxx-yolo_minx;
-    float width=yolo_maxy-yolo_miny;
-    if(length>2*width) yolo_maxy=(length/2.0)+yolo_miny;
-    else if(length<2*width) yolo_maxx=(2*width)+yolo_minx;
-
-    Error_manager code=mp_yolo_detector->set_region(yolo_minx,yolo_maxx,yolo_miny,
-        yolo_maxy,m_locate_parameter.yolo_parameter().freq());
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    //2,识别车辆位置
-    boxes.clear();
-    return mp_yolo_detector->detect(cloud_in,boxes,work_dir);
-
-}
-
-Error_manager Locater::locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox> boxes,
-                                   pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_wheel,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_car,
-                                   std::string work_dir)
-{
-    if(cloud_in.get()==0)
-    {
-        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,NORMAL,"sift input cloud uninit");
-    }
-    if(cloud_in->size()==0)
-    {
-        return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,NORMAL,"locate_sift input cloud empty");
-    }
-    if(mp_point_sift==0)
-    {
-        return Error_manager(LOCATER_POINTSIFT_UNINIT,NORMAL,"Point Sift unInit");
-    }
-    //第一步,根据点云边界调整PointSIft 参数
-    pcl::PointXYZ min_point,max_point;
-    pcl::getMinMax3D(*cloud_in,min_point,max_point);
-    Error_manager code ;
-    code=mp_point_sift->set_region(min_point,max_point);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    //第二步,点云抽稀,抽稀后的点云大约在8192左右,
-    // 如果有多辆车(多个yolo框),取第一个框
-    YoloBox yolo_box=boxes[0];
-    pcl::PointXYZ minp, maxp;
-    minp.x = yolo_box.x;
-    minp.y = yolo_box.y;
-    maxp.x = minp.x + yolo_box.w;
-    maxp.y = minp.y + yolo_box.h;
-
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_input_sift(new pcl::PointCloud<pcl::PointXYZ>());
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsample(new pcl::PointCloud<pcl::PointXYZ>());
-    ///	///体素网格 下采样
-    pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
-    vgfilter.setInputCloud(cloud_in);
-    vgfilter.setLeafSize(60.f, 60.f, 50.f);
-    vgfilter.filter(*cloud_downsample);
-    //限制 x y
-    pcl::PassThrough<pcl::PointXYZ> passX;
-    pcl::PassThrough<pcl::PointXYZ> passY;
-    pcl::PassThrough<pcl::PointXYZ> passZ;
-    passX.setInputCloud(cloud_downsample);
-    passX.setFilterFieldName("x");
-    passX.setFilterLimits(minp.x, maxp.x);
-    passX.filter(*cloud_in);
-
-    passY.setInputCloud(cloud_in);
-    passY.setFilterFieldName("y");
-    passY.setFilterLimits(minp.y, maxp.y);
-    passY.filter(*cloud_in);
-
-    passY.setInputCloud(cloud_in);
-    passY.setFilterFieldName("z");
-    passY.setFilterLimits(40, 2000);
-    passY.filter(*cloud_in);
-
-    //第三步,分割车辆点云
-    std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
-    code=mp_point_sift->seg(cloud_in, segmentation_clouds, work_dir);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    if(segmentation_clouds[0]->size()==0||segmentation_clouds[2]->size()==0)
-    {
-        return Error_manager(LOCATER_SIFT_PREDICT_NO_CAR_POINT,NORMAL,"sift predict no car or wheel");
-    }
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
-    //第0类即是轮胎点云,第二类为车身点云
-    pcl::copyPointCloud(*segmentation_clouds[0], *t_cloud_wheel);
-    pcl::copyPointCloud(*segmentation_clouds[2], *t_cloud_car);
-    cloud_wheel=t_cloud_wheel;
-    cloud_car=t_cloud_car;
-    return SUCCESS;
-}
-
-Error_manager Locater::locate_wheel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
-                                                float& center_x,float& center_y,
-                                                float& wheel_base,float& width,float& angle,
-                                    std::string work_dir)
-{
-    if(cloud.get()==0)
-    {
-        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_UNINIT,NORMAL,"3dcnn input cloud uninit");
-    }
-    if(cloud->size()==0)
-    {
-        return Error_manager(LOCATER_3DCNN_INPUT_CLOUD_EMPTY,NORMAL,"locate_3dcnn input cloud empty");
-    }
-    if(mp_cnn3d==0)
-    {
-        return Error_manager(LOCATER_3DCNN_UNINIT,NORMAL,"locate_wheel 3dcnn is not init");
-    }
-    //识别车轮
-    Error_manager code;
-
-    code=mp_cnn3d->predict(cloud,center_x,center_y,wheel_base,width,angle,work_dir);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    //根据plc限制判断合法性
-    if(center_y>=930 || center_y<=200)
-    {
-        char description[255]={0};
-        sprintf(description,"Y is out of range by plc (200  , 900)  Y:%.2f",center_y);
-        return Error_manager(LOCATER_Y_OUT_RANGE_BY_PLC,NORMAL,description);
-    }
-    return SUCCESS;
-
-
-}
-
-Error_manager Locater::measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height)
-{
-    if(cloud_car.get()==0)
-    {
-        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_UNINIT,NORMAL,"measure height input cloud uninit");
-    }
-    if(cloud_car->size()==0)
-    {
-        return Error_manager(LOCATER_MEASURE_HEIGHT_CLOUD_EMPTY,NORMAL,"measure height input cloud is empty");
-    }
-    pcl::PointXYZ min_point,max_point;
-    pcl::getMinMax3D(*cloud_car,min_point,max_point);
-    //限制车高的范围,检验结果
-    height=max_point.z;
-
-    return SUCCESS;
-}
-
-void Locater::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);
-}
-
-Error_manager Locater::locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src,
-                  Locate_information& locate_information, std::string work_dir)
-{
-    if(cloud_src.get()==0)
-    {
-        return Error_manager(LOCATER_INPUT_CLOUD_UNINIT,NORMAL,"locate input cloud is not init");
-    }
-    if(cloud_src->size()==0) {
-        return Error_manager(LOCATER_INPUT_CLOUD_EMPTY,NORMAL,"input cloud is empty");
-    }
-    //10mm网格过滤
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
-    ///	///体素网格 下采样
-    pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
-    vgfilter.setInputCloud(cloud_src);
-    vgfilter.setLeafSize(30.f, 30.f, 30.f);
-    vgfilter.filter(*cloud_in);
-    //保存原始点云 src.txt
-    std::string src_cloud_file=work_dir+"/src.txt";
-    save_cloud_txt(cloud_in,src_cloud_file);
-    //置初始结果为错误
-    locate_information.locate_correct=false;
-    Error_manager code;
-    //第一步 yolo定位车辆外接矩形
-    std::vector<YoloBox> t_yolo_boxes;
-    code=locate_yolo(cloud_in,t_yolo_boxes,work_dir);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-
-    //第二步 ,根据yolo框结合PointSift识别车身点云,第0类即轮胎,第二类为车身
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_wheel(new pcl::PointCloud<pcl::PointXYZ>);
-    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car(new pcl::PointCloud<pcl::PointXYZ>);
-    code=locate_sift(cloud_in,t_yolo_boxes,cloud_wheel,cloud_car,work_dir);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-
-    //第三步,计算车辆高度
-    float height_car=0;
-    code=measure_height(cloud_car,height_car);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-    locate_information.locate_hight=height_car;
-
-    //第四步,识别轮胎,并根据轮胎数量计算旋转矩形
-    float center_x;
-    float center_y;
-    float wheel_base;
-    float width;
-    float angle;
-    code=locate_wheel(cloud_wheel,center_x,center_y,wheel_base,width,angle,work_dir);
-    if(code!=SUCCESS)
-    {
-        return code;
-    }
-
-    //第六步,结果赋值
-    //角度以度为单位
-    locate_information.locate_x=center_x;
-    locate_information.locate_y=center_y;
-    locate_information.locate_theta = angle;
-    locate_information.locate_length=wheel_base;
-    locate_information.locate_width=width;
-    locate_information.locate_hight=height_car;
-    locate_information.locate_wheel_base=wheel_base;
-    locate_information.locate_correct=true;
-    LOG(INFO) << "Locate SUCCESS :" << " center:" << "[" << locate_information.locate_x
-                << "," << locate_information.locate_y<< "] size= "
-              << locate_information.locate_length << ", "
-              << locate_information.locate_width << "," << locate_information.locate_hight
-              << "  angle=" << locate_information.locate_theta ;
-
-    return SUCCESS;
-}

locate/locater.h

@@ -1,70 +0,0 @@
-//
-// Created by zx on 2019/12/30.
-//
-
-#ifndef LOCATER_H
-#define LOCATER_H
-#include "locate_task.h"
-#include "locater_parameter.pb.h"
-#include "../error_code/error_code.h"
-#include "point_sift_segmentation.h"
-#include "YoloDetector.h"
-#include "cnn3d_segmentation.h"
-#include <mutex>
-
-
-class Locater
-{
-public:
-    Locater();
-    ~Locater();
-    //初始化测量算法参数
-    Error_manager init(Measure::LocateParameter parameter);
-    //获取测量模块状态(错误码);返回是否能执行任务,以及不能执行任务的原因
-    Error_manager get_error();
-    //执行任务,
-    //task:测量任务
-    //time_out:超时时间,单位秒
-    Error_manager execute_task(Task_Base* task,double time_out=5);
-protected:
-    //测量算法函数
-    //cloud_in:输入点云
-    //locate_information:测量结果
-    //work_dir:中间文件保存路径
-    Error_manager locate(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
-        Locate_information& locate_information, std::string work_dir);
-    //yolo定位车辆外接矩形
-    //cloud_in:输入点云,定位算法内部不会修改点云内容
-    //boxes:定位到的外接box框
-    //work_dir:中间文件输出路径
-    Error_manager locate_yolo(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
-        std::vector<YoloBox>& boxes, std::string work_dir);
-    //根据yolo识别结果, 利用PointSift从场景中分割车辆点云
-    //cloud:输入点云
-    //boxes:yolo识别结果
-    //cloud_target:车辆点云
-    //work_dir:中间文件保存路径
-    Error_manager locate_sift(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<YoloBox> boxes,
-                           pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_wheel,pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_car,
-                           std::string work_dir);
-    //输入汽车点云,识别轮胎,并计算轴长,宽,中心,旋转角
-    Error_manager locate_wheel(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,float& center_x,float& center_y,
-                                           float& wheel_base,float& width,float& angle,std::string work_dir);
-    //根据汽车点云计算车高
-    //cloud_car:车辆点云
-    Error_manager measure_height(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_car,float& height);
-    /*
-     * 保存点云成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;
-    YoloDetector*                           mp_yolo_detector;
-    Cnn3d_segmentation*                     mp_cnn3d;
-
-    std::mutex						        m_mutex_lock;
-};
-
-
-#endif //LOCATER_H

locate/locater_parameter.proto

@@ -1,50 +0,0 @@
-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 int64 length=1 [default=224];
-    optional int64 width=2 [default=224];
-    optional int64 height=3 [default=96];
-    optional int64 freq=4 [default=25];
-    optional int64 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/point_sift_segmentation.cpp

@@ -2,6 +2,13 @@
 #include <glog/logging.h>
 #include <fstream>
 
+
+#include <time.h>
+#include <sys/time.h>
+#include <chrono>
+using namespace std;
+using namespace chrono;
+
 void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& pCloud)
 {
 	std::ofstream os;
@@ -55,6 +62,9 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
     //创建空数据,第一次初始化后空跑
 	float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
 	float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));
+
+	auto start   = system_clock::now();
+
 	if (false == Predict(cloud_data, output))
 	{
 		free(cloud_data);
@@ -69,6 +79,12 @@ Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
 		free(output);
 
 	}
+	auto end   = system_clock::now();
+	auto duration = duration_cast<microseconds>(end - start);
+	cout <<  "花费了"
+		 << double(duration.count()) * microseconds::period::num / microseconds::period::den   << "秒" << endl;
+
+
 	return SUCCESS;
 }
 
@@ -137,7 +153,7 @@ bool Point_sift_segmentation::Create_data(pcl::PointCloud<pcl::PointXYZ>::Ptr cl
 
 	if (cloud_select->points.size() != m_point_num)
 	{
-		LOG(ERROR) << "\tpointSIFT input select cloud is not " << m_point_num;
+//		LOG(ERROR) << "\tpointSIFT input select cloud is not " << m_point_num;
 		return false;
 	}
 
@@ -162,7 +178,7 @@ Error_manager Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr c
 	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;
+	//LOG(INFO) << "cloud size:" << cloud->size()<<"  /  "<<m_point_num;
 	if (!Create_data(cloud, data))
 	{
 		free(data);
@@ -170,12 +186,29 @@ Error_manager Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr c
 		return Error_manager(LOCATER_SIFT_CREATE_INPUT_DATA_FAILED,NORMAL,"pointSIFT Create input data 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 ;
+//	}
+
+	auto start = system_clock::now();
+
 	if (!Predict(data, output))
 	{
 		free(data);
 		free(output);
 		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,NORMAL,"pointSIFT predict ERROR");
 	}
+	auto end   = system_clock::now();
+	auto duration = duration_cast<microseconds>(end - start);
+	cout <<  "花费了"
+		 << double(duration.count()) * microseconds::period::num / microseconds::period::den   << "秒" << endl;
+	cout <<  double(duration.count()) << " "
+		 <<   microseconds::period::num << " " << microseconds::period::den   << "秒" << endl;
+
 	////������
 	RecoveryCloud(output, data, cloud_seg);
 	if (!FilterObs(cloud_seg,save_dir))
@@ -198,7 +231,7 @@ Error_manager Point_sift_segmentation::seg(pcl::PointCloud<pcl::PointXYZ>::Ptr c
 	{
 		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 };
@@ -222,9 +255,9 @@ bool Point_sift_segmentation::RecoveryCloud(float* output, float* cloud, std::ve
 	for (int i = 0; i < m_point_num; ++i)
 	{
 		pcl::PointXYZ point;
-		point.x = *(cloud + i * 3)*1000.+center.x;
-		point.y = *(cloud + i * 3 + 1)*1000.+center.y;
-		point.z = *(cloud + i * 3 + 2)*1000.;
+		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)

locate/tf_wheel_3Dcnn_api.h

@@ -1,30 +0,0 @@
-/*
- * 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

@@ -3,8 +3,13 @@
 //
 #include <iostream>
 
+#include "./laser/laser_manager.h"
 #include "./laser/Laser.h"
 #include "./laser/LivoxLaser.h"
+#include "./locate/point_sift_segmentation.h"
+#include <iostream>
+#include <fstream>
+
 #include "./error_code/error_code.h"
 #include "LogFiles.h"
 #include <string.h>
@@ -42,76 +47,24 @@ bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
 
 int main(int argc,char* argv[])
 {
-
-	std::cout << "huli 101 main start!" << std::endl;
-
-
-	Laser_base * m_laser_vector_array[LIVOX_NUMBER];
 	Error_manager err;
-	std::cout << err << std::endl;
-	std::string asd = "";
-	std::cout << asd.empty() << std::endl;
-
+	Error_manager code ;
 
-	for (int i = 0; i < LIVOX_NUMBER; ++i)
-	{
-		Laser_base *m_laser_vector;
-
-		Laser_proto::laser_parameter laser_param;
-		laser_param.set_frame_num(1000);
-		laser_param.set_type("Livox");
-		if ( i==0 )
-		{
-			laser_param.set_sn("0TFDFG700601881");
-		}
-		else if ( i==1 )
-		{
-			laser_param.set_sn("0TFDFCE00502001");
-		}
-
-//	int i = 0;
-		m_laser_vector_array[i] = m_laser_vector = LaserRegistory::CreateLaser(laser_param.type(), i,
-																			   laser_param);
-
-
-		if (m_laser_vector != NULL)
-		{
-			err = m_laser_vector->connect_laser();
-			if ( err != Error_code::SUCCESS)
-			{
-				char description[255] = {0};
-				sprintf(description, "Laser %d connect failed...", i);
-				err.error_manager_reset(LASER_CONNECT_FAILED, NORMAL, description);
-
-			}
-			else
-			{
-				std::cout << "huli 301 connect_laser Error_code::SUCCESS" << std::endl;
-			}
-
-			cout << "huli 311 " << err.to_string() << endl;
-
-
-
-
-		}
-		else
-		{
-			cout << "huli: 601 :" << err.to_string() << endl;
+	std::cout << "huli 101 main start!" << std::endl;
 
-		}
+	Laser_manager * tp_laser_manager = Laser_manager::get_instance_pointer();
+	tp_laser_manager->laser_manager_init();
 
-	}
 
+	Point_sift_segmentation*                mp_point_sift;
+	int point_size = 8192;
+	int cls_num = 3;
+	double freq = 20.0;
+	std::string graph = "../model_param/seg_model_404500.ckpt.meta";
+	std::string cpkt = "../model_param/seg_model_404500.ckpt";
+	pcl::PointXYZ minp(-10000,-10000,-10000);
+	pcl::PointXYZ maxp(10000,10000,10000);
 
-	if (!Start()) {
-		Uninit();
-		printf("Livox-SDK init fail!\n");
-	}
-	else
-	{
-		std::cout << "huli 201 Livox-SDK start" << std::endl;
-	}
 
 
 
@@ -119,24 +72,24 @@ int main(int argc,char* argv[])
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		std::mutex cloud_lock;
+		std::vector<int> select_laser_id_vector;
+
+		Laser_manager_task * laser_manager_task = new Laser_manager_task;
+		laser_manager_task->task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
+									  1000,true,"../data/laser_data",&cloud_lock,scan_cloud,
+									  true,select_laser_id_vector	);
 
-		Laser_task *laser_task = new Laser_task();
-		//
-		laser_task->task_init(TASK_CREATED, scan_cloud, &cloud_lock);
-		laser_task->set_task_frame_maxnum(1000);
 
-//		//保存文件初始化
-//		char time_string[256] = { 0 };
-//		sprintf(time_string, "../data/%d/", 0);
-//
-//		laser_task->set_task_save_path(time_string);
 
 		std::cout << "huli 401 connect_laser Error_code::SUCCESS" << std::endl;
 
 		std::cout << " press to start" << std::endl;
 		char ch ;
 //		= getchar();
+
+		std::this_thread::sleep_for(std::chrono::seconds(2));
 		std::cin >> ch ;
+
 		if ( ch == 'b' )
 		{
 			std::cout << "  end scan ------------" << std::endl;
@@ -144,60 +97,43 @@ int main(int argc,char* argv[])
 		}
 		std::cout << "  start scan ------------" << std::endl;
 
-//		err = m_laser_vector_array[0]->execute_task(laser_task);
-//		usleep(3000 * 1000);
-//		cout << "huli: 501 :" << err.to_string() << endl;
-		int n = 5;
+		int n = 1;
 		while(n)
 		{
 			n--;
 
-			for (int i = 0; i < LIVOX_NUMBER; ++i)
-			{
-				char time_string[256] = { 0 };
-				sprintf(time_string, "../data/%d/", i);
-				laser_task->set_task_save_flag_and_path(true,time_string);
-				err = m_laser_vector_array[i]->execute_task(laser_task);
-//				usleep(3000 * 1000);
-			}
+			task_command_manager::get_instance_references().execute_task(laser_manager_task);
+//			tp_laser_manager->execute_task(laser_manager_task);
 
-			usleep(3000 * 1000);
+			usleep(5000 * 1000);
 			cout << "huli: 501 :" << err.to_string() << endl;
 			cout << "huli: 501 :" << err.to_string() << endl;
 		}
 
 		cout << "huli: 601 :" << err.to_string() << endl;
 		//发送任务单给雷达
-		std::this_thread::sleep_for(std::chrono::seconds(5));
-
-	}
-
-
+		std::this_thread::sleep_for(std::chrono::seconds(2));
 
+		delete(laser_manager_task);
 
-	for (int i = 0; i < LIVOX_NUMBER; ++i)
-	{
-		std::this_thread::sleep_for(std::chrono::seconds(5));
-		cout << "huli: 701 :" << err.to_string() << endl;
 
-		m_laser_vector_array[i]->disconnect_laser();
-		cout << "huli: 801 :" << err.to_string() << endl;
+		//locate
+		pcl::getMinMax3D(*scan_cloud,minp,maxp);
+		std::cout << "huli 112" << std::endl;
+		mp_point_sift = new Point_sift_segmentation(point_size,cls_num,freq,minp,maxp);
+		std::cout << code.to_string() << std::endl;
 
-		m_laser_vector_array[i]->close_save_path();
-		cout << "huli: 901 :" << err.to_string() << endl;
+		code = mp_point_sift->init(graph,cpkt);
+		std::cout << code.to_string() << std::endl;
 
-		std::this_thread::sleep_for(std::chrono::seconds(2));
+		std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> segmentation_clouds;
+		code = mp_point_sift->seg(scan_cloud, segmentation_clouds, "../data/locate_data");
+		std::cout << code.to_string() << std::endl;
 
 	}
 
-	Uninit();
-
+	tp_laser_manager->laser_manager_uninit();
 	cout << "huli 1234 main end" << endl;
 }
 
 
-
-
-
-
-

setting/laser.prototxt

@@ -0,0 +1,42 @@
+#1号雷达
+#0TFDFG700601881
+
+
+laser_parameters
+{
+	type:"Livox"
+	sn:"0TFDFCE00502001"
+	frame_num:1000
+    mat_r00:1
+    mat_r01:0
+    mat_r02:0
+    mat_r03:0
+    mat_r10:0
+    mat_r11:1
+    mat_r12:0
+    mat_r13:0
+    mat_r20:0
+    mat_r21:0
+    mat_r22:1
+    mat_r23:0
+}
+
+
+laser_parameters
+{
+	type:"Livox"
+	sn:"0TFDFG700601881"
+	frame_num:1000
+    mat_r00:1
+    mat_r01:0
+    mat_r02:0
+    mat_r03:0
+    mat_r10:0
+    mat_r11:1
+    mat_r12:0
+    mat_r13:0
+    mat_r20:0
+    mat_r21:0
+    mat_r22:1
+    mat_r23:0
+}

task/task_base.cpp

@@ -0,0 +1,110 @@
+//
+// Created by zx on 2019/12/28.
+//
+
+#include "task_base.h"
+#include "../error_code/error_code.h"
+
+Task_Base::Task_Base()
+{
+    m_task_type = UNKNOW_TASK;
+	m_task_statu = TASK_CREATED;
+	mp_tast_receiver = NULL;
+
+	m_task_start_time = std::chrono::system_clock::now();	//获取当前时间
+	m_task_over_time = std::chrono::milliseconds(TASK_OVER_TIME_DEFAULT); //默认10秒
+}
+Task_Base::~Task_Base()
+{
+	mp_tast_receiver = NULL;
+}
+
+
+//更新任务单
+//task_statu: 任务状态
+//statu_information:状态说明
+Error_manager Task_Base::update_statu(Task_statu task_statu,std::string statu_information)
+{
+    m_task_statu=task_statu;
+    m_task_statu_information=statu_information;
+    return SUCCESS;
+}
+
+//判断是否超时。返回true表示任务超时，返回false表示任务没有超时
+bool Task_Base::is_over_time()
+{
+	return (std::chrono::system_clock::now() - m_task_start_time) > m_task_over_time;
+}
+
+
+
+//获取任务类型
+Task_type Task_Base::get_task_type()
+{
+    return m_task_type;
+}
+//获取任务单状态
+Task_statu  Task_Base::get_task_statu()
+{
+    return m_task_statu;
+}
+//设置 任务单状态
+void  Task_Base::set_task_statu(Task_statu task_statu)
+{
+	m_task_statu = task_statu;
+}
+//获取状态说明
+std::string Task_Base::get_task_statu_information()
+{
+    return m_task_statu_information;
+}
+//设置 状态说明
+void Task_Base::set_task_statu_information(std::string task_statu_information)
+{
+	m_task_statu_information = task_statu_information;
+}
+//获取 错误码，返回引用。
+Error_manager& Task_Base::get_task_error_manager()
+{
+	return m_task_error_manager;
+}
+//设置 错误码
+void Task_Base::set_task_error_manager(Error_manager & error_manager)
+{
+	m_task_error_manager = error_manager;
+}
+//获取任务接收方
+void * Task_Base::get_tast_receiver()
+{
+	return mp_tast_receiver;
+}
+//设置任务接收方
+void Task_Base::set_tast_receiver(void * p_tast_receiver)
+{
+	mp_tast_receiver = p_tast_receiver;
+}
+
+
+//获取 任务创建的时间点
+std::chrono::system_clock::time_point Task_Base::get_task_start_time()
+{
+	return m_task_start_time;
+}
+//设置 任务创建的时间点
+void Task_Base::set_task_start_time(std::chrono::system_clock::time_point task_start_time)
+{
+	m_task_start_time = task_start_time;
+}
+//获取 任务超时的时限
+std::chrono::milliseconds	Task_Base::get_task_over_time()
+{
+	return m_task_over_time;
+}
+//设置 任务超时的时限
+void	Task_Base::get_task_over_time(std::chrono::milliseconds task_over_time)
+{
+	m_task_over_time = task_over_time;
+}
+
+
+

task/task_base.h

@@ -0,0 +1,107 @@
+/*
+ * Task_Base 是任务基类，用作不同的模块之间的通信载体。
+ *	每一个模块创建一个任务子类，从Task_Base继承。
+ *	然后任务子类自定义一些数据和读写数据的接口函数。
+ *	然后在任务接受方实现 execute_task(Task_Base* p_laser_task)
+ * */
+
+#ifndef TASK_BASE_H
+#define TASK_BASE_H
+#include <string>
+#include "../error_code/error_code.h"
+#include <chrono>
+
+//任务超时时间默认值10000ms，10秒
+#define TASK_OVER_TIME_DEFAULT				10000
+
+//任务类型
+enum Task_type
+{
+	UNKNOW_TASK             =0,				//未知任务单//初始化,默认值
+	LASER_MANGER_SCAN_TASK  =1,             //雷达管理模块的扫描任务,
+	LASER_BASE_SCAN_TASK    =2,             //单个雷达的扫描任务,
+    LOCATE_TASK             =3,             //测量任务
+    PLC_TASK                =4,             //上传PLC任务
+
+    WJ_TASK,
+    
+};
+//任务状态，如果任务故障，任务状态改为TASK_OVER，然后在m_task_error_manager 补充错误码。
+enum Task_statu
+{
+    TASK_CREATED            =0,             //创建状态，默认值
+    TASK_SIGNED             =1,             //已签收
+    TASK_WORKING            =2,             //处理中
+    TASK_OVER               =3,             //已结束
+
+	TASK_ERROR              =11,             //任务错误
+	TASK_DEAD               =12,             //任务死亡，需要销毁
+
+};
+
+//任务单基类
+class Task_Base
+{
+protected:
+	//不允许构造基类，只允许子类构造，（多态）
+	Task_Base();
+public:
+    ~Task_Base();
+
+    //更新任务单
+    //task_statu: 任务状态
+    //statu_information:状态说明
+    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
+
+    //判断是否超时。返回true表示任务超时，返回false表示任务没有超时
+    bool is_over_time();
+
+public:
+    //获取 任务类型
+    Task_type   get_task_type();
+	//设置 任务类型
+//	void   set_task_type(Task_type task_type) = delete;
+    //获取 任务单状态
+    Task_statu  get_task_statu();
+	//设置 任务单状态
+	void  set_task_statu(Task_statu task_statu);
+    //获取 状态说明
+    std::string get_task_statu_information();
+	//设置 状态说明
+	void set_task_statu_information(std::string task_statu_information);
+	//获取 错误码，返回引用。
+	Error_manager& get_task_error_manager();
+	//设置 错误码
+	void set_task_error_manager(Error_manager & error_manager);
+
+	//获取任务接收方
+	void * get_tast_receiver();
+	//设置任务接收方
+	void set_tast_receiver(void * p_tast_receiver);
+	//获取 任务创建的时间点
+	std::chrono::system_clock::time_point get_task_start_time();
+	//设置 任务创建的时间点
+	void set_task_start_time(std::chrono::system_clock::time_point task_start_time);
+	//获取 任务超时的时限
+	std::chrono::milliseconds	get_task_over_time();
+	//设置 任务超时的时限
+	void	get_task_over_time(std::chrono::milliseconds task_over_time);
+
+
+protected:
+    Task_type                   m_task_type;					//任务类型，不允许中途修改
+    Task_statu                  m_task_statu;					//任务状态
+    std::string					m_task_statu_information;		//任务状态说明
+    void*						mp_tast_receiver;				//任务接收方，Task_Base并不分配和释放内存。
+    //注：mp_tast_receiver是可选的，可以为NULL。如果为NULL，则需要task_command_manager去找到接收对象。
+
+	std::chrono::system_clock::time_point 	m_task_start_time;	//任务创建的时间点
+	std::chrono::milliseconds	m_task_over_time;				//任务超时的时限
+	//注：std::chrono::system_clock::now();	//获取当前时间
+
+	//错误码，任务故障信息，任务输出
+	Error_manager               m_task_error_manager;
+};
+
+#endif //TASK_BASE_H
+

task/task_base.puml

@@ -0,0 +1,80 @@
+@startuml
+@startuml
+skinparam classAttributeIconSize 0
+
+
+title  Task_Base 任务单基类
+
+
+
+
+enum Task_type
+{
+//任务类型
+	UNKNOW_TASK             =0,				//未知任务单//初始化,默认值
+	LASER_TASK              =1,             //雷达扫描任务,
+    LOCATE_TASK             =2,             //测量任务
+    PLC_TASK                =3,             //上传PLC任务
+}
+
+
+enum Task_statu
+{
+//任务状态，如果任务故障，任务状态改为TASK_OVER，然后在m_task_error_manager 补充错误码。
+    TASK_CREATED            =0,             //创建状态，默认值
+    TASK_SIGNED             =1,             //已签收
+    TASK_WORKING            =2,             //处理中
+    TASK_OVER               =3,             //已结束
+}
+
+
+
+
+class Task_Base
+{
+//任务单基类
+==public:==
+    ~Task_Base();
+..
+    //初始化任务单,初始任务单类型为 UNKONW_TASK
+    virtual Error_manager init();
+..
+    //更新任务单
+    //task_statu: 任务状态
+    //statu_information:状态说明
+    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
+..
+    //获取任务类型
+    Task_type   get_task_type();
+..
+    //获取任务单状态
+    Task_statu  get_statu();
+..
+    //获取状态说明
+    std::string get_statu_information();
+..
+	//获取 错误码
+	Error_manager& get_task_error_manager();
+..
+	//设置 错误码
+	void set_task_error_manager(Error_manager & error_manager);
+==protected:==
+    Task_Base();
+==protected:==
+    Task_type                   m_task_type;                    //任务类型
+    Task_statu                  m_task_statu;                   //任务状态
+    std::string                 m_task_statu_information;       //任务状态说明
+	//错误码，任务故障信息，任务输出
+	Error_manager               m_task_error_manager;
+}
+
+class Error_manager
+{
+//错误码管理
+}
+Task_Base <-- Error_manager : include
+
+
+Task_Base <-- Task_type : include
+Task_Base <-- Task_statu : include
+@enduml

task/task_command_manager.cpp

@@ -1,60 +1,77 @@
-//
-// Created by zx on 2019/12/28.
-//
+
+
 
 #include "task_command_manager.h"
-#include "../error_code/error_code.h"
+#include "../laser/Laser.h"
+#include "../laser/laser_manager.h"
 
-Task_Base::Task_Base()
-{
-    m_task_type=UNKNOW_TASK;
-}
-Task_Base::~Task_Base()
-{
 
-}
-//初始化任务单,初始任务单类型为 UNKONW_TASK
-Error_manager Task_Base::init()
-{
-    m_task_type=UNKNOW_TASK;
-    return SUCCESS;
-}
-//更新任务单
-//task_statu: 任务状态
-//statu_information:状态说明
-Error_manager Task_Base::update_statu(Task_statu task_statu,std::string statu_information)
+//对外的接口函数，所有的任务发送方，都必须使用该函数。
+//execute_task在内部解析了Task_Base里面的Task_type，然后转发给具体某个模块的实例对象。
+//input：p_task_base 任务单，基类的指针，指向子类的实例，（多态）
+Error_manager task_command_manager::execute_task(Task_Base* p_task_base)
 {
-    m_task_statu=task_statu;
-    m_task_statu_information=statu_information;
-    return SUCCESS;
-}
-//获取任务类型
-Task_type Task_Base::get_task_type()
-{
-    return m_task_type;
-}
-//获取任务单状态
-Task_statu  Task_Base::get_statu()
-{
-    return m_task_statu;
-}
-//获取状态说明
-std::string Task_Base::get_statu_information()
-{
-    return m_task_statu_information;
-}
+	Error_manager t_error;
+	void * tp_tast_receiver = p_task_base->get_tast_receiver();
+	switch ( p_task_base->get_task_type() )
+	{
+	    case UNKNOW_TASK:
+			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;
+		case LASER_MANGER_SCAN_TASK:
+			if ( tp_tast_receiver != NULL )
+			{
+				((Laser_manager*)tp_tast_receiver)->execute_task(p_task_base);
+			}
+			else
+			{
+				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);
+			}
+			else
+			{
+				return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+									" fun error ");
+	        }
+	        break;
+		case LOCATE_TASK:
+			;
+			break;
+		case PLC_TASK:
+			;
+			break;
+		case WJ_TASK:
+			;
+			break;
+	    default:
 
-//获取 错误码
-Error_manager& Task_Base::get_task_error_manager()
-{
-	return m_task_error_manager;
-}
-//设置 错误码
-void Task_Base::set_task_error_manager(Error_manager & error_manager)
-{
-	m_task_error_manager = error_manager;
+	        break;
+	}
+
+	return t_error;
 }
 
 
 
 
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

task/task_command_manager.h

@@ -1,61 +1,42 @@
-//
-// Created by zx on 2019/12/28.
-//
+/*
+ * task_command_manager 是任务单的总管理，单例模式
+ * 负责管理任务单的派送和转发
+ * 所有的任务发送方都只需要调用 task_command_manager.get_instance_references().execute_task(p_task_base)
+ * 然后task_command_manager去找到对应的接受对象，来调用该对象的接口函数。 例如Laser_base::execute_task
+ * 这样发送方和接收方不直接绑定，双方完全独立。
+ * 没有在实例里面保存对方的回调函数或者对象指针
+ *
+ * */
 
 #ifndef TASK_COMAND_MANAGER_H
 #define TASK_COMAND_MANAGER_H
 #include <string>
 #include "../error_code/error_code.h"
+#include "../task/task_base.h"
+#include "../tool/singleton.h"
 
-//任务类型
-enum Task_type
-{
-	UNKNOW_TASK             =0,				//未知任务单//初始化,默认值
-	LASER_TASK              =1,             //雷达扫描任务,
-    LOCATE_TASK             =2,             //测量任务
-    PLC_TASK                =3,             //上传PLC任务
 
-    WJ_TASK,
-    
-};
-//任务状态，如果任务故障，任务状态改为TASK_OVER，然后在m_task_error_manager 补充错误码。
-enum Task_statu
+class task_command_manager:public Singleton<task_command_manager>
 {
-    TASK_CREATED            =0,             //创建状态，默认值
-    TASK_SIGNED             =1,             //已签收
-    TASK_WORKING            =2,             //处理中
-    TASK_OVER               =3,             //已结束
-};
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<task_command_manager>;
+public:
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	task_command_manager(const task_command_manager&)=delete;
+	task_command_manager& operator =(const task_command_manager&)= delete;
+	~task_command_manager()=default;
+private:
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+	task_command_manager()=default;
 
-//任务单基类
-class Task_Base
-{
 public:
-    ~Task_Base();
-    //初始化任务单,初始任务单类型为 UNKONW_TASK
-    virtual Error_manager init();
-    //更新任务单
-    //task_statu: 任务状态
-    //statu_information:状态说明
-    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
-    //获取任务类型
-    Task_type   get_task_type();
-    //获取任务单状态
-    Task_statu  get_statu();
-    //获取状态说明
-    std::string get_statu_information();
-	//获取 错误码
-	Error_manager& get_task_error_manager();
-	//设置 错误码
-	void set_task_error_manager(Error_manager & error_manager);
-protected:
-    Task_Base();
-protected:
-    Task_type                   m_task_type;					//任务类型
-    Task_statu                  m_task_statu;					//任务状态
-    std::string					m_task_statu_information;		//任务状态说明
-	//错误码，任务故障信息，任务输出
-	Error_manager               m_task_error_manager;
+	//对外的接口函数，所有的任务发送方，都必须使用该函数。
+	//execute_task在内部解析了Task_Base里面的Task_type，然后转发给具体某个模块的实例对象。
+	//input：p_task_base 任务单，基类的指针，指向子类的实例，（多态）
+	Error_manager execute_task(Task_Base* p_task_base);
 };
 
+
+
 #endif //TASK_COMAND_MANAGER_H
+

task/task_command_manager.puml

@@ -2,81 +2,11 @@
 @startuml
 skinparam classAttributeIconSize 0
 
-<<<<<<< HEAD
-title  Task_Base 任务单基类
-=======
-title  binary_buf是二进制缓存
->>>>>>> origin/hl
 
+title  task_command_manager 任务单管理类
 
 
-enum Task_type
-{
-//任务类型
-	UNKNOW_TASK             =0,				//未知任务单//初始化,默认值
-	LASER_TASK              =1,             //雷达扫描任务,
-    LOCATE_TASK             =2,             //测量任务
-    PLC_TASK                =3,             //上传PLC任务
-}
 
 
-enum Task_statu
-{
-//任务状态，如果任务故障，任务状态改为TASK_OVER，然后在m_task_error_manager 补充错误码。
-    TASK_CREATED            =0,             //创建状态，默认值
-    TASK_SIGNED             =1,             //已签收
-    TASK_WORKING            =2,             //处理中
-    TASK_OVER               =3,             //已结束
-}
 
-
-
-
-class Task_Base
-{
-//任务单基类
-==public:==
-    ~Task_Base();
-..
-    //初始化任务单,初始任务单类型为 UNKONW_TASK
-    virtual Error_manager init();
-..
-    //更新任务单
-    //task_statu: 任务状态
-    //statu_information:状态说明
-    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
-..
-    //获取任务类型
-    Task_type   get_task_type();
-..
-    //获取任务单状态
-    Task_statu  get_statu();
-..
-    //获取状态说明
-    std::string get_statu_information();
-..
-	//获取 错误码
-	Error_manager& get_task_error_manager();
-..
-	//设置 错误码
-	void set_task_error_manager(Error_manager & error_manager);
-==protected:==
-    Task_Base();
-==protected:==
-    Task_type                   m_task_type;                    //任务类型
-    Task_statu                  m_task_statu;                   //任务状态
-    std::string                 m_task_statu_information;       //任务状态说明
-	//错误码，任务故障信息，任务输出
-	Error_manager               m_task_error_manager;
-}
-
-class Error_manager
-{
-//错误码管理
-}
-Task_Base <-- Error_manager : include
-
-
-Task_Base <-- Task_type : include
-Task_Base <-- Task_statu : include
 @enduml

tool/MeasureTopicPublisher.cpp

@@ -1,36 +0,0 @@
-//
-// Created by zx on 2019/11/25.
-//
-
-#include "MeasureTopicPublisher.h"
-#include "sys/time.h"
-#include "unistd.h"
-#include "math.h"
-MeasureTopicPublisher* MeasureTopicPublisher::g_publisher=0;
-MeasureTopicPublisher* MeasureTopicPublisher::GetInstance()
-{
-    if(g_publisher==0)
-    {
-        g_publisher=new MeasureTopicPublisher(CONNECTSTRING);
-    }
-    return g_publisher;
-}
-MeasureTopicPublisher::MeasureTopicPublisher(std::string connectStr)
-{
-    m_sock.bind(connectStr);
-    gettimeofday(&m_last_time,0);
-}
-
-bool MeasureTopicPublisher::Publish(std::string data)
-{
-    std::lock_guard<std::mutex> lk(m_lock);
-    /*double tick=0;
-    do{
-        struct timeval tm;
-        gettimeofday(&tm, 0);
-        tick=fabs((tm.tv_sec-m_last_time.tv_sec)*1000.0+(tm.tv_usec-m_last_time.tv_usec)/1000.0);
-        m_last_time=tm;
-    }while(tick<10);*/
-    usleep(100);
-    return m_sock.send(data)==data.length();
-}

tool/MeasureTopicPublisher.h

@@ -1,37 +0,0 @@
-//
-// Created by zx on 2019/11/25.
-//
-
-#ifndef MEASURETOPICSERVER_H
-#define MEASURETOPICSERVER_H
-
-#include <nnxx/message.h>
-#include <nnxx/message_control.h>
-#include <nnxx/socket.h>
-#include <nnxx/pubsub.h>
-#include <nnxx/timeout.h>
-#include <nnxx/error.h>
-#include <string>
-#include <iostream>
-#include <mutex>
-
-#define CONNECTSTRING "tcp://127.0.0.1:10080"
-
-class MeasureTopicPublisher
-{
-public:
-    static MeasureTopicPublisher* GetInstance();
-    bool Publish(std::string data);
-private:
-    MeasureTopicPublisher(std::string connectStr);
-
-private:
-    struct timeval         m_last_time;
-    nnxx::socket m_sock{nnxx::SP, nnxx::PUB};
-
-    std::mutex  m_lock;
-    static MeasureTopicPublisher* g_publisher;
-};
-
-
-#endif //MEASURETOPICSERVER_H

tool/proto_tool.cpp

@@ -0,0 +1,42 @@
+
+
+
+#include "proto_tool.h"
+#include <fcntl.h>
+#include<unistd.h>
+#include <google/protobuf/io/zero_copy_stream_impl.h>
+#include <google/protobuf/text_format.h>
+using google::protobuf::io::FileInputStream;
+using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+
+//读取protobuf 配置文件，转化为protobuf参数形式
+//input：	prototxt_path ：prototxt文件路径
+//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+bool proto_tool::read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter)
+{
+	int fd = open(prototxt_path.c_str(), O_RDONLY);
+	if (fd == -1) return false;
+	FileInputStream* input = new FileInputStream(fd);
+	bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
+	delete input;
+	close(fd);
+	return success;
+}
+
+
+
+
+
+
+
+
+
+
+
+

tool/proto_tool.h

@@ -0,0 +1,56 @@
+
+
+
+
+
+#ifndef __PROTO_TOOL_H
+#define __PROTO_TOOL_H
+
+#include "../tool/singleton.h"
+#include <istream>
+#include <google/protobuf/message.h>
+
+class proto_tool:public Singleton<proto_tool>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<proto_tool>;
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	proto_tool(const proto_tool&)=delete;
+	proto_tool& operator =(const proto_tool&)= delete;
+	~proto_tool()=default;
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	proto_tool()=default;
+
+
+public:
+	//读取protobuf 配置文件，转化为protobuf参数形式
+	//input：	prototxt_path ：prototxt文件路径
+	//ouput：	parameter: protobuf参数，这里是消息基类，实际调用时传入对应的子类即可。
+	static bool read_proto_param(std::string prototxt_path, ::google::protobuf::Message& protobuf_parameter);
+};
+
+
+
+
+#endif //__PROTO_TOOL_H
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

tool/s7_plc.cpp

@@ -1,86 +0,0 @@
-#include "s7_plc.h"
-
-S7PLC::S7PLC():bConnected_(false)
-{
-}
-S7PLC::~S7PLC()
-{
-    std::cout<<"plc start delete"<<std::endl;
-    disconnect();
-    std::cout<<"plc finish delete"<<std::endl;
-}
-
-bool S7PLC::getConnection(){
-    return bConnected_;
-}
-
-bool S7PLC::connect(std::string ip)    
-{
-    std::lock_guard<std::mutex> lck (mutex_);
-    int ret=client_.ConnectTo(ip.c_str(),0,1);
-    bConnected_=(ret==0);
-    return bConnected_;
-}
-
-bool S7PLC::ReadShorts(int DBNumber,int start,int size,short* pdata)
-{
-    short* plc_data= (short*)malloc(size*sizeof(short));
-    bool ret=read_dbs(DBNumber,start*sizeof(short),size*sizeof(short),pdata);
-    if(ret)
-    {
-        reverse_byte(pdata,size*sizeof(short),plc_data);
-        for(int i=0;i<size;++i)
-            pdata[i]=plc_data[size-i-1];
-    }
-    free(plc_data);
-    return ret;
-
-}
-    bool S7PLC::WriteShorts(int DBNumber,int start,int size,short* pdata)
-    {
-        short* plc_data=(short*)malloc(size*sizeof(short));
-        memcpy(plc_data,pdata,size*sizeof(short));
-        for(int i=0;i<size;++i)
-            plc_data[i]=HTON(plc_data[i]);
-
-        bool ret=write_dbs(DBNumber,start*sizeof(short),size*sizeof(short),plc_data);
-        free(plc_data);
-        return ret;
-    }
-
- bool S7PLC::read_dbs(int DBNumber,int start,int size,void* pdata)
- {
-     std::lock_guard<std::mutex> lck (mutex_);
-     usleep(1000* 50);
-     if(bConnected_==false)
-        return false;
-     
-     int ret=client_.AsDBRead(DBNumber,start,size,pdata);
-     return ret == 0;
- }
- bool S7PLC::write_dbs(int DBNumber, int start, int size, void *pdata)
- {
-    std::lock_guard<std::mutex> lck(mutex_);
-    usleep(1000*50);
-     if(bConnected_==false)
-        return false;
-     
-     int ret = client_.AsDBWrite(DBNumber, start, size, pdata);
-
-     return ret == 0;
- }
- void S7PLC::disconnect()
- {
-     std::lock_guard<std::mutex> lck(mutex_);
-     client_.Disconnect();
- }
-
- void S7PLC::reverse_byte(void* pdata,int num_byte,void* out)
- {
-     char* pin=(char*)pdata;
-     char* pout=(char*)out;
-     for(int i=0;i<num_byte;++i)
-     {
-         pout[i]=pin[num_byte-i-1];
-     }
- }

tool/s7_plc.h

@@ -1,32 +0,0 @@
-#ifndef S7__PLC__H
-#define S7__PLC__H
-
-#include <s7_client.h>
-#include <mutex>
-#include <iostream>
-
-class S7PLC
-{
-public:
-#define HTON(T) ((T) << 8) | ((T) >> 8)
-protected:
-    bool        bConnected_;
-    std::mutex  mutex_;
-    TSnap7Client client_;
-public:
-    S7PLC();
-    ~S7PLC();
-
-    bool connect(std::string ip);
-    bool getConnection();
-    bool ReadShorts(int DBNumber,int start,int size,short* pdata);
-    bool WriteShorts(int DBNumber,int start,int size,short* pdata);
-    void disconnect();
-private:
-    bool read_dbs(int DBNumber,int start,int size,void* pdata);
-    bool write_dbs(int DBNumber,int start,int size,void* pdata);
-    void reverse_byte(void* pdata,int num_byte,void* out);
-
-};
-
-#endif // !S7__PLC__H

tool/singleton.h

@@ -51,24 +51,28 @@ protected:
 };
 
 /*
-// 使用样例：
+// 如下是 使用样例：
 // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
 // 父类的构造函数必须保护，子类的构造函数必须私有。
 // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
 
 class DerivedSingle:public Singleton<DerivedSingle>
 {
+ // 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
 	friend class Singleton<DerivedSingle>;
 public:
+ // 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
 	DerivedSingle(const DerivedSingle&)=delete;
 	DerivedSingle& operator =(const DerivedSingle&)= delete;
+ 	~DerivedSingle()=default;
 private:
+ // 父类的构造函数必须保护，子类的构造函数必须私有。
 	DerivedSingle()=default;
 };
 
 int main(int argc, char* argv[]){
-	DerivedSingle& instance1 = DerivedSingle::get_instance();
-	DerivedSingle& instance2 = DerivedSingle::get_instance();
+	DerivedSingle& instance1 = DerivedSingle::get_instance_references();
+	DerivedSingle* p_instance2 = DerivedSingle::get_instance_pointer();
 	return 0;
 }