Browse Source

20200914, 万集雷达

huli 4 years ago
parent
commit
206b717709

+ 9 - 3
CMakeLists.txt

@@ -10,7 +10,7 @@ FIND_PACKAGE(Protobuf REQUIRED)
 FIND_PACKAGE(Glog REQUIRED)
 FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL REQUIRED)
-
+FIND_PACKAGE(Ceres REQUIRED)
 
 MESSAGE(WARN "pcl:: ${PCL_INCLUDE_DIRS} --- ${PCL_LIBRARIES}")
 include_directories(
@@ -18,6 +18,7 @@ include_directories(
         ${PCL_INCLUDE_DIRS}
         ${OpenCV_INCLUDE_DIRS}
         ${PROTOBUF_INCLUDE_DIRS}
+		${CERES_INCLUDE_DIRS}
         laser
         Locate
         communication
@@ -36,6 +37,8 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/message message_src )
 #aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/lidar_locate locate_src )
 #aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/robot robot_src )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/laser LASER_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/wanji_lidar WANJI_LIDAR_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}/terminor TERMINOR_SRC )
@@ -43,6 +46,7 @@ aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/communication COMMUNICATION_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/system SYSTEM_SRC )
+aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY_SRC )
 
 add_executable(terminal
         main.cpp
@@ -51,7 +55,8 @@ add_executable(terminal
         ${robot_src}
         ${message_src}
 
-        ${LASER_SRC}
+		${LASER_SRC}
+		${WANJI_LIDAR_SRC}
         ${PLC_SRC}
         ${TERMINOR_SRC}
         ${LOCATE_SRC}
@@ -59,7 +64,7 @@ add_executable(terminal
         ${TOOL_SRC}
         ${COMMUNICATION_SRC}
         ${SYSTEM_SRC}
-
+		${VERIFY_SRC}
 
 		)
 
@@ -75,6 +80,7 @@ target_link_libraries(terminal
         ${OpenCV_LIBS}
         ${GLOG_LIBRARIES}
         ${PCL_LIBRARIES}
+		${CERES_LIBRARIES}
 
         libtensorflow_cc.so
 #        tf_3dcnn_api.so

+ 0 - 9
communication/communication_socket_base.h

@@ -152,13 +152,4 @@ private:
 
 
 
-
-
-
-
-
-
-
-
-
 #endif //__COMMUNICATION_SOCKET_BASE__HH__

+ 31 - 6
error_code/error_code.h

@@ -53,10 +53,12 @@ enum Error_code
     WARNING                         = 0x00000003,//警告
     FAILED                          = 0x00000004,//失败
 
-    NO_DATA                         = 0x00000010,//没有数据,传入参数容器内部没有数据时,
+    NODATA                         = 0x00000010,//没有数据,传入参数容器内部没有数据时,
 	INVALID_MESSAGE					= 0x00000011, //无效的消息,
     RESPONSE_TIMEOUT                = 0x00000012,
 
+    TASK_TIMEOVER					= 0x00000020,//任务超时
+
     POINTER_IS_NULL                 = 0x00000101,//空指针
     PARAMETER_ERROR                 = 0x00000102,//参数错误,传入参数不符合规范时,
     POINTER_MALLOC_FAIL             = 0x00000103,//手动分配内存失败
@@ -109,8 +111,8 @@ enum Error_code
 	LASER_MANAGER_STATUS_ERROR,						//雷达管理模块,状态错误
 	LASER_MANAGER_TASK_TYPE_ERROR,					//雷达管理模块,任务类型错误
 	LASER_MANAGER_IS_NOT_READY,						//雷达管理模块,不在准备状态
-	LASER_MANAGER_LASER_INDEX_ERRPR,				//雷达管理模块,雷达索引错误,编号错误。
 	LASER_MANAGER_TASK_OVER_TIME,					//雷达管理模块,任务超时
+	LASER_MANAGER_LASER_INDEX_ERRPR,				//雷达管理模块,雷达索引错误,编号错误。
 	LASER_MANAGER_LASER_INDEX_REPEAT,				//雷达管理模块,需要扫描的雷达索引重复,可忽略的错误,提示作用
 
 //livox_driver 雷达livox驱动模块
@@ -147,7 +149,7 @@ enum Error_code
 	LOCATER_MANAGER_TASK_TYPE_ERROR,					//定位管理模块,任务类型错误
 	LOCATER_MANAGER_IS_NOT_READY,						//定位管理模块,不在准备状态
 	LOCATER_MANAGER_CLOUD_MAP_ERROR,					//定位管理模块,任务输入点云map的error
-	LOCATE_MANAGER_TASK_OVER_TIME,
+	LOCATE_MANAGER_TASK_OVER_TIME,						//定位管理模块,任务超时
 
 
 	//Locater.cpp error from 0x0301000-0x030100FF
@@ -236,8 +238,21 @@ enum Error_code
     HARDWARE_LIMIT_TERMINAL_LR_ERROR,
 
 
-    //wj_lidar error from 0x06010000-0x0601FFFF
-        WJ_LIDAR_CONNECT_FAILED=0x06010000,
+
+    WANJI_LIDAR_DEVICE_ERROR_BASE=0x06080000,						//万集设备模块,错误基类
+	WANJI_LIDAR_DEVICE_STATUS_BUSY,									//万集设备模块,状态正忙
+	WANJI_LIDAR_DEVICE_STATUS_ERROR,								//万集设备模块,状态错误
+	WANJI_LIDAR_DEVICE_TASK_TYPE_ERROR,								//万集设备模块,任务类型错误
+	WANJI_LIDAR_DEVICE_TASK_OVER_TIME,								//万集设备模块,任务超时
+	WANJI_LIDAR_DEVICE_NO_CLOUD,									//万集设备模块,没有点云
+
+
+	//wj_lidar error from 0x06010000-0x0601FFFF
+	WJ_LIDAR_COMMUNICATION_ERROR_BASE=0x06010000,
+	WJ_LIDAR_COMMUNICATION_UNINITIALIZED,							//万集通信,未初始化
+	WJ_LIDAR_COMMUNICATION_DISCONNECT,								//万集通信,断连
+	WJ_LIDAR_COMMUNICATION_FAULT,									//万集通信,故障
+	WJ_LIDAR_CONNECT_FAILED,
     WJ_LIDAR_UNINITIALIZED,
     WJ_LIDAR_READ_FAILED,
     WJ_LIDAR_WRITE_FAILED,
@@ -245,6 +260,8 @@ enum Error_code
 
     //wj lidar protocol error from 0x06020000-0x0602FFFF
         WJ_PROTOCOL_ERROR_BASE=0x06020000,
+	WJ_PROTOCOL_STATUS_BUSY,										//万集解析, 状态正忙
+	WJ_PROTOCOL_STATUS_ERROR,										//系统执行模块, 状态错误
     WJ_PROTOCOL_INTEGRITY_ERROR,
     WJ_PROTOCOL_PARSE_FAILED,
     WJ_PROTOCOL_EMPTY_PACKAGE,
@@ -262,8 +279,16 @@ enum Error_code
     WJ_MANAGER_LIDAR_DISCONNECTED,
     WJ_MANAGER_PLC_DISCONNECTED,
     WJ_MANAGER_EMPTY_CLOUD,
+	WJ_MANAGER_READ_PROTOBUF_ERROR,								//万集管理模块,读取参数错误
+	WJ_MANAGER_INIT_REPEAT,										//万集管理模块,重复初始化
+	WJ_MANAGER_TASK_TYPE_ERROR,									//万集管理模块,任务类型错误
+	WJ_MANAGER_STATUS_BUSY,										//万集管理模块,状态正忙
+	WJ_MANAGER_STATUS_ERROR,									//万集管理模块,状态错误
+	WJ_MANAGER_LASER_INDEX_ERRPR,				//雷达管理模块,雷达索引错误,编号错误。
+	WJ_MANAGER_LASER_INDEX_REPEAT,				//雷达管理模块,需要扫描的雷达索引重复,可忽略的错误,提示作用
+
 
-    WJ_LIDAR_TASK_EMPTY_RESULT=0x06050000,
+	WJ_LIDAR_TASK_EMPTY_RESULT=0x06050000,
     WJ_LIDAR_TASK_EMPTY_TASK,
     WJ_LIDAR_TASK_WRONG_TYPE,
     WJ_LIDAR_TASK_INVALID_TASK,

+ 30 - 11
laser/Laser.cpp

@@ -122,7 +122,7 @@ Error_manager Laser_base::disconnect_laser()
 	{
 		mp_thread_transform->join();
 		delete mp_thread_transform;
-		mp_thread_transform = 0;
+		mp_thread_transform = NULL;
 	}
 	if (mp_thread_publish)
 	{
@@ -230,23 +230,21 @@ Error_manager Laser_base::end_task()
 
 	close_save_path();
 
+	if ( m_laser_statu == LASER_BUSY )
+	{
+		m_laser_statu = LASER_READY;
+	}
+
 	//在结束任务单时,将雷达任务状态改为 TASK_OVER 已结束
 	if(mp_laser_task !=NULL)
 	{
 		//判断任务单的错误等级,
 		if ( mp_laser_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
 		{
-			if ( m_laser_statu == LASER_BUSY )
-			{
-				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR,则恢复待机,可以接受下一次的任务单
-				m_laser_statu = LASER_READY;
-			}
 			mp_laser_task->set_task_statu(TASK_OVER);
 		}
 		else
 		{
-			//故障等级为 MAJOR_ERROR CRITICAL_ERROR,则雷达故障,不可以再接受任务单。
-			m_laser_statu = LASER_FAULT;
 			mp_laser_task->set_task_statu(TASK_ERROR);
 		}
 	}
@@ -256,9 +254,30 @@ Error_manager Laser_base::end_task()
 //取消任务单,由发送方提前取消任务单
 Error_manager Laser_base::cancel_task()
 {
-	end_task();
+	stop_scan();
+	m_laser_scan_flag=false;
+	m_condition_receive.notify_all(false);
+	m_condition_transform.notify_all(false);
+	//确保内部线程已经停下
+	while (m_condition_receive.is_working()	|| m_condition_transform.is_working())
+	{
+
+	}
+	m_points_count=0;
+	m_queue_laser_data.clear_and_delete();
+	m_last_data.clear();
+
+	close_save_path();
+
+
 	//强制改为 TASK_DEAD,不管它当前在做什么。
 	mp_laser_task->set_task_statu(TASK_DEAD);
+
+	if ( m_laser_statu == LASER_BUSY )
+	{
+		m_laser_statu = LASER_READY;
+	}
+
 	return Error_code::SUCCESS;
 }
 
@@ -379,7 +398,7 @@ void Laser_base::thread_receive()
 				//接受线程无法接受到数据的同时,也收到了雷达停止扫描的信息,那么线程停止
 				//如果m_laser_scan_flag停止扫描,但是receive_buf_to_queue仍然能够接受到数据。那么线程继续运行。
 				//m_laser_scan_flag停止的瞬间,可能还有残留的数据,需要继续执行。
-			else if(t_error == NO_DATA  )
+			else if(t_error == NODATA  )
 			{
 				if ( !m_laser_scan_flag )
 				{
@@ -423,7 +442,7 @@ void Laser_base::thread_transform()
 				//注:必须先停止接受线程,再停止转化线程。转化线程停止之后即可结束任务。
 				//如果m_laser_scan_flag停止扫描,接受线程和转化线程还有未完成的数据。那么线程继续运行。
 				//m_laser_scan_flag停止的瞬间,可能还有残留的数据,需要继续执行。防止数据遗留。
-			else if (t_error == NO_DATA )
+			else if (t_error == NODATA )
 			{
 				if(m_laser_scan_flag == false && m_condition_receive.get_pass_ever() == false)
 				{

+ 7 - 6
laser/LivoxLaser.cpp

@@ -208,10 +208,11 @@ Error_manager CLivoxLaser::end_task()
 //取消任务单,由发送方提前取消任务单
 Error_manager CLivoxLaser::cancel_task()
 {
-	end_task();
-	//强制改为 TASK_DEAD,不管它当前在做什么。
-	mp_laser_task->set_task_statu(TASK_DEAD);
-	return Error_code::SUCCESS;
+	stop_scan();
+	m_frame_count = 0;
+	m_queue_livox_data.clear_and_delete();
+	return Laser_base::cancel_task();
+
 }
 
 //判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
@@ -328,7 +329,7 @@ Error_manager CLivoxLaser::receive_buf_to_queue()
 	}
 	else
 	{
-		return NO_DATA;// m_queue_livox_data 没有数据并不意味着程序就出错了,
+		return NODATA;// m_queue_livox_data 没有数据并不意味着程序就出错了,
 	}
 	return Error_code::SUCCESS;
 }
@@ -384,7 +385,7 @@ Error_manager CLivoxLaser::transform_buf_to_points()
 	}
 	else
 	{
-		return NO_DATA;// m_queue_laser_data 没有数据并不意味着程序就出错了,
+		return NODATA;// m_queue_laser_data 没有数据并不意味着程序就出错了,
 	}
 	return Error_code::SUCCESS;
 }

+ 28 - 21
laser/laser_manager.cpp

@@ -8,16 +8,13 @@
 #include "livox_driver.h"
 
 #include <livox_sdk.h>
-
 Laser_manager::Laser_manager()
 {
 	m_laser_manager_status = LASER_MANAGER_UNKNOW;
-	m_laser_number = 0;
 	m_laser_manager_working_flag = false;
 	mp_laser_manager_thread = NULL;
 
 	mp_laser_manager_task = NULL;
-	m_laser_number = 0;
 }
 Laser_manager::~Laser_manager()
 {
@@ -46,7 +43,6 @@ Error_manager Laser_manager::laser_manager_init_from_protobuf(Laser_proto::Laser
 {
 	LOG(INFO) << " ---laser_manager_init_from_protobuf run--- "<< this;
 	Error_manager t_error;
-	m_laser_number = laser_parameters.laser_parameters_size();
 	m_laser_manager_working_flag = false;
 
 	//创建大疆雷达
@@ -108,13 +104,11 @@ Error_manager Laser_manager::laser_manager_uninit()
 	}
 
 	//回收雷达的内存
-	for (int i = 0; i < m_laser_number; ++i)
+	for (int i = 0; i < m_laser_vector.size(); ++i)
 	{
-
 		delete(m_laser_vector[i]);
 	}
 	m_laser_manager_status = LASER_MANAGER_UNKNOW;
-	m_laser_number = 0;
 	m_laser_vector.clear();
 	m_laser_manager_working_flag = false;
 	//反初始化 Livox_driver
@@ -221,7 +215,7 @@ Error_manager Laser_manager::check_status()
 	}
 	else if ( m_laser_manager_status == LASER_MANAGER_ISSUED_TASK || m_laser_manager_status == LASER_MANAGER_WAIT_REPLY )
 	{
-		return Error_manager(Error_code::LASER_MANAGER_STATUS_BUSY, Error_level::MINOR_ERROR,
+		return Error_manager(Error_code::LASER_MANAGER_STATUS_BUSY, Error_level::NEGLIGIBLE_ERROR,
 							 " Laser_manager::check_status error ");
 	}
 	else
@@ -244,27 +238,23 @@ Error_manager Laser_manager::end_task()
 	//释放下发的任务单
 	laser_task_map_clear_and_delete();
 
+	if ( m_laser_manager_status == LASER_MANAGER_ISSUED_TASK  || m_laser_manager_status == LASER_MANAGER_WAIT_REPLY )
+	{
+		m_laser_manager_status = LASER_MANAGER_READY;
+	}
+	//else 状态不变
+
 	//在结束任务单时,将雷达任务状态改为 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 状态不变
-
 			//强制改为TASK_OVER,不管它当前在做什么。
 			mp_laser_manager_task->set_task_statu(TASK_OVER);
 		}
 		else
 		{
-			//故障等级为 MAJOR_ERROR CRITICAL_ERROR,则雷达故障,不可以再接受任务单。
-			m_laser_manager_status = LASER_MANAGER_FAULT;
-
 			//强制改为 TASK_ERROR,不管它当前在做什么。
 			mp_laser_manager_task->set_task_statu(TASK_ERROR);
 		}
@@ -284,10 +274,27 @@ Error_manager Laser_manager::cancel_task()
 			map_iter->second->set_task_statu(TASK_DEAD);
 	}
 
-	end_task();
+	//关闭子线程
+	m_laser_manager_working_flag=false;
+	m_laser_manager_condition.notify_all(false);
+	//确保线程已经停下
+	while ( m_laser_manager_condition.is_working() )
+	{
+
+	}
+
+	//释放下发的任务单
+	laser_task_map_clear_and_delete();
+
 	//强制改为 TASK_DEAD,不管它当前在做什么。
 	mp_laser_manager_task->set_task_statu(TASK_DEAD);
 
+	if ( m_laser_manager_status == LASER_MANAGER_ISSUED_TASK  || m_laser_manager_status == LASER_MANAGER_WAIT_REPLY )
+	{
+		m_laser_manager_status = LASER_MANAGER_READY;
+	}
+	//else 状态不变
+
 	return Error_code::SUCCESS;
 }
 
@@ -328,7 +335,7 @@ Error_manager Laser_manager::issued_task(int laser_index)
 {
 	Error_manager t_error;
 
-	if ( laser_index < 0 || laser_index >= m_laser_number )
+	if ( laser_index < 0 || laser_index >= m_laser_vector.size() )
 	{
 		return Error_manager(Error_code::LASER_MANAGER_LASER_INDEX_ERRPR, Error_level::MINOR_ERROR,
 							 " issued_task laser_index  error ");
@@ -426,7 +433,7 @@ void Laser_manager::thread_work()
 				if ( mp_laser_manager_task->get_select_all_laser_flag() )
 				{
 					//下发任务给所有的雷达
-					for (int i = 0; i < m_laser_number; ++i)
+					for (int i = 0; i < m_laser_vector.size(); ++i)
 					{
 						//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
 						t_error = issued_task(i);

+ 1 - 1
laser/laser_manager.h

@@ -105,7 +105,7 @@ protected:
 protected://member variable
 
 	//子雷达
-	int 										m_laser_number;					//雷达的个数
+//	int 										m_laser_number;					//雷达的个数
 	std::vector<Laser_base*>                    m_laser_vector;					//雷达的对象实例,内存由本类管理
 	//注意:里面的Laser_base*是在init里面new出来的,析构之前必须使用ununit来销毁
 

+ 31 - 13
locate/locate_manager.cpp

@@ -296,29 +296,25 @@ Error_manager Locate_manager::end_task()
 	m_cloud_wheel_map.clear();
 	m_cloud_car_map.clear();
 
+	if ( m_locate_manager_status == LOCATE_MANAGER_SIFT
+		 || m_locate_manager_status == LOCATE_MANAGER_CAR
+		 || m_locate_manager_status == LOCATE_MANAGER_WHEEL)
+	{
+		m_locate_manager_status = LOCATE_MANAGER_READY;
+	}
+	//else 状态不变
+
 	//在结束任务单时,将雷达任务状态改为 TASK_OVER 已结束
 	if(mp_locate_manager_task !=NULL)
 	{
 		//判断任务单的错误等级,
 		if ( mp_locate_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
 		{
-			if ( m_locate_manager_status == LOCATE_MANAGER_SIFT
-				 || m_locate_manager_status == LOCATE_MANAGER_CAR
-					|| m_locate_manager_status == LOCATE_MANAGER_WHEEL)
-			{
-				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR,则恢复待机,可以接受下一次的任务单
-				m_locate_manager_status = LOCATE_MANAGER_READY;
-			}
-			//else 状态不变
-
 			//强制改为TASK_OVER,不管它当前在做什么。
 			mp_locate_manager_task->set_task_statu(TASK_OVER);
 		}
 		else
 		{
-			//故障等级为 MAJOR_ERROR CRITICAL_ERROR,则雷达故障,不可以再接受任务单。
-			m_locate_manager_status = LOCATE_MANAGER_FAULT;
-
 			//强制改为 TASK_ERROR,不管它当前在做什么。
 			mp_locate_manager_task->set_task_statu(TASK_ERROR);
 		}
@@ -329,9 +325,31 @@ Error_manager Locate_manager::end_task()
 //取消任务单,由发送方提前取消任务单
 Error_manager Locate_manager::cancel_task()
 {
-	end_task();
+	//关闭子线程
+	m_locate_manager_working_flag=false;
+	m_locate_manager_condition.notify_all(false);
+	//确保内部线程已经停下
+	while (m_locate_manager_condition.is_working())
+	{
+
+	}
+
+	//释放缓存
+	mp_locate_information = NULL;
+	m_cloud_wheel_map.clear();
+	m_cloud_car_map.clear();
+
 	//强制改为 TASK_DEAD,不管它当前在做什么。
 	mp_locate_manager_task->set_task_statu(TASK_DEAD);
+
+	if ( m_locate_manager_status == LOCATE_MANAGER_SIFT
+		 || m_locate_manager_status == LOCATE_MANAGER_CAR
+		 || m_locate_manager_status == LOCATE_MANAGER_WHEEL)
+	{
+		m_locate_manager_status = LOCATE_MANAGER_READY;
+	}
+	//else 状态不变
+
 	return Error_code::SUCCESS;
 }
 

+ 91 - 2
main.cpp

@@ -19,6 +19,10 @@
 #include "./system/system_communication.h"
 #include "./system/system_executor.h"
 
+#include "./wanji_lidar/wanji_lidar_device.h"
+#include "./tool/proto_tool.h"
+#include "./wanji_lidar/wanji_manager.h"
+
 #define LIVOX_NUMBER	     2
 
 GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
@@ -42,9 +46,15 @@ GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
 
 }
 
+
 int main(int argc,char* argv[])
 {
 
+
+	Error_manager t_error;
+	Error_manager t_result ;
+
+
 	const char* logPath = "./";
 	google::InitGoogleLogging("LidarMeasurement");
 	google::SetStderrLogging(google::INFO);
@@ -57,6 +67,86 @@ int main(int argc,char* argv[])
 	FLAGS_max_log_size = 1024;            // Set max log file size(GB)
 	FLAGS_stop_logging_if_full_disk = true;
 
+	Wanji_manager::get_instance_references().wanji_manager_init();
+	std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;
+
+	while (1  )
+	{
+		char ch1 ;
+		std::cin >> ch1 ;
+		if (ch1 == 'b'  )
+		{
+			break;
+		}
+
+		auto start   = std::chrono::system_clock::now();
+
+
+		Wanji_manager_task task;
+		task.task_init(TASK_CREATED, "", NULL,
+					   std::chrono::milliseconds(1000),
+					   0,
+					   std::chrono::system_clock::now());
+		t_error = Wanji_manager::get_instance_references().execute_task(&task);
+		std::cout << " huli test 123123:::: " << " t_error = " << t_error << std::endl;
+
+
+		//判断任务是否结束, 阻塞 等待, 直到任务完成或者超时
+		while ( task.is_task_end() == false)
+		{
+			if ( task.is_over_time() )
+			{
+				//超时处理。取消任务。
+				Wanji_manager::get_instance_references().cancel_task(&task);
+				task.set_task_statu(TASK_DEAD);
+				t_error.error_manager_reset(Error_code::WANJI_LIDAR_DEVICE_TASK_OVER_TIME, Error_level::MINOR_ERROR,
+											" locate_manager_task is_over_time ");
+				task.set_task_error_manager(t_error);
+			}
+			else
+			{
+				//继续等待
+				std::this_thread::sleep_for(std::chrono::microseconds(1));
+				std::this_thread::yield();
+			}
+
+		}
+		//提取任务单 的错误码
+		t_error = task.get_task_error_manager();
+		std::cout << "huli  error 456456:::::::"  << t_error << std::endl;
+		std::cout << " huli test :::: " << " task.get_task_statu() = " << task.get_task_statu() << std::endl;
+
+
+		auto end   = std::chrono::system_clock::now();
+		auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
+		std::cout <<  "花费了"
+			 << double(duration.count()*1000) * std::chrono::microseconds::period::num / std::chrono::microseconds::period::den   << "毫秒" << std::endl;
+		std::cout << " huli test :::: " << " std::chrono::microseconds::period::num = " << std::chrono::microseconds::period::num << std::endl;
+		std::cout << " huli test :::: " << " std::chrono::microseconds::period::den = " << std::chrono::microseconds::period::den << std::endl;
+		std::cout << " huli test :::: " << " duration.count() = " << duration.count() << std::endl;
+
+
+	}
+//	Wanji_manager::get_instance_references().wanji_manager_uninit();
+	std::cout << " huli test :::: " << " uninit = " << 999999 << std::endl;
+	return 0;
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 	int t_terminal_id = 0;
 	if ( argc == 2 )
 	{
@@ -89,8 +179,7 @@ int main(int argc,char* argv[])
 	LOG(INFO) << " --- main start --- " ;
 
 
-	Error_manager t_error;
-	Error_manager t_result ;
+
 
 
 

File diff suppressed because it is too large
+ 1643 - 15
message/measure_message.pb.cc


File diff suppressed because it is too large
+ 1360 - 327
message/measure_message.pb.h


+ 34 - 0
message/measure_message.proto

@@ -65,3 +65,37 @@ message Measure_response_msg
     optional Locate_information         locate_information=4;
     required Error_manager              error_manager = 5;
 }
+
+//点云坐标
+message Cloud_coordinate
+{
+    required float                      x=1;
+    required float                      y=2;
+    required float                      z=3;
+}
+//点云类型
+message Cloud_type
+{
+    required int32                      type=1;
+}
+
+//筛选点云, 请求消息
+message Locate_sift_request_msg
+{
+    required Base_info                  base_info=1;            //消息类型
+    required string                     command_key=2;          //指令唯一标识符id
+    required int32                      terminal_id=3;          //终端id
+    required int32                      lidar_id=4;             //雷达id
+    repeated Cloud_coordinate           cloud_coordinates=5;     //点云坐标
+}
+
+//筛选点云, 答复消息
+message Locate_sift_response_msg
+{
+    required Base_info                  base_info=1;            //消息类型
+    required string                     command_key=2;          //指令唯一标识符id
+    required int32                      terminal_id=3;          //终端id
+    required int32                      lidar_id=4;             //雷达id
+    repeated Cloud_type                 cloud_type=5;            //点云类型
+    required Error_manager              error_manager = 6;      //错误码
+}

+ 77 - 16
message/message_base.pb.cc

@@ -181,7 +181,7 @@ void InitDefaultsParkspace_info() {
 }
 
 ::google::protobuf::Metadata file_level_metadata[6];
-const ::google::protobuf::EnumDescriptor* file_level_enum_descriptors[6];
+const ::google::protobuf::EnumDescriptor* file_level_enum_descriptors[8];
 
 const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
   GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::message::Base_info, _has_bits_),
@@ -342,7 +342,7 @@ void AddDescriptorsImpl() {
       "\016\n\006height\030\007 \001(\002\0223\n\020parkspace_status\030\010 \001("
       "\0162\031.message.Parkspace_status\022#\n\010car_info"
       "\030\t \001(\0132\021.message.Car_info\022\022\n\nentry_time\030"
-      "\n \001(\t\022\022\n\nleave_time\030\013 \001(\t*\276\006\n\014Message_ty"
+      "\n \001(\t\022\022\n\nleave_time\030\013 \001(\t*\253\007\n\014Message_ty"
       "pe\022\r\n\teBase_msg\020\000\022\020\n\014eCommand_msg\020\001\022\026\n\022e"
       "Locate_status_msg\020\021\022\027\n\023eLocate_request_m"
       "sg\020\022\022\030\n\024eLocate_response_msg\020\023\022\030\n\024eDispa"
@@ -363,20 +363,33 @@ void AddDescriptorsImpl() {
       "msg\020B\022\037\n\033ePickup_command_request_msg\020C\022 "
       "\n\034ePickup_command_response_msg\020D\022\037\n\032eSto"
       "ring_process_statu_msg\020\220\001\022\037\n\032ePicking_pr"
-      "ocess_statu_msg\020\221\001*f\n\014Communicator\022\n\n\006eE"
-      "mpty\020\000\022\t\n\005eMain\020\001\022\016\n\teTerminor\020\200\002\022\017\n\nePa"
-      "rkspace\020\200\004\022\016\n\teMeasurer\020\200\006\022\016\n\teDispatch\020"
-      "\200\010*#\n\005Event\022\014\n\010eStoring\020\001\022\014\n\010ePicking\020\002*"
-      "e\n\013Error_level\022\n\n\006NORMAL\020\000\022\024\n\020NEGLIGIBLE"
-      "_ERROR\020\001\022\017\n\013MINOR_ERROR\020\002\022\017\n\013MAJOR_ERROR"
-      "\020\003\022\022\n\016CRITICAL_ERROR\020\004*q\n\020Parkspace_stat"
-      "us\022\024\n\020eParkspace_empty\020\000\022\027\n\023eParkspace_o"
-      "ccupied\020\001\022\030\n\024eParkspace_reserverd\020\002\022\024\n\020e"
-      "Parkspace_error\020\003*(\n\tDirection\022\014\n\010eForwa"
-      "rd\020\001\022\r\n\teBackward\020\002"
+      "ocess_statu_msg\020\221\001\022\"\n\035eCentral_controlle"
+      "r_statu_msg\020\240\001\022#\n\036eEntrance_manual_opera"
+      "tion_msg\020\260\001\022\"\n\035eProcess_manual_operation"
+      "_msg\020\261\001*f\n\014Communicator\022\n\n\006eEmpty\020\000\022\t\n\005e"
+      "Main\020\001\022\016\n\teTerminor\020\200\002\022\017\n\neParkspace\020\200\004\022"
+      "\016\n\teMeasurer\020\200\006\022\016\n\teDispatch\020\200\010**\n\014Proce"
+      "ss_type\022\014\n\010eStoring\020\001\022\014\n\010ePicking\020\002*e\n\013E"
+      "rror_level\022\n\n\006NORMAL\020\000\022\024\n\020NEGLIGIBLE_ERR"
+      "OR\020\001\022\017\n\013MINOR_ERROR\020\002\022\017\n\013MAJOR_ERROR\020\003\022\022"
+      "\n\016CRITICAL_ERROR\020\004*q\n\020Parkspace_status\022\024"
+      "\n\020eParkspace_empty\020\000\022\027\n\023eParkspace_occup"
+      "ied\020\001\022\030\n\024eParkspace_reserverd\020\002\022\024\n\020ePark"
+      "space_error\020\003*(\n\tDirection\022\014\n\010eForward\020\001"
+      "\022\r\n\teBackward\020\002*\335\002\n\tStep_type\022\017\n\013eAlloc_"
+      "step\020\000\022\021\n\reMeasure_step\020\001\022\021\n\reCompare_st"
+      "ep\020\002\022\022\n\016eDispatch_step\020\003\022\021\n\reConfirm_ste"
+      "p\020\004\022\020\n\014eSearch_step\020\005\022\016\n\neWait_step\020\006\022\021\n"
+      "\reRelease_step\020\007\022\r\n\teComplete\020\010\022\025\n\021eBack"
+      "Confirm_step\020\t\022\026\n\022eBack_compare_step\020\n\022\025"
+      "\n\021eBackMeasure_step\020\013\022\023\n\017eBackAlloc_step"
+      "\020\014\022\022\n\016eBackWait_step\020\r\022\026\n\022eBackDispatch_"
+      "step\020\016\022\024\n\020eBackSearch_step\020\017\022\021\n\reBackCom"
+      "plete\020\020*C\n\nStep_statu\022\014\n\010eWaiting\020\000\022\014\n\010e"
+      "Working\020\001\022\n\n\006eError\020\002\022\r\n\teFinished\020\003"
   };
   ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
-      descriptor, 2179);
+      descriptor, 2716);
   ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
     "message_base.proto", &protobuf_RegisterTypes);
 }
@@ -424,6 +437,9 @@ bool Message_type_IsValid(int value) {
     case 68:
     case 144:
     case 145:
+    case 160:
+    case 176:
+    case 177:
       return true;
     default:
       return false;
@@ -448,11 +464,11 @@ bool Communicator_IsValid(int value) {
   }
 }
 
-const ::google::protobuf::EnumDescriptor* Event_descriptor() {
+const ::google::protobuf::EnumDescriptor* Process_type_descriptor() {
   protobuf_message_5fbase_2eproto::protobuf_AssignDescriptorsOnce();
   return protobuf_message_5fbase_2eproto::file_level_enum_descriptors[2];
 }
-bool Event_IsValid(int value) {
+bool Process_type_IsValid(int value) {
   switch (value) {
     case 1:
     case 2:
@@ -509,6 +525,51 @@ bool Direction_IsValid(int value) {
   }
 }
 
+const ::google::protobuf::EnumDescriptor* Step_type_descriptor() {
+  protobuf_message_5fbase_2eproto::protobuf_AssignDescriptorsOnce();
+  return protobuf_message_5fbase_2eproto::file_level_enum_descriptors[6];
+}
+bool Step_type_IsValid(int value) {
+  switch (value) {
+    case 0:
+    case 1:
+    case 2:
+    case 3:
+    case 4:
+    case 5:
+    case 6:
+    case 7:
+    case 8:
+    case 9:
+    case 10:
+    case 11:
+    case 12:
+    case 13:
+    case 14:
+    case 15:
+    case 16:
+      return true;
+    default:
+      return false;
+  }
+}
+
+const ::google::protobuf::EnumDescriptor* Step_statu_descriptor() {
+  protobuf_message_5fbase_2eproto::protobuf_AssignDescriptorsOnce();
+  return protobuf_message_5fbase_2eproto::file_level_enum_descriptors[7];
+}
+bool Step_statu_IsValid(int value) {
+  switch (value) {
+    case 0:
+    case 1:
+    case 2:
+    case 3:
+      return true;
+    default:
+      return false;
+  }
+}
+
 
 // ===================================================================
 

+ 85 - 17
message/message_base.pb.h

@@ -111,11 +111,14 @@ enum Message_type {
   ePickup_command_request_msg = 67,
   ePickup_command_response_msg = 68,
   eStoring_process_statu_msg = 144,
-  ePicking_process_statu_msg = 145
+  ePicking_process_statu_msg = 145,
+  eCentral_controller_statu_msg = 160,
+  eEntrance_manual_operation_msg = 176,
+  eProcess_manual_operation_msg = 177
 };
 bool Message_type_IsValid(int value);
 const Message_type Message_type_MIN = eBase_msg;
-const Message_type Message_type_MAX = ePicking_process_statu_msg;
+const Message_type Message_type_MAX = eProcess_manual_operation_msg;
 const int Message_type_ARRAYSIZE = Message_type_MAX + 1;
 
 const ::google::protobuf::EnumDescriptor* Message_type_descriptor();
@@ -151,24 +154,24 @@ inline bool Communicator_Parse(
   return ::google::protobuf::internal::ParseNamedEnum<Communicator>(
     Communicator_descriptor(), name, value);
 }
-enum Event {
+enum Process_type {
   eStoring = 1,
   ePicking = 2
 };
-bool Event_IsValid(int value);
-const Event Event_MIN = eStoring;
-const Event Event_MAX = ePicking;
-const int Event_ARRAYSIZE = Event_MAX + 1;
+bool Process_type_IsValid(int value);
+const Process_type Process_type_MIN = eStoring;
+const Process_type Process_type_MAX = ePicking;
+const int Process_type_ARRAYSIZE = Process_type_MAX + 1;
 
-const ::google::protobuf::EnumDescriptor* Event_descriptor();
-inline const ::std::string& Event_Name(Event value) {
+const ::google::protobuf::EnumDescriptor* Process_type_descriptor();
+inline const ::std::string& Process_type_Name(Process_type value) {
   return ::google::protobuf::internal::NameOfEnum(
-    Event_descriptor(), value);
+    Process_type_descriptor(), value);
 }
-inline bool Event_Parse(
-    const ::std::string& name, Event* value) {
-  return ::google::protobuf::internal::ParseNamedEnum<Event>(
-    Event_descriptor(), name, value);
+inline bool Process_type_Parse(
+    const ::std::string& name, Process_type* value) {
+  return ::google::protobuf::internal::ParseNamedEnum<Process_type>(
+    Process_type_descriptor(), name, value);
 }
 enum Error_level {
   NORMAL = 0,
@@ -232,6 +235,61 @@ inline bool Direction_Parse(
   return ::google::protobuf::internal::ParseNamedEnum<Direction>(
     Direction_descriptor(), name, value);
 }
+enum Step_type {
+  eAlloc_step = 0,
+  eMeasure_step = 1,
+  eCompare_step = 2,
+  eDispatch_step = 3,
+  eConfirm_step = 4,
+  eSearch_step = 5,
+  eWait_step = 6,
+  eRelease_step = 7,
+  eComplete = 8,
+  eBackConfirm_step = 9,
+  eBack_compare_step = 10,
+  eBackMeasure_step = 11,
+  eBackAlloc_step = 12,
+  eBackWait_step = 13,
+  eBackDispatch_step = 14,
+  eBackSearch_step = 15,
+  eBackComplete = 16
+};
+bool Step_type_IsValid(int value);
+const Step_type Step_type_MIN = eAlloc_step;
+const Step_type Step_type_MAX = eBackComplete;
+const int Step_type_ARRAYSIZE = Step_type_MAX + 1;
+
+const ::google::protobuf::EnumDescriptor* Step_type_descriptor();
+inline const ::std::string& Step_type_Name(Step_type value) {
+  return ::google::protobuf::internal::NameOfEnum(
+    Step_type_descriptor(), value);
+}
+inline bool Step_type_Parse(
+    const ::std::string& name, Step_type* value) {
+  return ::google::protobuf::internal::ParseNamedEnum<Step_type>(
+    Step_type_descriptor(), name, value);
+}
+enum Step_statu {
+  eWaiting = 0,
+  eWorking = 1,
+  eError = 2,
+  eFinished = 3
+};
+bool Step_statu_IsValid(int value);
+const Step_statu Step_statu_MIN = eWaiting;
+const Step_statu Step_statu_MAX = eFinished;
+const int Step_statu_ARRAYSIZE = Step_statu_MAX + 1;
+
+const ::google::protobuf::EnumDescriptor* Step_statu_descriptor();
+inline const ::std::string& Step_statu_Name(Step_statu value) {
+  return ::google::protobuf::internal::NameOfEnum(
+    Step_statu_descriptor(), value);
+}
+inline bool Step_statu_Parse(
+    const ::std::string& name, Step_statu* value) {
+  return ::google::protobuf::internal::ParseNamedEnum<Step_statu>(
+    Step_statu_descriptor(), name, value);
+}
 // ===================================================================
 
 class Base_info : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:message.Base_info) */ {
@@ -2243,10 +2301,10 @@ template <>
 inline const EnumDescriptor* GetEnumDescriptor< ::message::Communicator>() {
   return ::message::Communicator_descriptor();
 }
-template <> struct is_proto_enum< ::message::Event> : ::google::protobuf::internal::true_type {};
+template <> struct is_proto_enum< ::message::Process_type> : ::google::protobuf::internal::true_type {};
 template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::message::Event>() {
-  return ::message::Event_descriptor();
+inline const EnumDescriptor* GetEnumDescriptor< ::message::Process_type>() {
+  return ::message::Process_type_descriptor();
 }
 template <> struct is_proto_enum< ::message::Error_level> : ::google::protobuf::internal::true_type {};
 template <>
@@ -2263,6 +2321,16 @@ template <>
 inline const EnumDescriptor* GetEnumDescriptor< ::message::Direction>() {
   return ::message::Direction_descriptor();
 }
+template <> struct is_proto_enum< ::message::Step_type> : ::google::protobuf::internal::true_type {};
+template <>
+inline const EnumDescriptor* GetEnumDescriptor< ::message::Step_type>() {
+  return ::message::Step_type_descriptor();
+}
+template <> struct is_proto_enum< ::message::Step_statu> : ::google::protobuf::internal::true_type {};
+template <>
+inline const EnumDescriptor* GetEnumDescriptor< ::message::Step_statu>() {
+  return ::message::Step_statu_descriptor();
+}
 
 }  // namespace protobuf
 }  // namespace google

+ 49 - 4
message/message_base.proto

@@ -37,8 +37,15 @@ enum Message_type
 
 
 
-    eStoring_process_statu_msg=0x90;        //停车进度条消息
-    ePicking_process_statu_msg=0x91;        //取车进度消息
+    eStoring_process_statu_msg=0x90;        //停车指令进度条消息
+    ePicking_process_statu_msg=0x91;        //取车指令进度消息
+
+
+    eCentral_controller_statu_msg=0xa0;     //中控系统状态消息
+
+
+    eEntrance_manual_operation_msg=0xb0;            //针对出入口状态操作的手动消息
+    eProcess_manual_operation_msg=0xb1;             //针对流程的手动消息
 
 
 }
@@ -70,7 +77,7 @@ message Base_info
 }
 
 // 事件,停车或者取车
-enum Event
+enum Process_type
 {
     eStoring=1;
     ePicking=2;
@@ -118,6 +125,7 @@ message Locate_information
     optional bool locate_correct = 9;		    //整车的校准标记位
 }
 
+//车辆基本信息
 message Car_info
 {
     optional float                      car_length=1;           //车长
@@ -141,7 +149,7 @@ enum Direction
     eBackward = 2;
 }
 
-//单个车位基本信息与状态信息
+//单个车位基本信息与状态信息,车位信息以及车位上的车辆信息
 message Parkspace_info
 {
     optional int32              parkspace_id=1;         //车位编号
@@ -155,4 +163,41 @@ message Parkspace_info
     optional Car_info           car_info=9;              //当前车位存入车辆的凭证号
     optional string             entry_time=10;          //入场时间
     optional string             leave_time=11;          //离场时间
+}
+
+/*
+*流程中的步骤类型, 例如:停车流程包含5个步骤 , 分配车位-测量-检验结果-搬运-更新车位表
+*/
+enum Step_type
+{
+    eAlloc_step=0;
+    eMeasure_step=1;
+    eCompare_step=2;
+    eDispatch_step=3;
+    eConfirm_step=4;
+
+    eSearch_step=5;        //查询数据库
+    eWait_step=6;             //等待车辆离开
+    eRelease_step=7;          //释放车位
+
+    eComplete=8;              //完成
+
+    eBackConfirm_step=9;
+    eBack_compare_step=10;
+    eBackMeasure_step=11;
+    eBackAlloc_step=12;
+
+    eBackWait_step=13;
+    eBackDispatch_step=14;
+    eBackSearch_step=15;
+
+    eBackComplete=16;
+}
+//步骤状态,每个步骤有四中可能状态 ,等待中-执行中-完成或者错误  四个状态
+enum Step_statu
+{
+    eWaiting=0;               //完成/空闲
+    eWorking=1;
+    eError=2;
+    eFinished=3;
 }

+ 1 - 0
proto.sh

@@ -6,4 +6,5 @@ protoc -I=./laser laser_parameter.proto --cpp_out=./laser
 protoc -I=./laser laser_message.proto --cpp_out=./laser
 protoc -I=./communication communication.proto --cpp_out=./communication
 
+protoc -I=./wanji_lidar wj_lidar_conf.proto --cpp_out=./wanji_lidar
 

+ 2 - 0
setting/communication.prototxt

@@ -12,7 +12,9 @@ communication_parameters
 
  #  bind_string:"tcp://192.168.2.192:30001"
  #  bind_string:"tcp://192.168.2.167:30001"
+
    connect_string_vector:"tcp://192.168.2.183:30002"
+   connect_string_vector:"tcp://192.168.2.125:7001"
 
 }
 

+ 17 - 0
task/task_base.cpp

@@ -23,6 +23,23 @@ Task_Base::~Task_Base()
 	mp_tast_receiver = NULL;
 }
 
+//初始化任务单,必须初始化之后才可以使用,
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+Error_manager Task_Base::task_init(Task_statu task_statu,
+				   std::string task_statu_information,
+				   void* p_tast_receiver,
+				   std::chrono::milliseconds task_over_time)
+{
+	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();
+	return Error_code::SUCCESS;
+}
 
 //更新任务单
 //task_statu: 任务状态

+ 14 - 2
task/task_base.h

@@ -23,7 +23,9 @@ enum Task_type
     LOCATE_MANGER_TASK		=3,             //测量任务
     PLC_TASK                =4,             //上传PLC任务
 
-    WJ_TASK,
+    WANJI_MANAGER_TASK,						//万集雷达管理任务
+	WANJI_LIDAR_SCAN,						//万集雷达扫描任务
+	WANJI_LIDAR_DETECT,						//万集雷达定位任务
     
 };
 //任务状态,如果任务故障,任务状态改为TASK_OVER,然后在m_task_error_manager 补充错误码。
@@ -49,7 +51,17 @@ protected:
 public:
     ~Task_Base();
 
-    //更新任务单
+	//初始化任务单,必须初始化之后才可以使用,
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	Error_manager task_init(Task_statu task_statu,
+					   std::string task_statu_information,
+					   void* p_tast_receiver,
+					   std::chrono::milliseconds task_over_time);
+
+	//更新任务单
     //task_statu: 任务状态
     //statu_information:状态说明
     Error_manager update_statu(Task_statu task_statu,std::string statu_information="");

+ 1 - 1
task/task_command_manager.cpp

@@ -53,7 +53,7 @@ Error_manager Task_command_manager::execute_task(Task_Base* p_task_base)
 		case PLC_TASK:
 			;
 			break;
-		case WJ_TASK:
+		case WANJI_MANAGER_TASK:
 			;
 			break;
 	    default:

+ 23 - 0
test.txt

@@ -18,6 +18,29 @@
 
 
 
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 
 
 

+ 6 - 10
tool/thread_safe_list.h

@@ -156,9 +156,8 @@ bool Thread_safe_list<T>::wait_and_pop(T& value)
 		}
 		else
 		{
-//			value = move(*m_data_list.front());
-			value = (*m_data_list.front());
-
+			value = std::move(*m_data_list.front());
+		
 			m_data_list.pop_front();
 			return true;
 		}
@@ -182,9 +181,8 @@ bool Thread_safe_list<T>::try_pop(T& value)
 		}
 		else
 		{
-//			value = move(*m_data_list.front());
-			value = (*m_data_list.front());
-
+			value = std::move(*m_data_list.front());
+	
 			m_data_list.pop();
 			return true;
 		}
@@ -256,8 +254,7 @@ bool Thread_safe_list<T>::push(T new_value)
 	}
 	else
 	{
-//		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
-		std::shared_ptr<T> data(std::make_shared<T>((new_value)));
+		std::shared_ptr<T> data(std::make_shared<T>(std::move(new_value)));
 		std::unique_lock<std::mutex> lk(m_mutex);
 		m_data_list.push_back(data);
 		m_data_cond.notify_one();
@@ -286,8 +283,7 @@ bool Thread_safe_list<T>::clear_and_delete()
 	while (!m_data_list.empty())
 	{
 		T res = NULL;
-//		res = move(*m_data_list.front());
-		res = (*m_data_list.front());
+		res = std::move(*m_data_list.front());
 
 		m_data_list.pop_front();
 		if(res != NULL)

+ 4 - 5
tool/thread_safe_queue.h

@@ -41,7 +41,6 @@
 #include <mutex>
 #include <condition_variable>
 
-
 template<class T>
 class Thread_safe_queue
 {
@@ -154,7 +153,7 @@ bool Thread_safe_queue<T>::wait_and_pop(T& value)
 		}
 		else
 		{
-			value = move(*m_data_queue.front());
+			value = std::move(*m_data_queue.front());
 			m_data_queue.pop();
 			return true;
 		}
@@ -178,7 +177,7 @@ bool Thread_safe_queue<T>::try_pop(T& value)
 		}
 		else
 		{
-			value = move(*m_data_queue.front());
+			value = std::move(*m_data_queue.front());
 			m_data_queue.pop();
 			return true;
 		}
@@ -247,7 +246,7 @@ bool Thread_safe_queue<T>::push(T new_value)
 	}
 	else
 	{
-		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
+		std::shared_ptr<T> data(std::make_shared<T>(std::move(new_value)));
 		std::unique_lock<std::mutex> lk(m_mutex);
 		m_data_queue.push(data);
 		m_data_cond.notify_one();
@@ -274,7 +273,7 @@ bool Thread_safe_queue<T>::clear_and_delete()
 	while (!m_data_queue.empty())
 	{
 		T res = NULL;
-		res = move(*m_data_queue.front());
+		res = std::move(*m_data_queue.front());
 		m_data_queue.pop();
 		if(res != NULL)
 		{