Browse Source

20200228,优化laser,添加流程图
1.优化底层工具库tool
2.重写task任务基类和雷达任务子类
3.完善laser基类的程序流程
4.添加uml图和流程图

huli 5 years ago
parent
commit
17801286e5
52 changed files with 1853 additions and 449 deletions
  1. 2 2
      CMakeLists.txt
  2. 39 3
      error_code/error_code.cpp
  3. 19 12
      error_code/error_code.h
  4. 313 188
      laser/Laser.cpp
  5. 25 28
      laser/Laser.h
  6. 300 0
      laser/Laser.puml
  7. 165 60
      laser/LivoxLaser.cpp
  8. 30 12
      laser/LivoxLaser.h
  9. 226 0
      laser/LivoxLaser.puml
  10. 94 55
      laser/LivoxMid100Laser.cpp
  11. 4 2
      laser/LivoxMid100Laser.h
  12. 2 1
      laser/Point3D.h
  13. 3 3
      laser/Sick511FileLaser.cpp
  14. 7 2
      laser/Sick511FileLaser.h
  15. 5 5
      laser/TcpLaser.cpp
  16. 7 3
      laser/TcpLaser.h
  17. 6 6
      laser/UdpLaser.cpp
  18. 7 3
      laser/UdpLaser.h
  19. 0 0
      laser/laser_parameter.pb.cc
  20. 0 0
      laser/laser_parameter.pb.h
  21. 0 0
      laser/laser_parameter.proto
  22. 1 5
      laser/laser_task_command.cpp
  23. 1 4
      laser/laser_task_command.h
  24. 110 0
      laser/laser_task_command.puml
  25. 52 33
      main.cpp
  26. BIN
      png/Laser.png
  27. BIN
      png/LivoxLaser.png
  28. BIN
      png/binary_buf.png
  29. BIN
      png/laser_task_command.png
  30. BIN
      png/laser功能模块图.eddx
  31. BIN
      png/laser功能模块图.png
  32. BIN
      png/laser数据流程图.eddx
  33. BIN
      png/laser数据流程图.png
  34. BIN
      png/laser程序流程图.eddx
  35. BIN
      png/laser程序流程图.png
  36. BIN
      png/task_command_manager.png
  37. BIN
      png/thread_condition.png
  38. BIN
      png/thread_safe_queue.png
  39. 16 1
      task/task_command_manager.cpp
  40. 15 10
      task/task_command_manager.h
  41. 78 0
      task/task_command_manager.puml
  42. 0 0
      tool/StdCondition.cpp
  43. 0 0
      tool/StdCondition.h
  44. 0 0
      tool/binary_buf.cpp
  45. 2 1
      tool/binary_buf.h
  46. 85 0
      tool/binary_buf.puml
  47. 0 0
      tool/thread_condition.cpp
  48. 0 0
      tool/thread_condition.h
  49. 96 0
      tool/thread_condition.puml
  50. 0 0
      tool/thread_safe_queue.cpp
  51. 35 10
      tool/thread_safe_queue.h
  52. 108 0
      tool/thread_safe_queue.puml

+ 2 - 2
CMakeLists.txt

@@ -13,8 +13,8 @@ FIND_PACKAGE(Glog REQUIRED)
 set(CMAKE_MODULE_PATH "/usr/local/share/")
 
 set(CMAKE_CXX_FLAGS "-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
-set(CMAKE_BUILD_TYPE "RELEASE")
-
+#set(CMAKE_BUILD_TYPE "RELEASE")
+set(CMAKE_BUILD_TYPE Debug)
 
 #find_package(Eigen3 REQUIRED)
 

+ 39 - 3
error_code/error_code.cpp

@@ -317,7 +317,7 @@ bool Error_manager::is_equal_error_manager(const Error_manager & error_manager)
 }
 //比较并覆盖错误,讲低级错误转为字符串存放于描述中,
 //如果错误相同,则保留this的,将输入参数转入描述。
-void Error_manager::compare_and_cover_error( Error_manager & error_manager)
+void Error_manager::compare_and_cover_error(const Error_manager & error_manager)
 {
     if(this->m_error_code == SUCCESS)
     {
@@ -325,7 +325,7 @@ void Error_manager::compare_and_cover_error( Error_manager & error_manager)
     }
     else if (error_manager.m_error_code == SUCCESS)
     {
-        //
+		return;
     }
     else
     {
@@ -345,12 +345,48 @@ void Error_manager::compare_and_cover_error( Error_manager & error_manager)
         {
             t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + error_manager.m_description_length;
             pt_string_inside = (char*)malloc(t_string_inside_length);
-            error_manager.translate_error_to_string(pt_string_inside, t_string_inside_length);
+			((Error_manager & )error_manager).translate_error_to_string(pt_string_inside, t_string_inside_length);
 
             add_error_description(pt_string_inside,t_string_inside_length);
         }
     }
 }
+//比较并覆盖错误,讲低级错误转为字符串存放于描述中,
+//如果错误相同,则保留this的,将输入参数转入描述。
+void Error_manager::compare_and_cover_error( Error_manager * p_error_manager)
+{
+	if(this->m_error_code == SUCCESS)
+	{
+		error_manager_reset(*p_error_manager);
+	}
+	else if (p_error_manager->m_error_code == SUCCESS)
+	{
+		//
+	}
+	else
+	{
+		Error_manager t_error_manager_new;
+		char* pt_string_inside = NULL;
+		int t_string_inside_length = 0;
+		if(this->m_error_level < p_error_manager->m_error_level)
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + this->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			error_manager_reset(*p_error_manager);
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+		else
+		{
+			t_string_inside_length = ERROR_NAMAGER_TO_STRING_FRONT_LENGTH + p_error_manager->m_description_length;
+			pt_string_inside = (char*)malloc(t_string_inside_length);
+			p_error_manager->translate_error_to_string(pt_string_inside, t_string_inside_length);
+
+			add_error_description(pt_string_inside,t_string_inside_length);
+		}
+	}
+}
 
 //将所有的错误信息,格式化为字符串,用作日志打印。
 //output:p_error_description     错误汇总的字符串指针,不可以为NULL,必须要有实际的内存

+ 19 - 12
error_code/error_code.h

@@ -71,20 +71,24 @@ enum Error_code
 //    注:错误码的制定从1开始,不要从0开始,
 //        0用作错误码的基数,用来位运算,来判断错误码的范围。
 
-//    laser模块
+//    laser扫描模块
     LASER_ERROR_BASE                = 0x01000000,
 
-//    laser.cpp文件
-
+//    laser_base基类
+	LASER_BASE_ERROR_BASE			= 0x01010000,
     LASER_TASK_PARAMETER_ERROR      = 0x01010001,   //雷达任务输入参数错误
-    LASER_TASK_TYPE_ERROR,
     LASER_CONNECT_FAILED,
-    LASER_LIVOX_SKD_INIT_FAILED,
+	LASER_START_FAILED,
+	LASER_CHECK_FAILED				,
+	LASER_STATUS_ERROR,								//雷达状态错误
 
 
-//    laser_livox.cpp的错误码
+    LASER_LIVOX_SKD_INIT_FAILED,
+
+//    livox子类
     LIVOX_ERROR_BASE                = 0x01020000,
     LIVOX_START_FAILE               = 0x01020101,
+	LIVOX_TASK_TYPE_ERROR,							//livox任务类型错误
 
 
     //PLC error code  ...
@@ -281,12 +285,15 @@ public://外部接口函数
 
     //比较错误是否相同,
     // 注:只比较错误码和等级
-    bool is_equal_error_manager(const Error_manager & error_manager);
-    //比较并覆盖错误,讲低级错误转为字符串存放于描述中,
-    //如果错误相同,则保留this的,将输入参数转入描述。
-    void compare_and_cover_error( Error_manager & error_manager);
-
-    //将所有的错误信息,格式化为字符串,用作日志打印。
+	bool is_equal_error_manager(const Error_manager & error_manager);
+	//比较并覆盖错误,讲低级错误转为字符串存放于描述中,
+	//如果错误相同,则保留this的,将输入参数转入描述。
+	void compare_and_cover_error(const Error_manager & error_manager);
+	//比较并覆盖错误,讲低级错误转为字符串存放于描述中,
+	//如果错误相同,则保留this的,将输入参数转入描述。
+	void compare_and_cover_error( Error_manager * p_error_manager);
+
+	//将所有的错误信息,格式化为字符串,用作日志打印。
     //output:p_error_description     错误汇总的字符串指针,不可以为NULL,必须要有实际的内存
     //output:description_length      错误汇总的字符串长度,不可以为0,长度最好足够大,一般256即可。
     void translate_error_to_string(char* p_error_aggregate, int aggregate_length);

+ 313 - 188
laser/Laser.cpp

@@ -19,53 +19,110 @@ Laser_base::Laser_base(int laser_id, Laser_proto::laser_parameter laser_param)
 }
 Laser_base::~Laser_base()
 {
-	m_queue_laser_data.termination_queue();
+	
+	disconnect_laser();
 	close_save_path();
-	m_condition_receive.kill_all();
-	m_condition_transform.kill_all();
-	m_condition_publish.kill_all();
 }
 
 //雷达链接设备,为3个线程添加线程执行函数。
 Error_manager Laser_base::connect_laser()
 {
-	m_bThreadRcvRun.notify_all(true);
-	if(m_ThreadRcv==0)
-		m_ThreadRcv = new std::thread(&Laser_base::thread_recv, this);
-	m_ThreadRcv->detach();
-
-	m_bThreadProRun.notify_all(true);
-	if(m_ThreadPro==0)
-		m_ThreadPro = new std::thread(&Laser_base::thread_toXYZ, this);
-	m_ThreadPro->detach();
+	//检查雷达状态
+	if ( m_laser_statu != LASER_DISCONNECT )
+	{
+	    return Error_manager(Error_code::LASER_STATUS_ERROR, Error_level::MINOR_ERROR,
+	    					" Laser_base::connect_laser  m_laser_statu != LASER_DISCONNECT ");
+	}
+	else
+	{
+		//这里不建议用detach,而是应该在disconnect里面使用join
+		//创建接受缓存的线程,不允许重复创建
+		if (mp_thread_receive != NULL)
+		{
+			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
+								 " mp_thread_receive is alive ");
+		}
+		else
+		{
+			//接受缓存的线程,线程存活,暂停运行
+			m_condition_receive.reset(false, false, false);
+			mp_thread_receive = new std::thread(&Laser_base::thread_receive, this);
+		}
+		//转化数据的线程,不允许重复创建
+		if (mp_thread_transform != NULL)
+		{
+			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
+								 " mp_thread_transform is alive ");
+		}
+		else
+		{
+			//转化数据的线程,线程存活,暂停运行
+			m_condition_transform.reset(false, false, false);
+			mp_thread_transform = new std::thread(&Laser_base::thread_transform, this);
+		}
+		//发布信息的线程,不允许重复创建
+		if (mp_thread_publish != NULL)
+		{
+			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
+								 " mp_thread_publish is alive ");
+		}
+		else
+		{
+			//发布信息的线程,线程存活,循环运行,
+			//注:mp_thread_publish 需要线程持续不断的往上位机发送信息,不受任务的影响。
+			m_condition_publish.reset(false, true, false);
+			mp_thread_publish = new std::thread(&Laser_base::thread_publish, this);
+		}
 
-	if(m_ThreadPub==0)
-		m_ThreadPub = new std::thread(threadPublish, this);
-	m_ThreadPub->detach();
+		m_laser_statu = LASER_READY;
 
+	}
 	return Error_code::SUCCESS;
 }
 //雷达断开链接,释放3个线程
 Error_manager Laser_base::disconnect_laser()
 {
-	if (m_ThreadRcv)
+	//终止队列,防止 wait_and_pop 阻塞线程。
+	m_queue_laser_data.termination_queue();
+	//杀死3个线程,强制退出
+	if (mp_thread_receive)
 	{
-		delete m_ThreadRcv;
-		m_ThreadRcv = 0;
+		m_condition_receive.kill_all();
 	}
-
-	if (m_ThreadPub)
+	if (mp_thread_transform)
+	{
+		m_condition_transform.kill_all();
+	}
+	if (mp_thread_publish)
+	{
+		m_condition_publish.kill_all();
+	}
+	//回收3个线程的资源
+	if (mp_thread_receive)
+	{
+		mp_thread_receive->join();
+		delete mp_thread_receive;
+		mp_thread_receive = NULL;
+	}
+	if (mp_thread_transform)
+	{
+		mp_thread_transform->join();
+		delete mp_thread_transform;
+		mp_thread_transform = 0;
+	}
+	if (mp_thread_publish)
 	{
-		delete m_ThreadPub;
-		m_ThreadPub = 0;
+		mp_thread_publish->join();
+		delete mp_thread_publish;
+		mp_thread_publish = NULL;
 	}
+	//清空队列
+	m_queue_laser_data.clear_and_delete();
 
-	if (m_ThreadPro)
+	if ( m_laser_statu != LASER_FAULT  )
 	{
-		delete m_ThreadPro;
-		m_ThreadPro = NULL;
+		m_laser_statu = LASER_DISCONNECT;
 	}
-	m_laser_statu = LASER_DISCONNECT;
 	return Error_code::SUCCESS;
 }
 
@@ -90,92 +147,83 @@ Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
 //检查雷达状态,是否正常运行
 Error_manager Laser_base::check_laser()
 {
-	return Error_code::SUCCESS;
+	if ( is_ready() )
+	{
+		return Error_code::SUCCESS;
+	}
+	return Error_manager(Error_code::LASER_CHECK_FAILED, Error_level::MINOR_ERROR,
+						" check_laser failed ");
 }
 //雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
 Error_manager Laser_base::start_scan()
 {
-	m_laser_scan_flag=true;
-	m_laser_statu = LASER_BUSY;
-	m_bStart_capture.notify_all(true);
-	m_points_count=0;
-	return Error_code::SUCCESS;
+	LOG(INFO) << " Laser_base::start_scan "<< this;
+	if ( is_ready() )
+	{
+		//启动雷达扫描
+		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
+	{
+		return Error_manager(Error_code::LASER_START_FAILED, Error_level::MINOR_ERROR,
+							 "Laser_base::start_scan() is not ready ");
+	}
 }
 //雷达的停止接口函数, 让雷达停止扫描,一般需要子类重载,不同的雷达结束方式不同。
 Error_manager Laser_base::stop_scan()
 {
+	LOG(INFO) << " Laser_base::stop_scan "<< this;
+	//stop_scan 只是将 m_laser_scan_flag 改为 stop。
+	//然后多线程内部判断 m_laser_scan_flag,然后自动停止线程
 	m_laser_scan_flag=false;
-	m_bStart_capture.notify_all(false);
-	m_binary_log_tool.close();
-	m_points_log_tool.close();
-	m_laser_statu=LASER_READY;
-
-	//在停止雷达时,将雷达任务状态改为 TASK_OVER 已结束
-	//强制改为TASK_OVER,不管它当前在做什么。
-	if(mp_laser_task !=NULL)
-	{
-		mp_laser_task->set_task_statu(TASK_OVER);
-	}
-
 	return Error_code::SUCCESS;
 }
 
 //结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
 Error_manager Laser_base::end_task()
 {
+	LOG(INFO) << " Laser_base::end_task "<< this;
+	m_laser_scan_flag=false;
+	m_condition_receive.notify_all(false);
+	m_condition_transform.notify_all(false);
+	m_points_count=0;
+	m_queue_laser_data.clear_and_delete();
+	m_last_data.clear();
+
+	close_save_path();
+
+	//在结束任务单时,将雷达任务状态改为 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;
+			}
+
+		}
+		else
+		{
+			//故障等级为 MAJOR_ERROR CRITICAL_ERROR,则雷达故障,不可以再接受任务单。
+			m_laser_statu = LASER_FAULT;
+		}
+		//强制改为TASK_OVER,不管它当前在做什么。
+		mp_laser_task->set_task_statu(TASK_OVER);
+	}
 	return Error_code::SUCCESS;
 }
-/*
-//对外的接口函数,负责接受并处理任务单,
-//input:laser_task 雷达任务单,必须是子类,并且任务正确。(传引用)
-Error_manager Laser_base::porform_task(Laser_task* p_laser_task)
-{
-    Error_manager t_error_manager;
-    //检查指针
-    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");
-    }
-    else
-    {
-        //接受任务,并将任务的状态改为TASK_SIGNED已签收
-        mp_laser_task = p_laser_task;
-        mp_laser_task->set_task_statu(TASK_SIGNED);
-    }
-
-    //检查消息内容是否正确,
-    //检查点云指针
-    if(p_laser_task->get_task_point3d_cloud_vector() == NULL)
-    {
-        t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
-                                            Error_level::MINOR_ERROR,
-                                            "Laser_base::porform_task failed, POINTER_IS_NULL");
-    }
-        //检查任务类型
-    else if(p_laser_task->get_task_type() != LASER_TASK)
-    {
-        t_error_manager.error_manager_reset(Error_code::LASER_TASK_PARAMETER_ERROR,
-                                            Error_level::MINOR_ERROR,
-                                            "Laser_base::porform_task failed, task_type is error");
-    }
-    else
-    {
-        //将任务的状态改为 TASK_WORKING 处理中
-        mp_laser_task->set_task_statu(TASK_WORKING);
-
-        //启动雷达扫描
-        Start();
-    }
-
-    //返回错误码
-    if(t_error_manager != Error_code::SUCCESS)
-    {
-        mp_laser_task->set_task_error_manager(t_error_manager);
-    }
-    return t_error_manager;
-}
-*/
+
 
 
 //设置保存文件的路径,并打开文件,
@@ -195,25 +243,30 @@ Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
 		char pts_file[255] = { 0 };
 		sprintf(pts_file, "%s/pts%d.txt", save_path.c_str(), m_laser_id+1);
 		m_points_log_tool.open(pts_file);
+		std::cout << "huli m_points_log_tool path "<< pts_file << std::endl;
 
 		char bin_file[255] = { 0 };
 		sprintf(bin_file, "%s/laser%d.data", save_path.c_str(), m_laser_id+1);
 		m_binary_log_tool.open(bin_file,true);
+		std::cout << "huli m_binary_log_tool path "<< bin_file << std::endl;
+
 		return Error_code::SUCCESS;
 	}
 }
 //关闭保存文件,推出前一定要执行
 Error_manager Laser_base::close_save_path()
 {
+	m_save_flag = false;
 	m_points_log_tool.close();
 	m_binary_log_tool.close();
 	return Error_code::SUCCESS;
 }
 
 //判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
 bool Laser_base::is_ready()
 {
-	return ( m_laser_statu == LASER_READY );
+	return ( get_laser_statu() == LASER_READY );
 }
 //获取雷达状态
 Laser_statu Laser_base::get_laser_statu()
@@ -226,135 +279,183 @@ int Laser_base::get_laser_id()
 	return m_laser_id;
 }
 
-
-void Laser_base::thread_recv()
+//线程执行函数,将二进制消息存入队列缓存,
+void Laser_base::thread_receive()
 {
-	while (m_bThreadRcvRun.wait_for_millisecond(1))
+	LOG(INFO) << " thread_receive start "<< this;
+	//接受雷达消息,每次循环只接受一个 Binary_buf
+	while (m_condition_receive.is_alive())
 	{
-		if (m_bStart_capture.wait_for_millisecond(1) == false)
-			continue;
-		Binary_buf* data=new Binary_buf();
-		if (this->RecvData(*data))
+		m_condition_receive.wait();
+		if ( m_condition_receive.is_alive() )
 		{
-			m_queue_laser_data.push(data);
-		}
-		else
-		{
-			delete data;
-			data=0;
-		}
+			//tp_binaty_buf的内存 需要手动分配,然后存入链表。
+			//m_queue_laser_data的内存由 Laser_base 基类管理。
+			Binary_buf* tp_binaty_buf = new Binary_buf();
+			//获取雷达的通信消息缓存
+			if (this->receive_buf_to_queue(*tp_binaty_buf))
+			{
+				//将缓存 存入队列。
+				//thread_receive存入前要手动分配内存,thread_transform取出后要记得释放。
+				m_queue_laser_data.push(tp_binaty_buf);
+				std::cout << "huli thread_receive " << m_queue_laser_data.size() << std::endl;
+			}
+			else
+			{
+				delete tp_binaty_buf;
+				tp_binaty_buf=NULL;
 
+				//接受线程无法接受到数据的同时,也收到了雷达停止扫描的信息,那么线程停止
+				//如果m_laser_scan_flag停止扫描,但是receive_buf_to_queue仍然能够接受到数据。那么线程继续运行。
+				//m_laser_scan_flag停止的瞬间,可能还有残留的数据,需要继续执行。
+				if ( !m_laser_scan_flag )
+				{
+					//停止线程,m_condition_receive.wait() 函数将会阻塞。
+					m_condition_receive.set_pass_ever(false);
+				}
+			}			
+		}
 	}
+	LOG(INFO) << " thread_receive end :"<<this;
+	return;
 }
 
 
-
-void Laser_base::thread_toXYZ()
+//线程执行函数,转化并处理三维点云。
+void Laser_base::thread_transform()
 {
-	while (m_bThreadProRun.wait_for_millisecond(1))
+	LOG(INFO) << " thread_transform start "<< this;
+	//转化雷达数据,每次循环只转化一个 Binary_buf,然后得到 t_point3D_cloud.size() 个三维点,
+	while (m_condition_transform.is_alive())
 	{
-		if (m_bStart_capture.wait_for_millisecond(1) == false)
+		m_condition_transform.wait();
+		if ( m_condition_transform.is_alive() )
 		{
-			m_queue_laser_data.clear();
-			continue;
-		}
-		Binary_buf* pData=NULL;
-		bool bPop = m_queue_laser_data.try_pop(pData);
-		Buf_type type = BUF_UNKNOW;
-		if (bPop || m_last_data.get_length() != 0)
-		{
-			std::vector<CPoint3D> cloud;
-			if (bPop)
+			//tp_binaty_buf的内存 从链表取出时,就自带内存了。需要手动释放。
+			//m_queue_laser_data的内存由 Laser_base 基类管理。
+			Binary_buf* tp_binaty_buf=NULL;
+			//第1步,从队列中取出缓存。
+			int huli_test = m_queue_laser_data.size();
+			//thread_receive存入前要手动分配内存,thread_transform取出后要记得释放。
+			bool t_pop_flag = m_queue_laser_data.try_pop(tp_binaty_buf);
+			if ( t_pop_flag )
 			{
-				if (pData == NULL)
-					continue;
-				if(m_save_flag)
-				{
-					m_binary_log_tool.write(pData->get_buf(), pData->get_length());
-				}
-				type= this->Data2PointXYZ(pData, cloud);
-			}
-			else
-			{
-				type = this->Data2PointXYZ(NULL, cloud);
-			}
-
+			    std::cout << "huli m_queue_laser_data.size() last = " << huli_test << std::endl;
+				std::cout << "huli m_queue_laser_data.size() now = " << m_queue_laser_data.size() << std::endl;
 
-			if (type == BUF_UNKNOW)
-			{
-				delete pData;
-				continue;
 			}
 
-			if (type == BUF_DATA || type == BUF_START || type == BUF_STOP)
+			//第2步,处理数据,缓存转化缓存为三维点。
+			//如果获取到了 Binary_buf,或者上次有未完成的 m_last_data,那么就执行数据转化。
+			//注注注注注意了,扫描停止后,m_last_data一定要保证清空,不能有垃圾数据残留,这会阻塞线程的暂停
+			if (t_pop_flag || m_last_data.get_length() > 0)
 			{
-				for (int i = 0; i < cloud.size(); ++i)
+				std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
+				std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
+				//缓存类型
+				Buf_type t_buf_type = BUF_UNKNOW;
+				//雷达扫描结果,三维点云(雷达自身坐标系)
+				std::vector<CPoint3D> t_point3D_cloud;
+				//第2.1步,缓存转化为三维点。
+				if (t_pop_flag)
 				{
-					CPoint3D point = transform_by_matrix(cloud[i]);
-					if(m_save_flag) {
-						char buf[64] = {0};
-						sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
-						m_points_log_tool.write(buf, strlen(buf));
-					}
-
-					//此时cloud为雷达采集数据的结果,转换后的三维点云。
-					//将每个点存入任务单里面的点云容器。
-
-					if(mp_laser_task!=NULL)
+					if (tp_binaty_buf == NULL)
 					{
-						Error_manager code= mp_laser_task->task_push_point(pcl::PointXYZ(point.x,point.y,point.z));
+						//try_pop返回true,但是tp_binaty_buf没有数据,直接跳过。这个一般不可能出现。
+						continue;
 					}
+					else
+					{
+						//保存雷达通信 二进制的源文件。
+						if(m_save_flag)
+						{
+							m_binary_log_tool.write(tp_binaty_buf->get_buf(), tp_binaty_buf->get_length());
+						}
+						//缓存转化为三维点。传入新的缓存,
+						t_buf_type= this->transform_buf_to_points(tp_binaty_buf, t_point3D_cloud);
+					}					
 				}
-				m_points_count+=cloud.size();
-				if (type == BUF_STOP)
+				else if(m_last_data.get_length() > 0)
 				{
-					if(m_save_flag) {
-						m_points_log_tool.close();
-						m_binary_log_tool.close();
-					}
-					m_laser_statu = LASER_READY;
+					//缓存转化为三维点。没有新消息,就传null,这将处理 m_last_data 上一次遗留的缓存。
+					t_buf_type = this->transform_buf_to_points(NULL, t_point3D_cloud);
+				}
 
-					//在停止雷达时,将雷达任务状态改为 TASK_OVER 已结束
-					if(mp_laser_task !=NULL)
+				//第2.2步,判断t_buf_type,处理雷达数据。并存入任务单
+				if (t_buf_type == BUF_UNKNOW || t_buf_type == BUF_READY)
+				{
+					//跳过这次循环。
+					continue;
+				}
+				else if (t_buf_type == BUF_DATA || t_buf_type == BUF_START || t_buf_type == BUF_STOP)
+				{
+					//循环处理雷达数据
+					for (int i = 0; i < t_point3D_cloud.size(); ++i)
 					{
-						if(mp_laser_task->get_statu() == TASK_WORKING)
+						//三维点的坐标变换,从雷达自己的坐标系,转化到公共坐标系,(目前以plc为公共坐标系)
+						CPoint3D t_point = transform_by_matrix(t_point3D_cloud[i]);
+						//保存雷达扫描 三维点云的最终结果。
+						if(m_save_flag) {
+							char buf[64] = {0};
+							sprintf(buf, "%f %f %f\n", t_point.x, t_point.y, t_point.z);
+							m_points_log_tool.write(buf, strlen(buf));
+//							std::cout << "huli m_points_log_tool.write" << std::endl;
+						}
+
+						//此时 t_point 为雷达采集数据的结果,转换后的三维点云。
+						//将每个点存入任务单里面的点云容器。
+						if(mp_laser_task!=NULL)
 						{
-							mp_laser_task->set_task_statu(TASK_OVER);
+							Error_manager t_error= mp_laser_task->task_push_point(pcl::PointXYZ(t_point.x,t_point.y,t_point.z));
+							if ( t_error != Error_code::SUCCESS )
+							{
+								mp_laser_task->get_task_error_manager().compare_and_cover_error(t_error);
+								stop_scan();
+							}
 						}
 					}
+					//统计扫描点个数。
+					m_points_count += t_point3D_cloud.size();
+					//思科雷达停止扫描。
+					if (t_buf_type == BUF_STOP)
+					{
+						//注:这个只是思科的雷达停止处,不同的雷达在不同的地方调用 stop_scan
+						stop_scan();
+					}
 				}
-
 			}
-			else if (type == BUF_READY)
+			else
 			{
-				if(m_save_flag) {
-					m_points_log_tool.close();
-					m_binary_log_tool.close();
+				//转化线程 不需要数据的同时,也收到了雷达停止扫描的信息并且接受线程已经停止,那么停止转化线程,
+				//注:必须先停止接受线程,再停止转化线程。转化线程停止之后即可结束任务。
+				//如果m_laser_scan_flag停止扫描,接受线程和转化线程还有未完成的数据。那么线程继续运行。
+				//m_laser_scan_flag停止的瞬间,可能还有残留的数据,需要继续执行。防止数据遗留。
+				if ( !m_laser_scan_flag && m_condition_receive.get_pass_ever() == false)
+				{
+					//停止线程,m_condition_transform.wait() 函数将会阻塞。
+					m_condition_transform.set_pass_ever(false);
+
+					//mp_thread_transform 停止时,雷达扫描任务彻底完成,此时结束任务
+					end_task();
 				}
-				m_laser_statu = LASER_READY;
 			}
 
-
-			delete pData;
+			//第3步,手动释放缓存
+			if ( tp_binaty_buf != NULL )
+			{
+				delete tp_binaty_buf;
+			}
 		}
-
 	}
+	LOG(INFO) << " thread_transform end "<< this;
+	return;
 }
 
 
-#include "unistd.h"
-void Laser_base::threadPublish(Laser_base *laser)
-{
-	if(laser==0) return ;
-	while(laser->m_bThreadRcvRun.wait_for_millisecond(1))
-	{
-		laser->PublishMsg();
-		usleep(1000*300);
-	}
-}
-
-void Laser_base::PublishMsg()
+//公开发布雷达信息的功能函数,
+Error_manager Laser_base::publish_laser_to_message()
 {
+	return Error_code::SUCCESS;
 	/*
 	globalmsg::msg msg;
 	msg.set_msg_type(globalmsg::eLaser);
@@ -372,6 +473,30 @@ void Laser_base::PublishMsg()
 	MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
 	 */
 }
+//线程执行函数,公开发布雷达的相关信息,用作上位机的监视。
+void Laser_base::thread_publish(Laser_base *p_laser)
+{
+	LOG(INFO) << " thread_publish start ";
+	if(p_laser==NULL)
+	{
+		return;
+	}
+
+	while (p_laser->m_condition_publish.is_alive())
+	{
+		p_laser->m_condition_publish.wait();
+		if ( p_laser->m_condition_publish.is_alive() )
+		{
+			p_laser->publish_laser_to_message();
+			//每隔300ms,发送一次雷达信息状态。
+			std::this_thread::sleep_for(std::chrono::milliseconds(300));
+		}
+	}
+
+	LOG(INFO) << " thread_publish end ";
+	return;
+}
+
 
 
 //初始化变换矩阵,设置默认值

+ 25 - 28
laser/Laser.h

@@ -1,5 +1,4 @@
 
-
 #ifndef __LASER__HH__
 #define __LASER__HH__
 #include "Point2D.h"
@@ -19,14 +18,16 @@
 
 
 
-//雷达的工作状态,
-//在start和stop中要切换状态
+//雷达工作状态,基类三线程的状态
+//connect_laser 和  disconnect_laser  	中要切换状态 LASER_DISCONNECT <===> LASER_READY
+//start_scan 和 stop_scan 和 end_task	中要切换状态 LASER_READY <===> LASER_BUSY
 enum Laser_statu
-{//默认值 LASER_DISCONNECT	=0
+{
+	//默认值 LASER_DISCONNECT	=0
 	LASER_DISCONNECT	=0,	        //雷达断连
 	LASER_READY			=1,			//雷达正常待机,空闲
 	LASER_BUSY			=2,	        //雷达正在工作,正忙
-	LASER_ERROR			=3,         //雷达错误
+	LASER_FAULT			=3,         //雷达错误
 };
 
 //雷达变换矩阵的数组长度,默认为12个参数, size = 3 * ( 3 + 1 ) ,旋转加平移,没有缩放
@@ -41,7 +42,7 @@ public:
 	//唯一的构造函数,按照设备名称和雷达参数来创建实例。
 	//input:id: 雷达设备的id,(唯一索引)
 	//input:laser_param:雷达的参数,
-	//注:利用protobuf创建stLaserCalibParam类,然后从文件读取参数
+	//注:利用protobuf创建 laser_parameter 类,然后从文件读取参数
 	Laser_base(int laser_id,Laser_proto::laser_parameter laser_param);
 	//析构函数
 	~Laser_base();
@@ -69,9 +70,9 @@ public:
 	//关闭保存文件,推出前一定要执行
 	Error_manager close_save_path();
 	//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
-	bool is_ready();
-	//获取雷达状态
-	virtual Laser_statu get_laser_statu();
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+	virtual bool is_ready();
+
 	//获取雷达id
 	int get_laser_id();
 
@@ -79,19 +80,23 @@ public:
 protected:
 	//接受二进制消息的功能函数,每次只接受一个CBinaryData
 	// 纯虚函数,必须由子类重载,
-	virtual bool RecvData(Binary_buf& data) = 0;
+	virtual bool receive_buf_to_queue(Binary_buf& binary_buf) = 0;
 	//线程执行函数,将二进制消息存入队列缓存,
-	void thread_recv();
+	void thread_receive();
+
 	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
 	// 纯虚函数,必须由子类重载,
-	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)=0;
+	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)=0;
 	//线程执行函数,转化并处理三维点云。
-	void thread_toXYZ();
-	//线程执行函数,公开发布雷达的相关信息,用作上位机的监视。
-	static void threadPublish(Laser_base* laser);
+	void thread_transform();
+
 	//公开发布雷达信息的功能函数,
-	void PublishMsg();
+	Error_manager publish_laser_to_message();
+	//线程执行函数,公开发布雷达的相关信息,用作上位机的监视。
+	static void thread_publish(Laser_base* p_laser);
 
+	//获取雷达状态
+	Laser_statu get_laser_statu();
 protected:
 	//初始化变换矩阵,设置默认值
 	Error_manager init_laser_matrix();
@@ -115,22 +120,21 @@ protected:
 	//雷达扫描事件的标志位,受start和stop控制,然后其他线程判断m_scan_flag标准位,来进行启停。
 	std::atomic<bool>                	m_laser_scan_flag;          //雷达扫描的使能标志位
 	//雷达状态和任务状态同步,m_scan_flag停止之后,还要等任务执行完成,才会切回 ready。
-	std::atomic<Laser_statu>			m_laser_statu;             	//雷达状态
+	std::atomic<Laser_statu>			m_laser_statu;             	//雷达工作状态,基类三线程的状态
+	//注:m_laser_statu是基类的三线程的状态,和livox sdk后台线程状态没有任何关系。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
 
 	std::atomic<int>            		m_points_count;         	//雷达采集点的计数
 	Thread_safe_queue<Binary_buf*>		m_queue_laser_data;         //二进制缓存的队列容器
 	Binary_buf							m_last_data;			    //上一个二进制缓存,用作数据拼接
 
+
 	std::atomic<bool> 					m_save_flag;           		//雷达保存文件的使能标志位
 	std::string							m_save_path;            	//雷达保存文件的保存路径
 	CLogFile							m_binary_log_tool;          //二进制缓存的日志工具
 	CLogFile							m_points_log_tool;          //三维点云的日志工具
 
 
-	Thread_condition	m_bStart_capture;           //线程条件???????
-
-
-
 	//线程指针的内存管理,由Connect和Disconnect进行分配和释放。
 	std::thread*						mp_thread_receive;    		//接受缓存的线程指针
 	Thread_condition					m_condition_receive;		//接受缓存的条件变量
@@ -139,13 +143,6 @@ protected:
 	std::thread*        				mp_thread_publish;   		//发布信息的线程指针
 	Thread_condition					m_condition_publish;		//发布信息的条件变量
 
-	//线程指针的内存管理,由Connect和Disconnect进行分配和释放。
-	std::thread*		m_ThreadRcv;            //接受缓存的线程指针
-	std::thread*		m_ThreadPro;            //转化缓存的线程指针
-	std::thread*        m_ThreadPub;            //发布信息的线程指针
-	Thread_condition		m_bThreadRcvRun;        //接受缓存的线程条件
-	Thread_condition		m_bThreadProRun;        //转化缓存的线程条件
-
 	//任务单的指针,实际内存由应用层管理,
 	//接受任务后,指向新的任务单
 	Laser_task *  						mp_laser_task;        		//任务单的指针

+ 300 - 0
laser/Laser.puml

@@ -0,0 +1,300 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  Laser_base
+
+
+
+enum Laser_statu
+{
+//雷达工作状态,基类三线程的状态
+//connect_laser 和  disconnect_laser  	中要切换状态 LASER_DISCONNECT <===> LASER_READY
+//start_scan 和 stop_scan 和 end_task	中要切换状态 LASER_READY <===> LASER_BUSY
+//默认值 LASER_DISCONNECT	=0
+	LASER_DISCONNECT	=0,	        //雷达断连
+	LASER_READY			=1,			//雷达正常待机,空闲
+	LASER_BUSY			=2,	        //雷达正在工作,正忙
+	LASER_FAULT			=3,         //雷达错误
+}
+
+
+
+class Laser_base
+{
+//雷达的基类,不能直接使用,必须子类继承
+==public:==
+	Laser_base() = delete;
+	Laser_base(const Laser_base& other) = delete;
+..
+	//唯一的构造函数,按照设备名称和雷达参数来创建实例。
+	//input:id: 雷达设备的id,(唯一索引)
+	//input:laser_param:雷达的参数,
+	//注:利用protobuf创建 laser_parameter 类,然后从文件读取参数
+	Laser_base(int laser_id,Laser_proto::laser_parameter laser_param);
+	//析构函数
+	~Laser_base();
+..
+	//雷达链接设备,为3个线程添加线程执行函数。
+	virtual Error_manager connect_laser();
+	//雷达断开链接,释放3个线程
+	virtual Error_manager disconnect_laser();
+	//对外的接口函数,负责接受并处理任务单,
+	//input:p_laser_task 雷达任务单,基类的指针,指向子类的实例,(多态)
+	//注:这个函数为虚函数,实际的处理任务的代码由子类重载并实现。
+	virtual Error_manager execute_task(Task_Base* p_laser_task);
+	//检查雷达状态,是否正常运行
+	virtual Error_manager check_laser();
+	//雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
+	virtual Error_manager start_scan();
+	//雷达的停止接口函数, 让雷达停止扫描,一般需要子类重载,不同的雷达结束方式不同。
+	virtual Error_manager stop_scan();
+	//结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
+	virtual Error_manager end_task();
+==public:==
+	//设置保存文件的路径,并打开文件,
+	Error_manager set_open_save_path(std::string save_path,bool is_save=true);
+	//关闭保存文件,推出前一定要执行
+	Error_manager close_save_path();
+	//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+	virtual bool is_ready();
+	//获取雷达id
+	int get_laser_id();
+==protected:==
+	//接受二进制消息的功能函数,每次只接受一个CBinaryData
+	// 纯虚函数,必须由子类重载,
+	virtual bool receive_buf_to_queue(Binary_buf& binary_buf) = 0;
+	//线程执行函数,将二进制消息存入队列缓存,
+	void thread_receive();
+..
+	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+	// 纯虚函数,必须由子类重载,
+	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)=0;
+	//线程执行函数,转化并处理三维点云。
+	void thread_transform();
+..
+	//公开发布雷达信息的功能函数,
+	Error_manager publish_laser_to_message();
+	//线程执行函数,公开发布雷达的相关信息,用作上位机的监视。
+	static void thread_publish(Laser_base* p_laser);
+..
+	//获取雷达状态
+	Laser_statu get_laser_statu();
+==protected:==
+	//初始化变换矩阵,设置默认值
+	Error_manager init_laser_matrix();
+	//设置变换矩阵,用作三维点的坐标变换,
+	Error_manager set_laser_matrix(double* p_matrix, int size);
+..
+	//三维点的坐标变换的功能函数,从雷达自己的坐标系,转化到公共坐标系,(目前以plc为公共坐标系)
+	virtual CPoint3D transform_by_matrix(CPoint3D point);
+==protected:==
+	//为了保证多线程的数据安全,修改共享数据必须加锁。  atomic 和 安全队列 可以不加锁进行读写
+	//建议:判断标志位使用 atomic, 容器要封装并使用 安全容器,
+	std::mutex          				m_laser_lock;            	//雷达数据锁
+..
+	std::atomic<int>					m_laser_id;                 //雷达设备id
+	Laser_proto::laser_parameter  		m_laser_param;				//雷达的配置参数
+	//雷达变换矩阵,三维点的坐标变换的矩阵,从雷达自己的坐标系,转化到公共坐标系,(目前以plc为公共坐标系)
+	//必须在set_laser_matrix之后,才能使用。(connect时,从雷达的配置参数导入)
+	double								mp_laser_matrix[LASER_MATRIX_ARRAY_SIZE];	//雷达变换矩阵
+	//雷达扫描事件的标志位,受start和stop控制,然后其他线程判断m_scan_flag标准位,来进行启停。
+	std::atomic<bool>                	m_laser_scan_flag;          //雷达扫描的使能标志位
+	//雷达状态和任务状态同步,m_scan_flag停止之后,还要等任务执行完成,才会切回 ready。
+	std::atomic<Laser_statu>			m_laser_statu;             	//雷达工作状态,基类三线程的状态
+	//注:m_laser_statu是基类的三线程的状态,和livox sdk后台线程状态没有任何关系。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+..
+	std::atomic<int>            		m_points_count;         	//雷达采集点的计数
+	Thread_safe_queue<Binary_buf*>		m_queue_laser_data;         //二进制缓存的队列容器
+	Binary_buf							m_last_data;			    //上一个二进制缓存,用作数据拼接
+..
+	std::atomic<bool> 					m_save_flag;           		//雷达保存文件的使能标志位
+	std::string							m_save_path;            	//雷达保存文件的保存路径
+	CLogFile							m_binary_log_tool;          //二进制缓存的日志工具
+	CLogFile							m_points_log_tool;          //三维点云的日志工具
+..
+	//线程指针的内存管理,由Connect和Disconnect进行分配和释放。
+	std::thread*						mp_thread_receive;    		//接受缓存的线程指针
+	Thread_condition					m_condition_receive;		//接受缓存的条件变量
+	std::thread*						mp_thread_transform;   		//转化数据的线程指针
+	Thread_condition					m_condition_transform;		//转化数据的条件变量
+	std::thread*        				mp_thread_publish;   		//发布信息的线程指针
+	Thread_condition					m_condition_publish;		//发布信息的条件变量
+..
+	//任务单的指针,实际内存由应用层管理,
+	//接受任务后,指向新的任务单
+	Laser_task *  						mp_laser_task;        		//任务单的指针
+}
+
+
+class CPoint3D
+{
+//三维点
+	double x, y, z;
+}
+
+class CLogFile
+{
+//日志文件
+	void open(const char *_Filename, bool binary = false);
+	void write(const char *_Str, streamsize _Count);
+	void close();
+	fstream m_file;
+    std::mutex m_lock;
+}
+
+class laser_parameter
+{
+//雷达参数
+}
+
+
+class Laser_task
+{
+//雷达模块的任务指令,从Task_Base继承,
+//补充了雷达专属的数据输入和输出
+==public:==
+    //初始化任务单,必须初始化之后才可以使用,(必选参数)
+    Error_manager task_init(Task_statu task_statu,
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
+                            std::mutex* p_task_cloud_lock);
+..
+    //初始化任务单,必须初始化之后才可以使用,(可选参数)
+    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);
+..
+    //push点云,把 point_xyz 添加到 p_task_point_cloud 里面。
+    Error_manager task_push_point(pcl::PointXYZ point_xyz);
+==protected:==
+    //点云的采集帧数最大值,任务输入
+    unsigned int                    m_task_frame_maxnum;
+    //点云保存文件的路径,任务输入
+    std::string                     m_task_save_path;
+    //三维点云的数据保护锁,任务输入
+    std::mutex*                     mp_task_cloud_lock;
+    //采集结果,三维点云容器的智能指针,任务输出
+    //这里只是指针,实际内存由应用层管理,初始化时必须保证内存有效。
+    pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+}
+
+
+class Task_Base
+{
+//任务单基类
+==public:==
+    //初始化任务单,初始任务单类型为 UNKONW_TASK
+    virtual Error_manager init();
+    //更新任务单
+    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
+    //获取任务类型
+    Task_type   get_task_type();
+==protected:==
+    Task_type                   m_task_type;                    //任务类型
+    Task_statu                  m_task_statu;                   //任务状态
+    std::string                 m_task_statu_information;       //任务状态说明
+	Error_manager               m_task_error_manager;//错误码,任务故障信息,任务输出
+}
+
+Laser_task <-- Task_Base : inherit
+
+class Error_manager
+{
+//错误码管理
+}
+
+
+class Binary_buf
+{
+//二进制缓存,
+	//比较前面部分的buf是否相等,使用 other.m_length 为标准
+	bool is_equal_front(const Binary_buf& other);
+	//比较前面部分的buf是否相等,len为0时,使用strlen(buf)为标准,不比较结束符'\0'
+	bool is_equal_front(const char* p_buf, int len = 0);
+..
+	char* get_buf()const;
+	int	get_length()const;
+--
+	char*		mp_buf;				//二进制缓存指针
+	int			m_length;			//二进制缓存长度
+}
+
+
+class Thread_condition
+{
+	bool wait();
+	//等待一定的时间(默认时间单位:毫秒ms),由 is_pass_wait 决定是否阻塞。
+	bool wait_for_millisecond(unsigned int millisecond);
+	//唤醒已经阻塞的线程,唤醒一个线程
+	void notify_one(bool pass_ever, bool pass_once = false);
+	//唤醒已经阻塞的线程,唤醒全部线程
+	void notify_all(bool pass_ever, bool pass_once = false);
+	//杀死所有的线程,强制退出线程函数,只是操作受当前Thread_condition影响的所有线程
+	void kill_all();
+	//判断是否或者,return !m_kill_flag
+	bool is_alive();
+==protected:==
+	static bool is_pass_wait(Thread_condition * other);
+..
+	std::atomic<bool> 		m_kill_flag;			//是否杀死线程,让线程强制退出,
+	std::atomic<bool> 		m_pass_ever;			//线程能否直接通过等待,对后面的线程也生效。
+	std::atomic<bool> 		m_pass_once;			//线程能否直接通过等待,一次(通过一次之后,wait里面自动改为false)
+	std::mutex 				m_mutex;				//线程的锁
+	std::condition_variable m_condition_variable;	//线程的条件变量
+}
+
+
+class Thread_safe_queue <<    template<class T>    >>
+{
+	//等待并弹出数据,成功弹出则返回true
+	bool wait_and_pop(T& value);
+	//尝试弹出数据,成功弹出则返回true
+	bool try_pop(T& value);
+	//等待并弹出数据,成功弹出则返回true
+	std::shared_ptr<T> wait_and_pop();
+	//尝试弹出数据,成功弹出则返回true
+	std::shared_ptr<T> try_pop();
+	//插入一项,并唤醒一个线程,
+	bool push(T new_value);
+	//清除队列,只是将队列的实例抛出。T是实例内存,系统自动回收的。
+	bool clear();
+	//清除队列,抛出之后还要delete指针。T是动态内存,需要手动回收的。
+	bool clear_and_delete();
+==public:==
+	//判空
+	bool empty();
+	//获取队列大小
+	size_t size();
+	//设置队列为退出状态。并唤醒所有的线程,使其通过wait
+	void termination_queue();
+	//唤醒队列,恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_queue();
+	//获取退出状态
+	bool get_termination_flag();
+	//判断是否可以直接通过wait,  m_data_queue不为空或者m_termination终止时都可以通过等待。
+	bool is_pass();
+==protected:==
+	std::mutex 						m_mutex;				//队列的锁
+	std::queue<std::shared_ptr<T>> 	m_data_queue;			//队列数据,使用智能指针shared_ptr
+	std::condition_variable 		m_data_cond;			//条件变量
+	std::atomic<bool> 				m_termination_flag;		//终止标志位
+}
+
+
+
+Laser_base <-- Laser_statu : include
+Laser_base <-- CPoint3D : include
+Laser_base <-- CLogFile : include
+Laser_base <-- laser_parameter : include
+Laser_base <-- Laser_task : include
+Laser_base <-- Error_manager : include
+Laser_base <-- Binary_buf : include
+Laser_base <-- Thread_condition : include
+Laser_base <-- Thread_safe_queue : include
+
+
+
+@enduml

+ 165 - 60
laser/LivoxLaser.cpp

@@ -18,12 +18,16 @@ unsigned int  CLivoxLaser::g_count[kMaxLidarCount] = { 0 };
 CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
 :Laser_base(id, laser_param)
 {
-
+	//设备livox扫描最大帧数
 	m_frame_maxnum = laser_param.frame_num();
+	//判断参数类型,
 	if(laser_param.type()=="Livox")
+	{
+		//填充雷达设备的广播码
 		g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
-
-	InitLivox();
+		//初始化livox
+		InitLivox();
+	}
 }
 
 CLivoxLaser::~CLivoxLaser()
@@ -46,29 +50,89 @@ Error_manager CLivoxLaser::disconnect_laser()
 //注:这个函数为虚函数,实际的处理任务的代码由子类重载并实现。
 Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
 {
+	LOG(INFO) << " CLivoxLaser::execute_task start  "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
 	//检查指针
-	if(p_laser_task == NULL)
-	{
+	if (p_laser_task == NULL) {
 		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-							 "CLivoxLaser::execute_task failed, POINTER_IS_NULL");
+							 "Laser_base::porform_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型,
+	if (p_laser_task->get_task_type() != LASER_TASK)
+	{
+		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 "laser task type error != LASER_TASK");
+	}
+
+	//接受任务,并将任务的状态改为TASK_SIGNED已签收
+	mp_laser_task = (Laser_task *) p_laser_task;
+	mp_laser_task->set_task_statu(TASK_SIGNED);
+
+	//检查消息内容是否正确,
+	//检查三维点云指针
+	if (mp_laser_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_OVER 结束任务
+		mp_laser_task->set_task_statu(TASK_OVER);
+		//返回错误码
+		mp_laser_task->set_task_error_manager(t_result);
+		return t_result;
 	}
 	else
 	{
-		return Error_manager(Error_code::CLASS_BASE_FUNCTION_CANNOT_USE, Error_level::NEGLIGIBLE_ERROR,
-							 "CLivoxLaser::execute_task cannot use");
+		m_frame_maxnum=mp_laser_task->get_task_frame_maxnum();
+
+
+		//设置保存文件的路径
+		std::string save_path = mp_laser_task->get_task_save_path();
+		t_error = set_open_save_path(save_path);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			//文件保存文件的路径的设置 允许失败。继续后面的动作
+			t_result.compare_and_cover_error(t_error);
+		}
+		//启动雷达扫描
+		t_error = start_scan();
+		if ( t_error != Error_code::SUCCESS )
+		{
+			t_result.compare_and_cover_error(t_error);
+
+			//将任务的状态改为 TASK_OVER 结束任务
+			mp_laser_task->set_task_statu(TASK_OVER);
+			//返回错误码
+			mp_laser_task->set_task_error_manager(t_result);
+			return t_result;
+		}
+		else
+		{
+			//将任务的状态改为 TASK_WORKING 处理中
+			mp_laser_task->set_task_statu(TASK_WORKING);
+		}
+	}
+	//返回错误码
+	if (t_result != Error_code::SUCCESS)
+	{
+		mp_laser_task->set_task_error_manager(t_result);
 	}
+	return t_result;
 }
+
 //检查雷达状态,是否正常运行
 Error_manager CLivoxLaser::check_laser()
 {
-	return Error_code::SUCCESS;
+	return Laser_base::check_laser();
 }
 //雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
 Error_manager CLivoxLaser::start_scan()
 {
 	LOG(INFO) << " livox start :"<<this;
-	//???????????
-	m_queue_livox_data.clear();
+	//清空livox子类的队列,
+	m_queue_livox_data.clear_and_delete();
 	g_count[m_handle] = 0;
 	return Laser_base::start_scan();
 }
@@ -80,62 +144,78 @@ Error_manager CLivoxLaser::stop_scan()
 //结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
 Error_manager CLivoxLaser::end_task()
 {
-	return Error_code::SUCCESS;
+	return Laser_base::end_task();
 }
 
-bool CLivoxLaser::RecvData(Binary_buf& data)
+//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+bool CLivoxLaser::is_ready()
 {
-	Binary_buf* livox_data;
-	if (m_queue_livox_data.try_pop(livox_data))
+	//livox雷达设备的状态,livox sdk后台线程的状态
+	if ( m_laser_statu == LASER_READY &&
+		 g_devices[m_handle].device_state != kDeviceStateDisconnect  )
 	{
-		data = *livox_data;
-		delete livox_data;
-		return true;
+		true;
 	}
-	return false;
-}
-Buf_type CLivoxLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
-{
-	LivoxRawPoint *p_point_data = (LivoxRawPoint *)pData->get_buf();
-	int count = pData->get_length() / (sizeof(LivoxRawPoint));
-
-	if (count <= 0)
-		return BUF_UNKNOW;
-	for (int i = 0; i < count; ++i)
+	else
 	{
-		LivoxRawPoint point = p_point_data[i];
-		CPoint3D pt;
-		pt.x = point.x;
-		pt.y = point.y;
-		pt.z = point.z;
-		points.push_back(pt);
+	    false;
 	}
-	return BUF_DATA;
 }
 
-Laser_statu CLivoxLaser::get_laser_statu()
+//接受二进制消息的功能函数,每次只接受一个CBinaryData
+// 纯虚函数,必须由子类重载,
+bool CLivoxLaser::receive_buf_to_queue(Binary_buf& binary_buf)
 {
-	if(g_devices[m_handle].device_state == kDeviceStateConnect||
-	   g_devices[m_handle].device_state == kDeviceStateSampling)
+	Binary_buf* tp_livox_buf;
+	if (m_queue_livox_data.try_pop(tp_livox_buf))
 	{
-		m_laser_statu=LASER_READY;
+		binary_buf = *tp_livox_buf;
+		delete tp_livox_buf;
+		std::cout << "huli m_queue_livox_data.try_pop" << std::endl;
+		return true;
 	}
-	if(g_devices[m_handle].device_state == kDeviceStateDisconnect)
+	return false;
+}
+//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+// 纯虚函数,必须由子类重载,
+Buf_type CLivoxLaser::transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)
+{
+	if ( p_binary_buf ==NULL )
 	{
-		m_laser_statu=LASER_DISCONNECT;
+		return BUF_UNKNOW;
 	}
-	return m_laser_statu;
-}
-
-
-
-
-
-
-
-
+	else
+	{
+		//livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
+		LivoxRawPoint *tp_Livox_data = (LivoxRawPoint *)p_binary_buf->get_buf();
+		//计算这一帧数据有多少个三维点。
+		int t_count = p_binary_buf->get_length() / (sizeof(LivoxRawPoint));
 
+		if (t_count <= 0)
+		{
+			return BUF_UNKNOW;
+		}
+		else
+		{
+			//转变三维点格式,并存入vector。
+			for (int i = 0; i < t_count; ++i)
+			{
+				//LivoxRawPoint 转为 CPoint3D
+				//int32_t 转 double。不要信号强度
+				LivoxRawPoint t_livox_point = tp_Livox_data[i];
+				CPoint3D t_point3D;
+				t_point3D.x = t_livox_point.x;
+				t_point3D.y = t_livox_point.y;
+				t_point3D.z = t_livox_point.z;
+				point3D_cloud.push_back(t_point3D);
+//				std::cout << "huli points:" << t_point3D.x << " : " << t_point3D.y << " : " << t_point3D.z<< std::endl;
+			}
+			return BUF_DATA;
+		}
+	}
 
+}
 
 
 
@@ -225,11 +305,14 @@ void CLivoxLaser::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
 	if (g_devices[handle].device_state == kDeviceStateConnect)
 	{
 
-		if (g_devices[handle].info.state == kLidarStateNormal) {
-			if (g_devices[handle].info.type == kDeviceTypeHub) {
+		if (g_devices[handle].info.state == kLidarStateNormal)
+		{
+			if (g_devices[handle].info.type == kDeviceTypeHub)
+			{
 				HubStartSampling(OnSampleCallback, NULL);
 			}
-			else {
+			else
+			{
 				LidarStartSampling(handle, OnSampleCallback, NULL);
 			}
 			g_devices[handle].device_state = kDeviceStateSampling;
@@ -271,9 +354,10 @@ void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
 
 void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data)
 {
-	///?????????????? ?????????handle
+//	std::cout << "huli LidarDataCallback" << std::endl;
+
 	CLivoxLaser* livox = g_all_laser[handle];
-	if (livox == 0)
+	if (livox == NULL)
 	{
 		if (g_handle_sn.find(handle) != g_handle_sn.end())
 		{
@@ -287,17 +371,29 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 			}
 		}
 	}
+//	std::cout << "huli LidarDataCallback " << std::endl;
 
 
 	if (data && livox)
 	{
-		if (livox->m_bStart_capture.wait_for_millisecond(1))
+//		std::cout << "huli m_laser_scan_flag = " << livox->m_laser_scan_flag << std::endl;
+
+		//判断雷达扫描标志位
+		if (livox->m_laser_scan_flag)
 		{
 			if (livox->IsScanComplete())
 			{
+				std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
 				livox->stop_scan();
 				return;
 			}
+			else
+			{
+				std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
+
+			}
+//			std::cout << "huli LidarDataCallback " << std::endl;
+
 			if (g_count[handle] >= livox->m_frame_maxnum)
 				return;
 			uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
@@ -305,15 +401,24 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 
 			Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
 			livox->m_queue_livox_data.push(data_bin);
+			std::cout << "huli m_queue_livox_data.push " << std::endl;
+
 			g_count[handle]++;
 
 		}
-		else if(livox->m_laser_statu!=LASER_READY)
+	/*	else
 		{
+			std::cout << "huli 1z error " << "data = "<< data  << std::endl;
+			std::cout << "huli 1z error " << "livox = "<< livox  << std::endl;
+
 			//?????????????????
 			livox->m_laser_statu = LASER_READY;
 			usleep(10*1000);
-		}
+		}*/
+	}
+	else
+	{
+		std::cout << "huli 2z error " << "data = "<< data  << std::endl;
+		std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
 	}
-
 }

+ 30 - 12
laser/LivoxLaser.h

@@ -5,23 +5,34 @@
 #include "livox_sdk.h"
 #include <map>
 
-
+//大疆livox雷达,从Laser_base继承。
 class CLivoxLaser :public Laser_base
 {
 protected:
-	typedef enum {
-		kDeviceStateDisconnect = 0,
-		kDeviceStateConnect = 1,
-		kDeviceStateSampling = 2,
+	//雷达设备状态,livox管理底层sdk,后台线程的工作状态
+	typedef enum
+	{
+		kDeviceStateDisconnect = 0,		//雷达设备状态	断开连接
+		kDeviceStateConnect = 1,		//雷达设备状态	连接正常
+		kDeviceStateSampling = 2,		//雷达设备状态	正在扫描
 	} DeviceState;
-	typedef struct {
-		uint8_t handle;
-		DeviceState device_state;
-		DeviceInfo info;
+
+	//雷达设备信息,
+	typedef struct
+	{
+		uint8_t handle;					//雷达控制句柄
+		DeviceState device_state;		//雷达设备状态
+		DeviceInfo info;				//雷达基本信息
 	} DeviceItem;
 
 
 public:
+	CLivoxLaser() = delete;
+	CLivoxLaser(const CLivoxLaser& other) = delete;
+	//唯一的构造函数,按照设备名称和雷达参数来创建实例。
+	//input:id: 雷达设备的id,(唯一索引)
+	//input:laser_param:雷达的参数,
+	//注:利用protobuf创建 laser_parameter 类,然后从文件读取参数
 	CLivoxLaser(int id, Laser_proto::laser_parameter laser_param);
 	~CLivoxLaser();
 
@@ -42,11 +53,18 @@ public:
 	//结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
 	virtual Error_manager end_task();
 
+	//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+	virtual bool is_ready();
 
 protected:
-	virtual bool RecvData(Binary_buf& data);
-	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
-	virtual Laser_statu get_laser_statu();
+	//接受二进制消息的功能函数,每次只接受一个CBinaryData
+	// 纯虚函数,必须由子类重载,
+	virtual bool receive_buf_to_queue(Binary_buf& binary_buf);
+	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+	// 纯虚函数,必须由子类重载,
+	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud);
+
 
 
 protected:

+ 226 - 0
laser/LivoxLaser.puml

@@ -0,0 +1,226 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  CLivoxLaser
+
+
+
+
+class CLivoxLaser
+{
+//大疆livox雷达,从Laser_base继承。
+==protected:==
+	//雷达设备状态,livox管理底层sdk,后台线程的工作状态
+	typedef enum
+	{
+		kDeviceStateDisconnect = 0,		//雷达设备状态	断开连接
+		kDeviceStateConnect = 1,		//雷达设备状态	连接正常
+		kDeviceStateSampling = 2,		//雷达设备状态	正在扫描
+	} DeviceState;
+..
+	//雷达设备信息,
+	typedef struct
+	{
+		uint8_t handle;					//雷达控制句柄
+		DeviceState device_state;		//雷达设备状态
+		DeviceInfo info;				//雷达基本信息
+	} DeviceItem;
+==public:==
+	CLivoxLaser() = delete;
+	CLivoxLaser(const CLivoxLaser& other) = delete;
+	//唯一的构造函数,按照设备名称和雷达参数来创建实例。
+	CLivoxLaser(int id, Laser_proto::laser_parameter laser_param);
+	~CLivoxLaser();
+..
+	//雷达链接设备,为3个线程添加线程执行函数。
+	virtual Error_manager connect_laser();
+	//雷达断开链接,释放3个线程
+	virtual Error_manager disconnect_laser();
+	//对外的接口函数,负责接受并处理任务单,
+	virtual Error_manager execute_task(Task_Base* p_laser_task);
+	//检查雷达状态,是否正常运行
+	virtual Error_manager check_laser();
+	//雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
+	virtual Error_manager start_scan();
+	//雷达的停止接口函数, 让雷达停止扫描,一般需要子类重载,不同的雷达结束方式不同。
+	virtual Error_manager stop_scan();
+	//结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
+	virtual Error_manager end_task();
+..
+	//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+	virtual bool is_ready();
+==protected:==
+	//接受二进制消息的功能函数,每次只接受一个CBinaryData
+	virtual bool receive_buf_to_queue(Binary_buf& binary_buf);
+	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud);
+==protected:==
+	static void InitLivox();
+	virtual bool IsScanComplete();
+	virtual void UpdataHandle();
+	static void LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser);
+	static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
+	static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
+	static void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data);
+==protected:==
+	uint8_t									m_handle;
+	unsigned int							m_frame_maxnum;
+	Thread_safe_queue<Binary_buf*>			m_queue_livox_data;
+	static DeviceItem						g_devices[kMaxLidarCount];
+	static std::map<uint8_t,std::string>	g_handle_sn;
+	static std::map<std::string, uint8_t>	g_sn_handle;
+	static std::map<std::string, CLivoxLaser*> g_sn_laser;
+	static CLivoxLaser*						g_all_laser[kMaxLidarCount];
+	static unsigned int						g_count[kMaxLidarCount];
+}
+
+
+class Laser_base
+{
+//雷达的基类,不能直接使用,必须子类继承
+==public:==
+	Laser_base() = delete;
+	Laser_base(const Laser_base& other) = delete;
+..
+	//唯一的构造函数,按照设备名称和雷达参数来创建实例。
+	//input:id: 雷达设备的id,(唯一索引)
+	//input:laser_param:雷达的参数,
+	//注:利用protobuf创建 laser_parameter 类,然后从文件读取参数
+	Laser_base(int laser_id,Laser_proto::laser_parameter laser_param);
+	//析构函数
+	~Laser_base();
+..
+	//雷达链接设备,为3个线程添加线程执行函数。
+	virtual Error_manager connect_laser();
+	//雷达断开链接,释放3个线程
+	virtual Error_manager disconnect_laser();
+	//对外的接口函数,负责接受并处理任务单,
+	//input:p_laser_task 雷达任务单,基类的指针,指向子类的实例,(多态)
+	//注:这个函数为虚函数,实际的处理任务的代码由子类重载并实现。
+	virtual Error_manager execute_task(Task_Base* p_laser_task);
+	//检查雷达状态,是否正常运行
+	virtual Error_manager check_laser();
+	//雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
+	virtual Error_manager start_scan();
+	//雷达的停止接口函数, 让雷达停止扫描,一般需要子类重载,不同的雷达结束方式不同。
+	virtual Error_manager stop_scan();
+	//结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
+	virtual Error_manager end_task();
+==public:==
+	//设置保存文件的路径,并打开文件,
+	Error_manager set_open_save_path(std::string save_path,bool is_save=true);
+	//关闭保存文件,推出前一定要执行
+	Error_manager close_save_path();
+	//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+	virtual bool is_ready();
+	//获取雷达id
+	int get_laser_id();
+==protected:==
+	//接受二进制消息的功能函数,每次只接受一个CBinaryData
+	// 纯虚函数,必须由子类重载,
+	virtual bool receive_buf_to_queue(Binary_buf& binary_buf) = 0;
+	//线程执行函数,将二进制消息存入队列缓存,
+	void thread_receive();
+..
+	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+	// 纯虚函数,必须由子类重载,
+	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)=0;
+	//线程执行函数,转化并处理三维点云。
+	void thread_transform();
+..
+	//公开发布雷达信息的功能函数,
+	Error_manager publish_laser_to_message();
+	//线程执行函数,公开发布雷达的相关信息,用作上位机的监视。
+	static void thread_publish(Laser_base* p_laser);
+..
+	//获取雷达状态
+	Laser_statu get_laser_statu();
+==protected:==
+	//初始化变换矩阵,设置默认值
+	Error_manager init_laser_matrix();
+	//设置变换矩阵,用作三维点的坐标变换,
+	Error_manager set_laser_matrix(double* p_matrix, int size);
+..
+	//三维点的坐标变换的功能函数,从雷达自己的坐标系,转化到公共坐标系,(目前以plc为公共坐标系)
+	virtual CPoint3D transform_by_matrix(CPoint3D point);
+==protected:==
+	//为了保证多线程的数据安全,修改共享数据必须加锁。  atomic 和 安全队列 可以不加锁进行读写
+	//建议:判断标志位使用 atomic, 容器要封装并使用 安全容器,
+	std::mutex          				m_laser_lock;            	//雷达数据锁
+..
+	std::atomic<int>					m_laser_id;                 //雷达设备id
+	Laser_proto::laser_parameter  		m_laser_param;				//雷达的配置参数
+	//雷达变换矩阵,三维点的坐标变换的矩阵,从雷达自己的坐标系,转化到公共坐标系,(目前以plc为公共坐标系)
+	//必须在set_laser_matrix之后,才能使用。(connect时,从雷达的配置参数导入)
+	double								mp_laser_matrix[LASER_MATRIX_ARRAY_SIZE];	//雷达变换矩阵
+	//雷达扫描事件的标志位,受start和stop控制,然后其他线程判断m_scan_flag标准位,来进行启停。
+	std::atomic<bool>                	m_laser_scan_flag;          //雷达扫描的使能标志位
+	//雷达状态和任务状态同步,m_scan_flag停止之后,还要等任务执行完成,才会切回 ready。
+	std::atomic<Laser_statu>			m_laser_statu;             	//雷达工作状态,基类三线程的状态
+	//注:m_laser_statu是基类的三线程的状态,和livox sdk后台线程状态没有任何关系。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+..
+	std::atomic<int>            		m_points_count;         	//雷达采集点的计数
+	Thread_safe_queue<Binary_buf*>		m_queue_laser_data;         //二进制缓存的队列容器
+	Binary_buf							m_last_data;			    //上一个二进制缓存,用作数据拼接
+..
+	std::atomic<bool> 					m_save_flag;           		//雷达保存文件的使能标志位
+	std::string							m_save_path;            	//雷达保存文件的保存路径
+	CLogFile							m_binary_log_tool;          //二进制缓存的日志工具
+	CLogFile							m_points_log_tool;          //三维点云的日志工具
+..
+	//线程指针的内存管理,由Connect和Disconnect进行分配和释放。
+	std::thread*						mp_thread_receive;    		//接受缓存的线程指针
+	Thread_condition					m_condition_receive;		//接受缓存的条件变量
+	std::thread*						mp_thread_transform;   		//转化数据的线程指针
+	Thread_condition					m_condition_transform;		//转化数据的条件变量
+	std::thread*        				mp_thread_publish;   		//发布信息的线程指针
+	Thread_condition					m_condition_publish;		//发布信息的条件变量
+..
+	//任务单的指针,实际内存由应用层管理,
+	//接受任务后,指向新的任务单
+	Laser_task *  						mp_laser_task;        		//任务单的指针
+}
+
+class CLivoxMid100Laser
+{
+==public:==
+	CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param);
+	~CLivoxMid100Laser();
+..
+	//雷达链接设备,为3个线程添加线程执行函数。
+	virtual Error_manager connect_laser();
+	//雷达断开链接,释放3个线程
+	virtual Error_manager disconnect_laser();
+	//对外的接口函数,负责接受并处理任务单,
+	//input:p_laser_task 雷达任务单,基类的指针,指向子类的实例,(多态)
+	//注:这个函数为虚函数,实际的处理任务的代码由子类重载并实现。
+	virtual Error_manager execute_task(Task_Base* p_laser_task);
+	//检查雷达状态,是否正常运行
+	virtual Error_manager check_laser();
+	//雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
+	virtual Error_manager start_scan();
+	//雷达的停止接口函数, 让雷达停止扫描,一般需要子类重载,不同的雷达结束方式不同。
+	virtual Error_manager stop_scan();
+	//结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
+	virtual Error_manager end_task();
+..
+	//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+	virtual bool is_ready();
+==protected:==
+	virtual bool IsScanComplete();
+	virtual void UpdataHandle();
+==protected:==
+	uint8_t		m_handle1;
+	uint8_t		m_handle2;
+	uint8_t		m_handle3;
+}
+
+Laser_base -> CLivoxLaser : inherit
+CLivoxLaser -> CLivoxMid100Laser : inherit
+
+
+@enduml

+ 94 - 55
laser/LivoxMid100Laser.cpp

@@ -7,7 +7,9 @@ RegisterLaser(LivoxMid100)
 CLivoxMid100Laser::CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param)
 :CLivoxLaser(id, laser_param)
 {
+	//设备livox扫描最大帧数
     m_frame_maxnum = laser_param.frame_num();
+	//判断参数类型,
     if (laser_param.type() == "LivoxMid100")
     {
         std::string sn = laser_param.sn();
@@ -18,6 +20,8 @@ CLivoxMid100Laser::CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_
         g_sn_laser.insert(std::make_pair(sn1, this));
         g_sn_laser.insert(std::make_pair(sn2, this));
         g_sn_laser.insert(std::make_pair(sn3, this));
+		//初始化livox
+		InitLivox();
     }
 }
 CLivoxMid100Laser::~CLivoxMid100Laser()
@@ -28,6 +32,7 @@ CLivoxMid100Laser::~CLivoxMid100Laser()
 //雷达链接设备,为3个线程添加线程执行函数。
 Error_manager CLivoxMid100Laser::connect_laser()
 {
+	Error_manager t_error;
 	//设置点云变换矩阵
 	double matrix[12]={0};
 	matrix[0]=m_laser_param.mat_r00();
@@ -44,7 +49,11 @@ Error_manager CLivoxMid100Laser::connect_laser()
 	matrix[9]=m_laser_param.mat_r21();
 	matrix[10]=m_laser_param.mat_r22();
 	matrix[11]=m_laser_param.mat_r23();
-	set_laser_matrix(matrix, 12);
+	t_error = set_laser_matrix(matrix, 12);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
 	return CLivoxLaser::connect_laser();
 }
 //雷达断开链接,释放3个线程
@@ -60,59 +69,90 @@ Error_manager CLivoxMid100Laser::disconnect_laser()
 //注:这个函数为虚函数,实际的处理任务的代码由子类重载并实现。
 Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
 {
-    Error_manager t_error_manager;
-//检查指针
-    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");
-    }
-    if (p_laser_task->get_task_type() != LASER_TASK) {
-        return Error_manager(Error_code::LASER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
-                             "laser task type error");
-    }
-
-
-//接受任务,并将任务的状态改为TASK_SIGNED已签收
-    mp_laser_task = (Laser_task *) p_laser_task;
-    mp_laser_task->set_task_statu(TASK_SIGNED);
+	LOG(INFO) << " CLivoxMid100Laser::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_base::porform_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型,
+	if (p_laser_task->get_task_type() != LASER_TASK)
+	{
+		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 "laser task type error != LASER_TASK");
+	}
 
+	//接受任务,并将任务的状态改为TASK_SIGNED已签收
+	mp_laser_task = (Laser_task *) p_laser_task;
+	mp_laser_task->set_task_statu(TASK_SIGNED);
 
-//检查消息内容是否正确,
-//检查点云指针
-    if (mp_laser_task->get_task_point_cloud().get() == NULL)
-    {
-        t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
-                                            Error_level::MINOR_ERROR,
-                                            "execute_task mp_task_point_cloud is null");
-    }
-    else
-    {
-        m_frame_maxnum=mp_laser_task->get_task_frame_maxnum();
-//将任务的状态改为 TASK_WORKING 处理中
-        mp_laser_task->set_task_statu(TASK_WORKING);
-        std::string save_path = mp_laser_task->get_task_save_path();
-		set_open_save_path(save_path);
-//启动雷达扫描
-        Start();
-    }
-//返回错误码
-    if (t_error_manager != Error_code::SUCCESS) {
-        mp_laser_task->set_task_error_manager(t_error_manager);
-    }
-    return t_error_manager;
+	//检查消息内容是否正确,
+	//检查三维点云指针
+	if (mp_laser_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_OVER 结束任务
+		mp_laser_task->set_task_statu(TASK_OVER);
+		//返回错误码
+		mp_laser_task->set_task_error_manager(t_result);
+		return t_result;
+	}
+	else
+	{
+		m_frame_maxnum=mp_laser_task->get_task_frame_maxnum();
+
+
+		//设置保存文件的路径
+		std::string save_path = mp_laser_task->get_task_save_path();
+		t_error = set_open_save_path(save_path);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			//文件保存文件的路径的设置 允许失败。继续后面的动作
+			t_result.compare_and_cover_error(t_error);
+		}
+		//启动雷达扫描
+		t_error = start_scan();
+		if ( t_error != Error_code::SUCCESS )
+		{
+			t_result.compare_and_cover_error(t_error);
+
+			//将任务的状态改为 TASK_OVER 结束任务
+			mp_laser_task->set_task_statu(TASK_OVER);
+			//返回错误码
+			mp_laser_task->set_task_error_manager(t_result);
+			return t_result;
+		}
+		else
+		{
+			//将任务的状态改为 TASK_WORKING 处理中
+			mp_laser_task->set_task_statu(TASK_WORKING);
+		}
+	}
+	//返回错误码
+	if (t_result != Error_code::SUCCESS)
+	{
+		mp_laser_task->set_task_error_manager(t_result);
+	}
+	return t_result;
 }
 
 //检查雷达状态,是否正常运行
 Error_manager CLivoxMid100Laser::check_laser()
 {
-	return Error_code::SUCCESS;
+	return CLivoxLaser::check_laser();
 }
 
 //雷达的启动接口函数, 让雷达进行扫描,一般需要子类重载,不同的雷达开始方式不同。
 Error_manager CLivoxMid100Laser::start_scan()
 {
 	LOG(INFO) << " livoxMid100 start :" << m_laser_param.sn();
-	m_queue_livox_data.clear();
+	//清空livox子类的队列,
+	m_queue_livox_data.clear_and_delete();
 	g_count[m_handle1] = 0;
 	g_count[m_handle2] = 0;
 	g_count[m_handle3] = 0;
@@ -127,32 +167,31 @@ Error_manager CLivoxMid100Laser::stop_scan()
 //结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
 Error_manager CLivoxMid100Laser::end_task()
 {
-	return Error_code::SUCCESS;
+	return CLivoxLaser::end_task();
 }
-
-Laser_statu CLivoxMid100Laser::get_laser_statu()
+//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+bool CLivoxMid100Laser::is_ready()
 {
+	//3个livox雷达设备的状态,livox sdk后台线程的状态
 	bool cond1=g_devices[m_handle1].device_state == kDeviceStateConnect ||
 			   g_devices[m_handle1].device_state == kDeviceStateSampling;
 	bool cond2=g_devices[m_handle2].device_state == kDeviceStateConnect ||
 			   g_devices[m_handle2].device_state == kDeviceStateSampling;
 	bool cond3=g_devices[m_handle3].device_state == kDeviceStateConnect ||
 			   g_devices[m_handle3].device_state == kDeviceStateSampling;
-	if (cond1 && cond2 && cond3) {
-		if(m_queue_laser_data.size()==0 && m_laser_scan_flag==false)
-			m_laser_statu = LASER_READY;
-		else
-			m_laser_statu = LASER_BUSY;
+
+	if ( cond1 && cond2 && cond3 && m_laser_statu == LASER_READY )
+	{
+		true;
 	}
-	else {
-		m_laser_statu = LASER_DISCONNECT;
+	else
+	{
+		false;
 	}
-
-
-	return m_laser_statu;
-
 }
 
+
 void CLivoxMid100Laser::UpdataHandle()
 {
 	std::string sn = m_laser_param.sn();

+ 4 - 2
laser/LivoxMid100Laser.h

@@ -27,9 +27,11 @@ public:
 	//结束任务单,stop之后,要检查线程状态和数据结果,然后才能 end_task
 	virtual Error_manager end_task();
 
+	//判断雷达状态是否为待机,如果已经准备好,则可以执行任务。
+	//子类重载 is_ready(),里面增加livox sdk后台线程状态的判断。
+	virtual bool is_ready();
+
 
-protected:
-	virtual Laser_statu get_laser_statu();
 protected:
 	virtual bool IsScanComplete();
 	virtual void UpdataHandle();

+ 2 - 1
laser/Point3D.h

@@ -4,6 +4,7 @@ class CPoint3D
 {
 public:
 	double x, y, z;
+
 	CPoint3D();
 	~CPoint3D();
     CPoint3D(double c1, double c2, double c3);
@@ -12,7 +13,7 @@ public:
 	{
 		*this = pt;
 	}
-    void Translate(double cx, double cy, double cz); //ת»¯
+    void Translate(double cx, double cy, double cz); //ת��
 };
 
 #endif

+ 3 - 3
laser/Sick511FileLaser.cpp

@@ -82,7 +82,7 @@ Error_manager CSick511FileLaser::end_task()
 
 
 
-Buf_type CSick511FileLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
+Buf_type CSick511FileLaser::transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points)
 {
 	////ƴ���ϴ�δ�������
 
@@ -185,7 +185,7 @@ Buf_type CSick511FileLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3
 	return type;
 }
 
-bool CSick511FileLaser::RecvData(Binary_buf& data)
+bool CSick511FileLaser::receive_buf_to_queue(Binary_buf& data)
 {
 	if (m_start_read == false)
 		return false;
@@ -216,7 +216,7 @@ int CSick511FileLaser::FindHead(char* buf, int b_len)
 {
 	int i = 0;
 	if (b_len < 2)
-		return NULL;
+		return 0;
 	for (i = 0; i <= b_len; i++)
 	{
 		if (b_len > 10)

+ 7 - 2
laser/Sick511FileLaser.h

@@ -30,8 +30,13 @@ public:
 	virtual Error_manager end_task();
 
 protected:
-	virtual bool RecvData(Binary_buf& data);
-	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
+	//接受二进制消息的功能函数,每次只接受一个CBinaryData
+	// 纯虚函数,必须由子类重载,
+	virtual bool receive_buf_to_queue(Binary_buf& data);
+	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+	// 纯虚函数,必须由子类重载,
+	virtual Buf_type transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points);
+
 
 protected:
 	int FindHead(char* buf, int b_len);

+ 5 - 5
laser/TcpLaser.cpp

@@ -74,7 +74,7 @@ Error_manager CTcpLaser::start_scan()
 		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
 							"  is_ready ERROR ");
 
-	char* sendMsg = "$SLSSTA*0A\r\n";
+	const char* sendMsg = "$SLSSTA*0A\r\n";
 	if (Send(sendMsg, strlen(sendMsg)))
 	{
 		return Laser_base::start_scan();
@@ -99,7 +99,7 @@ Error_manager CTcpLaser::end_task()
 }
 
 
-Buf_type CTcpLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
+Buf_type CTcpLaser::transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points)
 {
 	////ƴ���ϴ�δ�������
 
@@ -198,7 +198,7 @@ Buf_type CTcpLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& poin
 	return type;
 }
 
-bool CTcpLaser::RecvData(Binary_buf& data)
+bool CTcpLaser::receive_buf_to_queue(Binary_buf& data)
 {
 	char buf[4096] = { 0 };
 	int bytesRead = Recv(buf, 4096);
@@ -216,7 +216,7 @@ int CTcpLaser::FindHead(char* buf, int b_len)
 {
 	int i = 0;
 	if (b_len < 2)
-		return NULL;
+		return 0;
 	for (i = 0; i <= b_len; i++)
 	{
 		if (b_len > 10)
@@ -249,7 +249,7 @@ int CTcpLaser::FindTail(char* buf, int b_len)
 	return -9999999;
 }
 
-bool CTcpLaser::Send(char* sendbuf, int len)
+bool CTcpLaser::Send(const char* sendbuf, int len)
 {
 	int ret = 0;
 	do

+ 7 - 3
laser/TcpLaser.h

@@ -37,12 +37,16 @@ public:
 	virtual Error_manager end_task();
 
 protected:
-	virtual bool RecvData(Binary_buf& data);
-	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
+	//接受二进制消息的功能函数,每次只接受一个CBinaryData
+	// 纯虚函数,必须由子类重载,
+	virtual bool receive_buf_to_queue(Binary_buf& data);
+	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+	// 纯虚函数,必须由子类重载,
+	virtual Buf_type transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points);
 
 
 private:
-	bool	Send(char* sendbuf, int len);                          // ����ָ��
+	bool	Send(const char* sendbuf, int len);                          // ����ָ��
 	int		Recv(char* recvbuf, int len);                           // ��������
 	static int FindHead(char* buf, int b_len);
 	static int FindTail(char* buf, int b_len);

+ 6 - 6
laser/UdpLaser.cpp

@@ -81,7 +81,7 @@ Error_manager CUdpLaser::start_scan()
 		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
 							"is_ready error ");
 
-	char* sendMsg = "$SLSSTA*0A\r\n";
+	const char* sendMsg = (char*)"$SLSSTA*0A\r\n";
 	if (Send(sendMsg, strlen(sendMsg)))
 	{
 		m_laser_statu = LASER_BUSY;
@@ -94,7 +94,7 @@ Error_manager CUdpLaser::start_scan()
 //雷达的停止接口函数, 让雷达停止扫描,一般需要子类重载,不同的雷达结束方式不同。
 Error_manager CUdpLaser::stop_scan()
 {
-	char sendMsg[] = "$SLSSTP*1B\r\n";
+	const char sendMsg[] = "$SLSSTP*1B\r\n";
 	std::lock_guard<std::mutex> lk(m_mutex);
 	if (Send(sendMsg, strlen(sendMsg)))
 		return Error_code::SUCCESS;
@@ -108,7 +108,7 @@ Error_manager CUdpLaser::end_task()
 }
 
 
-Buf_type CUdpLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
+Buf_type CUdpLaser::transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points)
 {
 	////ƴ���ϴ�δ�������
 
@@ -207,7 +207,7 @@ Buf_type CUdpLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& poin
 	return type;
 }
 
-bool CUdpLaser::RecvData(Binary_buf& data)
+bool CUdpLaser::receive_buf_to_queue(Binary_buf& data)
 {
 	char buf[4096 * 10] = { 0 };
     int bytesRead = Recv(buf, 4096);
@@ -224,7 +224,7 @@ int CUdpLaser::FindHead(char* buf, int b_len)
 {
 	int i = 0;
 	if (b_len < 2)
-		return NULL;
+		return 0;
 	for (i = 0; i <= b_len; i++)
 	{
 		if (b_len > 10)
@@ -257,7 +257,7 @@ int CUdpLaser::FindTail(char* buf, int b_len)
 	return -9999999;
 }
 
-bool CUdpLaser::Send(char* sendbuf, int len)
+bool CUdpLaser::Send(const char* sendbuf, int len)
 {
 	int ret = 0;
 	do 

+ 7 - 3
laser/UdpLaser.h

@@ -32,12 +32,16 @@ public:
 	virtual Error_manager end_task();
 
 protected:
-	virtual bool RecvData(Binary_buf& data);
-	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
+	//接受二进制消息的功能函数,每次只接受一个CBinaryData
+	// 纯虚函数,必须由子类重载,
+	virtual bool receive_buf_to_queue(Binary_buf& data);
+	//将二进制消息转化为三维点云的功能函数,每次只转化一个CBinaryData,
+	// 纯虚函数,必须由子类重载,
+	virtual Buf_type transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points);
 	
 
 private:
-	bool Send(char* sendbuf, int len);                          // ����ָ��
+	bool Send(const char* sendbuf, int len);                          // ����ָ��
 	int Recv(char* recvbuf, int len);                           // ��������
 	static int FindHead(char* buf, int b_len);
 	static int FindTail(char* buf, int b_len);

+ 0 - 0
laser/laser_parameter.pb.cc


+ 0 - 0
laser/laser_parameter.pb.h


+ 0 - 0
laser/laser_parameter.proto


+ 1 - 5
laser/laser_task_command.cpp

@@ -135,11 +135,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_task::get_task_point_cloud()
 {
     return mp_task_point_cloud;
 }
-//获取 错误码
-Error_manager Laser_task::get_task_error_manager()
-{
-    return m_task_error_manager;
-}
+
 
 
 

+ 1 - 4
laser/laser_task_command.h

@@ -66,8 +66,7 @@ public:
     std::string get_task_save_path();
     //获取 三维点云容器的智能指针
     pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
-    //获取 错误码
-    Error_manager get_task_error_manager();
+
 
 
 
@@ -98,8 +97,6 @@ protected:
     //这里只是指针,实际内存由应用层管理,初始化时必须保证内存有效。
     pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
 
-    //错误码,任务故障信息,任务输出
-    Error_manager                   m_task_error_manager;
 
 };
 

+ 110 - 0
laser/laser_task_command.puml

@@ -0,0 +1,110 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  binary_buf是二进制缓存
+
+note left of Laser_task
+//laser_task_command,是雷达任务指令的相关功能
+//功能:用作应用层向laser模块传输任务,的指令消息
+
+//用法:应用层直接调用laser的接口函数,并将Laser_task类作为参数传递
+//CLaser类则按照Laser_task的功能码和指定的参数,执行相应的功能
+//并将结果填充到Laser_task,返回给应用层
+end note
+
+
+class Laser_task
+{
+//雷达模块的任务指令,从Task_Base继承,
+//补充了雷达专属的数据输入和输出
+==public:==
+    //构造函数,构造函数锁定任务类型为LASER_TASK,后续不允许更改
+    Laser_task();
+    //析构函数
+    ~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);
+..
+    //push点云,把 point_xyz 添加到 p_task_point_cloud 里面。
+    Error_manager task_push_point(pcl::PointXYZ point_xyz);
+==public:==
+    //获取 点云的采集帧数最大值
+    unsigned int get_task_frame_maxnum();
+    //获取采集的点云保存路径
+    std::string get_task_save_path();
+    //获取 三维点云容器的智能指针
+    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);
+    //设置采集的点云保存路径
+    void set_task_save_path(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:==
+    //点云的采集帧数最大值,任务输入
+    unsigned int                    m_task_frame_maxnum;
+    //点云保存文件的路径,任务输入
+    std::string                     m_task_save_path;
+    //三维点云的数据保护锁,任务输入
+    std::mutex*                     mp_task_cloud_lock;
+    //采集结果,三维点云容器的智能指针,任务输出
+    //这里只是指针,实际内存由应用层管理,初始化时必须保证内存有效。
+    pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+}
+
+
+class Task_Base
+{
+//任务单基类
+==public:==
+    //初始化任务单,初始任务单类型为 UNKONW_TASK
+    virtual Error_manager init();
+..
+    //更新任务单
+    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();
+==protected:==
+    Task_type                   m_task_type;                    //任务类型
+    Task_statu                  m_task_statu;                   //任务状态
+    std::string                 m_task_statu_information;       //任务状态说明
+	Error_manager               m_task_error_manager;//错误码,任务故障信息,任务输出
+}
+
+Laser_task <-- Task_Base : inherit
+
+@enduml

+ 52 - 33
main.cpp

@@ -6,7 +6,7 @@
 #include "./laser/Laser.h"
 #include "./laser/LivoxLaser.h"
 #include "./error_code/error_code.h"
-
+#include "LogFiles.h"
 #include <string.h>
 #include <fcntl.h>
 #include <google/protobuf/io/zero_copy_stream_impl.h>
@@ -35,62 +35,61 @@ bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
 	return success;
 }
 
-bool fun(int a)
-{
-	if ( a==2 )
-	{
-		return true;
-	}
 
-}
 
 int main(int argc,char* argv[])
 {
-int a=2;
-cout << fun(a) << endl;
-	 a=1;
-	cout << fun(a) << endl;
-	return 0;
-
-
 
 
-
-
-	std::cout << "start!" << std::endl;
+	std::cout << "huli 101 main start!" << std::endl;
 
 	Error_manager err;
 
 	Laser_base *m_laser_vector;
 
-	Laser_proto::Laser_parameter_all laser_parameters;
-
-	if (!read_proto_param("./setting/laser.prototxt", laser_parameters))
+//	Laser_proto::Laser_parameter_all laser_parameters;
+/*	if (!read_proto_param("./setting/laser.prototxt", laser_parameters))
 	{
 		err.error_manager_reset(SYSTEM_READ_PARAMETER_ERROR, NORMAL, "read laser parameter failed");
 		cout << err.to_string() << endl;
 		return err.get_error_code();
 	}
+*/
+
+
+	Laser_proto::laser_parameter laser_param;
+	laser_param.set_frame_num(1000);
+	laser_param.set_type("Livox");
+	laser_param.set_sn("0TFDFG700601881");
 
 	int i = 0;
-	m_laser_vector = LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(), i,
-												 laser_parameters.laser_parameters(i));
+	m_laser_vector = LaserRegistory::CreateLaser(laser_param.type(), i,
+												 laser_param);
+	if (!Start()) {
+		Uninit();
+		printf("Livox-SDK init fail!\n");
+	}
+	else
+	{
+		std::cout << "huli 201 Livox-SDK start" << std::endl;
+	}
 
 	if (m_laser_vector != NULL)
 	{
-		if ( m_laser_vector->connect_laser() != Error_code::SUCCESS)
+		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);
-			cout << err.to_string() << endl;
-			return err.get_error_code();
+
 		}
 		else
 		{
-			Uninit();
+		    std::cout << "huli 301 connect_laser Error_code::SUCCESS" << std::endl;
 		}
 
+		cout << "huli 311 " << err.to_string() << endl;
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr scan_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		std::mutex cloud_lock;
@@ -98,19 +97,39 @@ cout << fun(a) << endl;
 		Laser_task *laser_task = new Laser_task();
 		//
 		laser_task->task_init(TASK_CREATED, scan_cloud, &cloud_lock);
-		laser_task->set_task_frame_maxnum(1000);
+		laser_task->set_task_frame_maxnum(100);
 		laser_task->set_task_save_path("123");
 
 		//发送任务单给雷达
-		std::this_thread::sleep_for(std::chrono::seconds(2));
+		std::this_thread::sleep_for(std::chrono::seconds(5));
+
+		std::cout << "huli 401 connect_laser Error_code::SUCCESS" << std::endl;
+
 		err = m_laser_vector->execute_task(laser_task);
-		cout << err.to_string() << endl;
+		cout << "huli: 501 :" << err.to_string() << endl;
+		cout << "huli: 501 :" << err.to_string() << endl;
 	}
+	else
+	{
+		cout << "huli: 601 :" << err.to_string() << endl;
+
+	}
+
+
+	std::this_thread::sleep_for(std::chrono::seconds(5));
+	cout << "huli: 701 :" << err.to_string() << endl;
+
+	m_laser_vector->disconnect_laser();
+	cout << "huli: 801 :" << err.to_string() << endl;
+
+	m_laser_vector->close_save_path();
+	cout << "huli: 901 :" << err.to_string() << endl;
+
+	std::this_thread::sleep_for(std::chrono::seconds(2));
 
+	Uninit();
 
-	int x;
-	cin >> x ;
-	cout << "end" << endl;
+	cout << "huli 1234 main end" << endl;
 }
 
 

BIN
png/Laser.png


BIN
png/LivoxLaser.png


BIN
png/binary_buf.png


BIN
png/laser_task_command.png


BIN
png/laser功能模块图.eddx


BIN
png/laser功能模块图.png


BIN
png/laser数据流程图.eddx


BIN
png/laser数据流程图.png


BIN
png/laser程序流程图.eddx


BIN
png/laser程序流程图.png


BIN
png/task_command_manager.png


BIN
png/thread_condition.png


BIN
png/thread_safe_queue.png


+ 16 - 1
task/task_command_manager.cpp

@@ -42,4 +42,19 @@ Task_statu  Task_Base::get_statu()
 std::string Task_Base::get_statu_information()
 {
     return m_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;
+}
+
+
+
+

+ 15 - 10
task/task_command_manager.h

@@ -10,12 +10,12 @@
 //任务类型
 enum Task_type
 {
-    LASER_TASK              =0,             //雷达扫描任务,
-    LOCATE_TASK             =1,             //测量任务
-    PLC_TASK                =2,             //上传PLC任务
-    UNKNOW_TASK             =3              //未知任务单/初始化,默认值
+	UNKNOW_TASK             =0,				//未知任务单//初始化,默认值
+	LASER_TASK              =1,             //雷达扫描任务,
+    LOCATE_TASK             =2,             //测量任务
+    PLC_TASK                =3,             //上传PLC任务
 };
-//任务状态
+//任务状态,如果任务故障,任务状态改为TASK_OVER,然后在m_task_error_manager 补充错误码。
 enum Task_statu
 {
     TASK_CREATED            =0,             //创建状态,默认值
@@ -24,7 +24,7 @@ enum Task_statu
     TASK_OVER               =3,             //已结束
 };
 
-//
+//任务单基类
 class Task_Base
 {
 public:
@@ -41,13 +41,18 @@ public:
     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;        //任务状态说明
+    Task_type                   m_task_type;					//任务类型
+    Task_statu                  m_task_statu;					//任务状态
+    std::string					m_task_statu_information;		//任务状态说明
+	//错误码,任务故障信息,任务输出
+	Error_manager               m_task_error_manager;
 };
 
 #endif //TASK_COMAND_MANAGER_H

+ 78 - 0
task/task_command_manager.puml

@@ -0,0 +1,78 @@
+@startuml
+@startuml
+skinparam classAttributeIconSize 0
+
+title  binary_buf是二进制缓存
+
+
+
+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

+ 0 - 0
tool/StdCondition.cpp


+ 0 - 0
tool/StdCondition.h


+ 0 - 0
tool/binary_buf.cpp


+ 2 - 1
tool/binary_buf.h

@@ -21,7 +21,8 @@
 //在通信消息的前面一部分字符串,表示这条消息的类型。
 //在解析消息的时候,先解析前面的消息类型,来判断这条消息的功用
 enum Buf_type
-{//默认值 BUF_UNKNOW = 0
+{
+	//默认值 BUF_UNKNOW = 0
 	BUF_UNKNOW   		=0,	//未知消息
 	BUF_READY  			=1,	//待机消息
 	BUF_START 			=2,	//开始消息

+ 85 - 0
tool/binary_buf.puml

@@ -0,0 +1,85 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  binary_buf是二进制缓存
+
+note left of Binary_buf
+/*
+ * binary_buf是二进制缓存
+ * 这里用字符串,来存储雷达的通信消息的原始数据
+ * Binary_buf 的内容格式:消息类型 + 消息数据
+ *
+ * 例如思科的雷达的消息类型
+ * ready->ready->start->data->data->data->stop->ready->ready
+ *
+ * 提供了 is_equal 系列的函数,来进行判断前面的消息类型
+ *
+ * 注意了:m_buf是中间可以允许有‘\0’的,不是单纯的字符串格式
+ * 			末尾也不一定是‘\0’
+ */
+end note
+
+
+
+enum Buf_type
+{
+//雷达消息的类型
+//在通信消息的前面一部分字符串,表示这条消息的类型。
+//在解析消息的时候,先解析前面的消息类型,来判断这条消息的功用
+	//默认值 BUF_UNKNOW = 0
+	BUF_UNKNOW   		=0,	//未知消息
+	BUF_READY  			=1,	//待机消息
+	BUF_START 			=2,	//开始消息
+	BUF_DATA   			=3,	//数据消息
+	BUF_STOP  			=4,	//结束消息
+	BUF_ERROR   		=5,	//错误消息
+}
+
+
+
+class Binary_buf
+{
+//二进制缓存,
+==public:==
+	Binary_buf();
+	Binary_buf(const Binary_buf& other);
+	~Binary_buf();
+..
+	//使用参数构造,深拷贝,len为0时,使用strlen(buf),不存储结束符'\0'
+	Binary_buf(const char* p_buf, int len = 0);
+	//重载=,深拷贝,
+	Binary_buf& operator=(const Binary_buf& other);
+	//重载=,深拷贝,使用strlen(buf),不存储结束符'\0'
+	Binary_buf& operator=(const char* p_buf);
+	//重载+,other追加在this的后面,
+	Binary_buf& operator+(Binary_buf& other);
+	//重载+,追加在this的后面,使用strlen(buf),不存储结束符'\0'
+	Binary_buf& operator+(const char* p_buf);
+	//重载[],允许直接使用数组的形式,直接访问buf的内存。注意,n值必须在0~m_length之间,
+	char& operator[](int n);
+..
+	//判空
+	bool is_empty();
+	//清空
+	void clear();
+..
+	//比较前面部分的buf是否相等,使用 other.m_length 为标准
+	bool is_equal_front(const Binary_buf& other);
+	//比较前面部分的buf是否相等,len为0时,使用strlen(buf)为标准,不比较结束符'\0'
+	bool is_equal_front(const char* p_buf, int len = 0);
+
+	//比较的buf是否全部相等,
+	bool is_equal_all(const Binary_buf& other);
+	//比较的buf是否全部相等,不比较结束符'\0'
+	bool is_equal_all(const char* p_buf);
+==public:==
+	char* get_buf()const;
+	int	get_length()const;
+==protected:==
+	char*		mp_buf;				//二进制缓存指针
+	int			m_length;			//二进制缓存长度
+==private:==
+}
+
+
+@enduml

+ 0 - 0
tool/thread_condition.cpp


+ 0 - 0
tool/thread_condition.h


+ 96 - 0
tool/thread_condition.puml

@@ -0,0 +1,96 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+title  Thread_condition 是多线程的条件控制类,主要是控制线程的启停和退出
+note left of Thread_condition
+/* Thread_condition 是多线程的条件控制类,主要是控制线程的启停和退出
+ * 线程创建后,一般是循环运行,
+ * 为了防止线程暂满整个cpu,那么需要线程在不工作的是否进入等待状态。
+ * Thread_condition 就可以控制线程的运行状态。
+ *
+	std::atomic<bool> m_pass_ever		//线程能否直接通过等待,对后面的线程也生效。
+	std::atomic<bool> m_pass_once		//线程能否直接通过等待,一次(通过一次之后,wait里面自动改为false)
+ * 外部调用notify系列的函数,唤醒等待的线程,让线程执行功能函数。
+ * 如果需要线程循环多次执行功能函数,那么就使用 notify_all(true),后面的线程可以直接通过等待了。
+ * 再使用 notify_all(false) ,即可停止线程,让其继续等待。
+ * 如果只想要线程执行一次,那就使用 notify_all(false, true)
+ * 注:notify_all(false, true)和notify_one(false, true) 一样,只能让其中一个线程执行一次
+ *
+ * m_kill_flag //是否杀死线程,让线程强制退出,
+ * 外部调用 kill_all() 函数,可以直接通知线程自动退出。
+	//杀死所有的线程,强制退出线程函数,只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程,使其通过等待,但是不能运行功能函数,必须直接return
+	// 注:只是修改m_kill为true,需要线程函数实时检测kill的状态,来return线程。
+	//		通过等待之后,也要检查kill的状态,如果为真,那么就不能执行功能函数,应该直接return
+
+ 注:notify唤醒线程之后,wait里面的判断函数会重新判断。
+ */
+end note
+
+
+
+class Thread_condition
+{
+==public:==
+	Thread_condition();
+	Thread_condition(const Thread_condition& other) = delete;
+	~Thread_condition();
+..
+	//无限等待,由 is_pass_wait 决定是否阻塞。
+	//返回m_pass,
+	bool wait();
+..
+	//等待一定的时间(默认时间单位:毫秒ms),由 is_pass_wait 决定是否阻塞。
+	//return:is_pass_wait的结果,	true:线程直接通过等待,false:线程超时了,然后通过等待。
+	//注意了:线程阻塞期间,是不会return的。
+	bool wait_for_millisecond(unsigned int millisecond);
+..
+	//等待一定的时间(时间单位可调),由 is_pass_wait 决定是否阻塞。
+	//return:is_pass_wait的结果,	true:线程直接通过等待,false:线程超时了,然后通过等待。
+	//注意了:线程阻塞期间,是不会return的。
+	template<typename _Rep, typename _Period>
+	bool wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration);
+..
+	//唤醒已经阻塞的线程,唤醒一个线程
+	//pass_ever 或者 pass_once 为真时,才能唤醒线程。都为假时,线程进入等待。
+	void notify_one(bool pass_ever, bool pass_once = false);
+..
+	//唤醒已经阻塞的线程,唤醒全部线程
+	//pass_ever 或者 pass_once 为真时,才能唤醒线程。都为假时,线程进入等待。
+	void notify_all(bool pass_ever, bool pass_once = false);
+	//注:notify_all(false, true)和notify_one(false, true) 一样,只能让其中一个线程执行一次
+..
+	//杀死所有的线程,强制退出线程函数,只是操作受当前Thread_condition影响的所有线程
+	//唤醒所有线程,使其通过等待,但是不能运行功能函数,必须直接return
+	// 注:只是修改m_kill为true,需要线程函数实时检测kill的状态,来return线程。
+	//		通过等待之后,也要检查kill的状态,如果为真,那么就不能执行功能函数,应该直接return
+	void kill_all();
+..
+	//判断是否或者,return !m_kill_flag
+	bool is_alive();
+==public:==
+	bool get_kill_flag();
+	bool get_pass_ever();
+	bool get_pass_once();
+	void set_kill_flag(bool kill);
+	void set_pass_ever(bool pass_ever);
+	void set_pass_once(bool pass_once);
+	void reset(bool kill = false, bool pass_ever = false, bool pass_once = false);
+==protected:==
+	//判断线程是否可以通过等待,wait系列函数的判断标志
+	//注:m_kill或者m_pass为真时,return true
+	static bool is_pass_wait(Thread_condition * other);
+
+	std::atomic<bool> 		m_kill_flag;			//是否杀死线程,让线程强制退出,
+	std::atomic<bool> 		m_pass_ever;			//线程能否直接通过等待,对后面的线程也生效。
+	std::atomic<bool> 		m_pass_once;			//线程能否直接通过等待,一次(通过一次之后,wait里面自动改为false)
+
+	std::mutex 				m_mutex;				//线程的锁
+	std::condition_variable m_condition_variable;	//线程的条件变量
+
+==private:==
+
+}
+
+
+@enduml

+ 0 - 0
tool/thread_safe_queue.cpp


+ 35 - 10
tool/thread_safe_queue.h

@@ -70,8 +70,10 @@ public:
 	//如果成功插入,则返回true,  失败则返回false
 	//注:只能唤醒一个线程,防止多线程误判empty()
 	bool push(T new_value);
-	//清除队列
+	//清除队列,只是将队列的实例抛出。T是实例内存,系统自动回收的。
 	bool clear();
+	//清除队列,抛出之后还要delete指针。T是动态内存,需要手动回收的。
+	bool clear_and_delete();
 
 public:
 	//判空
@@ -82,6 +84,8 @@ public:
 	// 在退出状态下,所有的功能函数不可用,必须直接返回false或者null。
 	// wait_and_pop不会阻塞。让其直接通过,通过后直接return,不允许做其他的。
 	void termination_queue();
+	//唤醒队列,恢复所有的功能函数。wait_and_pop会继续阻塞。
+	void wake_queue();
 	//获取退出状态
 	bool get_termination_flag();
 	//判断是否可以直接通过wait,  m_data_queue不为空或者m_termination终止时都可以通过等待。
@@ -250,23 +254,36 @@ bool Thread_safe_queue<T>::push(T new_value)
 		return true;
 	}
 }
-//清除队列
+//清除队列,只是将队列的实例抛出。T是实例内存,系统自动回收的。
 template<class T>
 bool Thread_safe_queue<T>::clear()
 {
-	if ( m_termination_flag )
-	{
-		return false;
-	}
-	else
-	{
 		std::unique_lock<std::mutex> lk(m_mutex);
-		while (m_data_queue.empty())
+		while (!m_data_queue.empty())
 		{
 			m_data_queue.pop();
 		}
 		return true;
-	}
+
+}
+//清除队列,抛出之后还要delete指针。T是动态内存,需要手动回收的。
+template<class T>
+bool Thread_safe_queue<T>::clear_and_delete()
+{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		while (!m_data_queue.empty())
+		{
+			T res = NULL;
+			res = move(*m_data_queue.front());
+			m_data_queue.pop();
+			if(res != NULL)
+			{
+				delete(res);
+
+			}
+		}
+	return true;
+
 }
 
 //判空
@@ -293,6 +310,14 @@ void Thread_safe_queue<T>::termination_queue()
 	m_termination_flag = true;
 	m_data_cond.notify_all();
 }
+//唤醒队列,恢复所有的功能函数。wait_and_pop会继续阻塞。
+template<class T>
+void Thread_safe_queue<T>::wake_queue()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = false;
+	m_data_cond.notify_all();
+}
 //获取退出状态
 template<class T>
 bool Thread_safe_queue<T>::get_termination_flag()

+ 108 - 0
tool/thread_safe_queue.puml

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