20200702, 代码合并

laser/Laser.cpp

@@ -0,0 +1,560 @@
+
+
+#include "Laser.h"
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <time.h>
+#include <stdint.h>
+#include <stdio.h>
+
+
+Laser_base::Laser_base(int laser_id, Laser_proto::laser_parameter laser_param)
+:m_laser_id(laser_id)
+,m_laser_param(laser_param)
+,m_laser_scan_flag(false)
+,m_laser_statu(LASER_DISCONNECT)
+,m_points_count(0)
+,m_save_flag(false)
+{
+	mp_thread_receive = NULL;
+	mp_thread_transform = NULL;
+	mp_thread_publish = NULL;
+	init_laser_matrix();
+	mp_laser_task = NULL;
+}
+Laser_base::~Laser_base()
+{
+
+	disconnect_laser();
+	close_save_path();
+}
+
+//雷达链接设备，为3个线程添加线程执行函数。
+Error_manager Laser_base::connect_laser()
+{
+	LOG(INFO) << " ---Laser_base::connect_laser() run--- "<< this;
+	//检查雷达状态
+	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
+	{
+		//清空队列
+		m_queue_laser_data.clear_and_delete();
+		//唤醒队列，
+		m_queue_laser_data.wake_queue();
+
+
+		//这里不建议用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);
+		}
+
+		m_laser_statu = LASER_READY;
+
+	}
+	return Error_code::SUCCESS;
+}
+//雷达断开链接，释放3个线程
+Error_manager Laser_base::disconnect_laser()
+{
+	LOG(INFO) << " ---Laser_base::disconnect_laser() run--- "<< this;
+	//终止队列，防止 wait_and_pop 阻塞线程。
+	m_queue_laser_data.termination_queue();
+	//杀死3个线程，强制退出
+	if (mp_thread_receive)
+	{
+		m_condition_receive.kill_all();
+	}
+	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)
+	{
+		mp_thread_publish->join();
+		delete mp_thread_publish;
+		mp_thread_publish = NULL;
+	}
+	//清空队列
+	m_queue_laser_data.clear_and_delete();
+
+	if ( m_laser_statu != LASER_FAULT  )
+	{
+		m_laser_statu = LASER_DISCONNECT;
+	}
+	return Error_code::SUCCESS;
+}
+
+
+//对外的接口函数，负责接受并处理任务单，
+//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
+Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
+{
+	LOG(INFO) << " --Laser_base::execute_task run--- "<< this;
+	//检查指针
+	if(p_laser_task == NULL)
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Laser_base::execute_task failed, POINTER_IS_NULL");
+	}
+	else
+	{
+		return Error_manager(Error_code::CLASS_BASE_FUNCTION_CANNOT_USE, Error_level::NEGLIGIBLE_ERROR,
+							 "Laser_base::execute_task cannot use");
+	}
+}
+//检查雷达状态,是否正常运行
+Error_manager Laser_base::check_status()
+{
+	if ( get_laser_statu() == LASER_READY  )
+	{
+		return Error_code::SUCCESS;
+	}
+	else if ( get_laser_statu() == LASER_BUSY  )
+	{
+		return Error_manager(Error_code::LASER_STATUS_BUSY, Error_level::MINOR_ERROR,
+							 " CLivoxLaser::check_status error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::LASER_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " CLivoxLaser::check_status error ");
+	}
+	return Error_code::SUCCESS;
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager Laser_base::start_scan()
+{
+	LOG(INFO) << " ---Laser_base::start_scan ---"<< this;
+	if ( is_ready() )
+	{
+		//重置数据
+		m_points_count=0;
+		m_queue_laser_data.clear_and_delete();
+		m_last_data.clear();
+
+		//启动雷达扫描
+		m_laser_scan_flag=true;				//启动雷达扫描
+		m_laser_statu = LASER_BUSY;			//雷达状态 繁忙
+		m_condition_receive.notify_all(true);		//唤醒接受线程
+		m_condition_transform.notify_all(true);	//唤醒转化线程
+
+		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()
+{
+	if(m_laser_scan_flag )
+	{
+		LOG(INFO) << " ---Laser_base::stop_scan ---"<< this;
+		//stop_scan 只是将 m_laser_scan_flag 改为 stop。
+		//然后多线程内部判断 m_laser_scan_flag，然后自动停止线程
+		m_laser_scan_flag=false;
+	}
+	return Error_code::SUCCESS;
+}
+
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager Laser_base::end_task()
+{
+	LOG(INFO) << " ---Laser_base::end_task ---"<< this;
+	stop_scan();
+	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;
+			}
+			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);
+		}
+	}
+	return Error_code::SUCCESS;
+}
+
+//取消任务单，由发送方提前取消任务单
+Error_manager Laser_base::cancel_task()
+{
+	end_task();
+	//强制改为 TASK_DEAD，不管它当前在做什么。
+	mp_laser_task->set_task_statu(TASK_DEAD);
+	return Error_code::SUCCESS;
+}
+
+//设置保存文件的路径，并打开文件，
+Error_manager Laser_base::set_open_save_path(std::string save_path,bool is_save)
+{
+	m_save_flag = is_save;
+	m_points_log_tool.close();
+	m_binary_log_tool.close();
+
+	if (is_save == false)
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		uint32_t beginCmpPath = 0;
+		uint32_t endCmpPath = 0;
+		std::string fullPath = save_path;
+
+		if (fullPath[fullPath.size() - 1] != '/')
+		{
+			fullPath += "/";
+		}
+		fullPath += "laser_data/";
+		endCmpPath = fullPath.size();
+
+		//create dirs;
+		for(uint32_t i = beginCmpPath; i < endCmpPath ; i++ )
+		{
+			if('/' == fullPath[i])
+			{
+				std::string curPath = fullPath.substr(0, i);
+				if(access(curPath.c_str(), F_OK) != 0)
+				{
+					if(mkdir(curPath.c_str(),0777) == -1)
+					{
+						printf("mkdir(%s) failed\n", curPath.c_str());
+						return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+											 "set_open_save_path mkdir error ");
+					}
+				}
+			}
+		}
+
+
+		time_t nowTime;
+		nowTime = time(NULL);
+		struct tm* sysTime = localtime(&nowTime);
+		char time_string[256] = { 0 };
+		sprintf(time_string, "%04d%02d%02d_%02d%02d%02d",
+				sysTime->tm_year + 1900, sysTime->tm_mon + 1, sysTime->tm_mday, sysTime->tm_hour, sysTime->tm_min, sysTime->tm_sec);
+
+		m_save_path = save_path;
+		char pts_file[255] = { 0 };
+//		sprintf(pts_file, "%s/pts%d.txt", save_path.c_str(), m_laser_id+1);
+		sprintf(pts_file, "%s/laser_pst_%s__%d.txt", fullPath.c_str(), time_string, m_laser_id+0);
+		m_points_log_tool.open(pts_file);
+
+//		char bin_file[255] = { 0 };
+////		sprintf(bin_file, "%s/laser%d.data", save_path.c_str(), m_laser_id+1);
+//		sprintf(bin_file, "%s/laser_binary_%s__%d.data", fullPath.c_str(),time_string, 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  get_laser_statu() == LASER_READY ;
+}
+//获取雷达状态
+Laser_statu Laser_base::get_laser_statu()
+{
+	return m_laser_statu;
+}
+//获取雷达id
+int Laser_base::get_laser_id()
+{
+	return m_laser_id;
+}
+
+//线程执行函数，将二进制消息存入队列缓存，
+void Laser_base::thread_receive()
+{
+	LOG(INFO) << " ---thread_receive start ---"<< this;
+	Error_manager t_error;
+
+	//接受雷达消息，每次循环只接受一个 Binary_buf
+	while (m_condition_receive.is_alive())
+	{
+		m_condition_receive.wait();
+		if ( m_condition_receive.is_alive() )
+		{
+			std::this_thread::yield();
+			//获取雷达的通信消息缓存, ，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，(循环执行)
+			t_error = receive_buf_to_queue();
+			if (t_error == SUCCESS)
+			{
+				//这里非常特殊,receive_buf_to_queue会为 Binary_buf 分配内存. 是new出来的
+				//此时 Binary_buf 的内存管理权限转交给 m_queue_laser_data .之后由 Laser_base 负责管理.
+				;
+			}
+				//接受线程无法接受到数据的同时，也收到了雷达停止扫描的信息，那么线程停止
+				//如果m_laser_scan_flag停止扫描，但是receive_buf_to_queue仍然能够接受到数据。那么线程继续运行。
+				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。
+			else if(t_error == NO_DATA  )
+			{
+				if ( !m_laser_scan_flag )
+				{
+					//停止线程，m_condition_receive.wait() 函数将会阻塞。正常停止接受线程
+					m_condition_receive.set_pass_ever(false);
+					//注:此时thread_transform线程仍然继续,任务也没有停止.
+				}
+			}
+			else
+			{
+				//因为故障,而提前结束任务.
+				mp_laser_task->compare_and_cover_task_error_manager(t_error);
+				end_task();
+			}
+		}
+	}
+	LOG(INFO) << " thread_receive end :"<<this;
+	return;
+}
+
+
+//线程执行函数，转化并处理三维点云。
+void Laser_base::thread_transform()
+{
+	LOG(INFO) << " thread_transform start "<< this;
+	Error_manager t_error;
+
+	//转化雷达数据，每次循环只转化一个 Binary_buf，然后得到 t_point3D_cloud.size() 个三维点，
+	while (m_condition_transform.is_alive())
+	{
+		m_condition_transform.wait();
+		if ( m_condition_transform.is_alive() )
+		{
+			std::this_thread::yield();
+			t_error = transform_buf_to_points();
+			if ( t_error == SUCCESS )
+			{
+				;
+			}
+				//转化线程 不需要数据的同时，也收到了雷达停止扫描的信息并且接受线程已经停止，那么停止转化线程，
+				//注：必须先停止接受线程，再停止转化线程。转化线程停止之后即可结束任务。
+				//如果m_laser_scan_flag停止扫描，接受线程和转化线程还有未完成的数据。那么线程继续运行。
+				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。防止数据遗留。
+			else if (t_error == NO_DATA )
+			{
+				if(m_laser_scan_flag == false && m_condition_receive.get_pass_ever() == false)
+				{
+					//停止转化线程，m_condition_transform.wait() 函数将会阻塞。
+					m_condition_transform.set_pass_ever(false);
+
+					//mp_thread_transform 停止时，雷达扫描任务彻底完成，此时任务正常结束
+					end_task();
+				}
+			}
+			else
+			{
+				//因为故障,而提前结束任务.
+				mp_laser_task->compare_and_cover_task_error_manager(t_error);
+				end_task();
+			}
+
+		}
+	}
+	LOG(INFO) << " thread_transform end "<< this;
+	return;
+}
+
+
+//公开发布雷达信息的功能函数，
+Error_manager Laser_base::publish_laser_to_message()
+{
+	return Error_code::SUCCESS;
+	/*
+	globalmsg::msg msg;
+	msg.set_msg_type(globalmsg::eLaser);
+
+	globalmsg::laserStatus status;
+	if(GetStatu()==eLaser_ready) status=globalmsg::eLaserConnected;
+	else if(GetStatu()==eLaser_disconnect) status=globalmsg::eLaserDisconnected;
+	else if(GetStatu()==eLaser_busy) status=globalmsg::eLaserBusy;
+	else  status=globalmsg::eLaserUnknown;
+
+	msg.mutable_laser_msg()->set_id(m_laser_id);
+	msg.mutable_laser_msg()->set_laser_status(status);
+	msg.mutable_laser_msg()->set_queue_data_count(m_queue_laser_data.size());
+	msg.mutable_laser_msg()->set_cloud_count(m_points_count);
+	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;
+}
+
+
+
+//初始化变换矩阵，设置默认值
+Error_manager Laser_base::init_laser_matrix()
+{
+	if ( LASER_MATRIX_ARRAY_SIZE == 12 )
+	{
+		//详见转化的算法transfor。保证转化前后坐标一致。
+		mp_laser_matrix[0] = 1;
+		mp_laser_matrix[1] = 0;
+		mp_laser_matrix[2] = 0;
+		mp_laser_matrix[3] = 0;
+		mp_laser_matrix[4] = 0;
+		mp_laser_matrix[5] = 1;
+		mp_laser_matrix[6] = 0;
+		mp_laser_matrix[7] = 0;
+		mp_laser_matrix[8] = 0;
+		mp_laser_matrix[9] = 0;
+		mp_laser_matrix[10] = 1;
+		mp_laser_matrix[11] = 0;
+	}
+	else
+	{
+		for (int i = 0; i < LASER_MATRIX_ARRAY_SIZE; ++i)
+		{
+			//设为0之后，变换之后，新的点坐标全部为0
+			mp_laser_matrix[i] = 0;
+		}
+	}
+	return Error_code::SUCCESS;
+}
+//设置变换矩阵，用作三维点的坐标变换，
+Error_manager Laser_base::set_laser_matrix(double* p_matrix, int size)
+{
+	if ( p_matrix == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " set_laser_matrix p_matrix IS_NULL ");
+	}
+	else if ( size != LASER_MATRIX_ARRAY_SIZE )
+	{
+		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+							 " set_laser_matrix  size is not Match");
+	}
+	else
+	{
+		memcpy(mp_laser_matrix, p_matrix, LASER_MATRIX_ARRAY_SIZE * sizeof(double));
+		return Error_code::SUCCESS;
+	}
+	return Error_code::SUCCESS;
+}
+//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
+CPoint3D Laser_base::transform_by_matrix(CPoint3D point)
+{
+	CPoint3D result;
+	double x = point.x;
+	double y = point.y;
+	double z = point.z;
+	result.x = x * mp_laser_matrix[0] + y*mp_laser_matrix[1] + z*mp_laser_matrix[2] + mp_laser_matrix[3];
+	result.y = x * mp_laser_matrix[4] + y*mp_laser_matrix[5] + z*mp_laser_matrix[6] + mp_laser_matrix[7];
+	result.z = x * mp_laser_matrix[8] + y*mp_laser_matrix[9] + z*mp_laser_matrix[10] + mp_laser_matrix[11];
+	return result;
+}
+
+
+
+

laser/Laser.h

@@ -0,0 +1,200 @@
+
+/*
+ * laser雷达核心模块，主要是负责管理所有的雷达设备，按需采集三维点。
+ * 由上层下发任务单，雷达模块按照任务指示，进行相应的雷达扫描。
+ * 转换和拼接为标准参考系的三维坐标点云。
+ * 然后通过任务单返回上层。
+ */
+
+#ifndef __LASER__HH__
+#define __LASER__HH__
+#include "Point2D.h"
+#include "Point3D.h"
+#include "LogFiles.h"
+
+#include <glog/logging.h>
+
+#include "laser_parameter.pb.h"
+#include "laser_task_command.h"
+
+#include "../error_code/error_code.h"
+#include "../tool/binary_buf.h"
+#include "../tool/thread_safe_queue.h"
+#include "../tool/thread_condition.h"
+
+
+
+
+//雷达工作状态，基类三线程的状态
+//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_READY			=1,			//雷达正常待机，空闲
+	LASER_BUSY			=2,	        //雷达正在工作，正忙
+	LASER_FAULT			=3,         //雷达错误
+};
+
+//雷达变换矩阵的数组长度，默认为12个参数， size = 3 * ( 3 + 1 ) ，旋转加平移，没有缩放
+#define LASER_MATRIX_ARRAY_SIZE 12
+
+//雷达的基类，不能直接使用，必须子类继承
+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_status();
+	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+	virtual Error_manager start_scan();
+	//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+	virtual Error_manager stop_scan();
+	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+	virtual Error_manager end_task();
+	//取消任务单，由发送方提前取消任务单
+	virtual Error_manager cancel_task();
+
+public:
+	//设置保存文件的路径，并打开文件，
+	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();
+	//设置变换矩阵，用作三维点的坐标变换，
+	Error_manager set_laser_matrix(double* p_matrix, int size);
+
+protected:
+	//接受二进制消息的功能函数，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，
+	// 纯虚函数，必须由子类重载，
+	virtual Error_manager receive_buf_to_queue() = 0;
+	//线程执行函数，将二进制消息存入队列缓存，
+	void thread_receive();
+
+	//将二进制消息转化为三维点云的功能函数，并将其存入 任务单 , 每次只转化一个CBinaryData，
+	// 纯虚函数，必须由子类重载，
+	virtual Error_manager transform_buf_to_points()=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();
+
+
+	//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以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;        		//任务单的指针
+
+};
+
+
+//雷达的注册类，
+//雷达模块可能有多个不同的雷达，这些雷达需要制定不同的子类，从laser基类继承。
+//使用类的多态，从基类操作不同的子类。
+//雷达的注册器为单例模式，没有任何成员变量。
+//里面定义了 static std::map<std::string, CreateLaserFunc>* g_map  注册表，作为永久的内存。
+//然后提供了注册函数和访问函数，来操作这段静态变量（唯一，永久）
+class LaserRegistory
+{
+	typedef Laser_base*  (*CreateLaserFunc)(int id, Laser_proto::laser_parameter laser_param);
+public:
+	LaserRegistory(std::string name, CreateLaserFunc pFun) {
+		AddCreator(name, pFun);
+	}
+	static Laser_base* CreateLaser(std::string name, int id,Laser_proto::laser_parameter laser_param) {
+		if (GetFuncMap().count(name) == 0)
+			return 0;
+		return GetFuncMap()[name](id,laser_param);
+	}
+private:
+	static std::map<std::string, CreateLaserFunc>& GetFuncMap() {
+		static std::map<std::string, CreateLaserFunc>* g_map = new std::map<std::string, CreateLaserFunc>;
+
+		return *g_map;
+	}
+	void AddCreator(std::string name, CreateLaserFunc pFun) {
+		GetFuncMap()[name] = pFun;
+	}
+};
+
+
+#define RegisterLaser(NAME) \
+	static Laser_base* Create_##NAME##_Laser(int id, Laser_proto::laser_parameter param)	\
+	{																\
+		return new C##NAME##Laser(id,param);							\
+	}																\
+	LaserRegistory g_##NAME##_Laser(#NAME,Create_##NAME##_Laser);
+
+
+#endif

laser/Laser.puml

@@ -0,0 +1,298 @@
+@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

laser/LivoxHubLaser.cpp

@@ -0,0 +1,10 @@
+#include "LivoxHubLaser.h"
+
+CLivoxHubLaser::CLivoxHubLaser()
+{
+
+}
+CLivoxHubLaser::~CLivoxHubLaser()
+{
+
+}

laser/LivoxHubLaser.h

@@ -0,0 +1,19 @@
+#ifndef LIVOXHUBLASER_H
+#define LIVOXHUBLASER_H
+
+
+class CLivoxHubLaser
+{
+public:
+    CLivoxHubLaser();
+    ~CLivoxHubLaser();
+protected:
+    /*static void InitLivoxHub();
+    static void LidarDataCallbackHub(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser);
+    static void OnDeviceChangeHub(const DeviceInfo *info, DeviceEvent type);
+    static void OnDeviceBroadcastHub(const BroadcastDeviceInfo *info);
+    static void OnSampleCallbackHub(uint8_t status, uint8_t handle, uint8_t response, void *data);*/
+
+};
+
+#endif // LIVOXHUBLASER_H

laser/LivoxLaser.cpp

@@ -0,0 +1,382 @@
+
+#include "LivoxLaser.h"
+#include "livox_driver.h"
+
+
+RegisterLaser(Livox)
+
+
+
+CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
+:Laser_base(id, laser_param)
+{
+	Error_manager t_error;
+	m_frame_count = 0;
+	m_frame_maxnum = laser_param.frame_num();//初始化从外部参数导入,实际运行前从任务单导入.
+
+	//默认handle为-1,切记不能初始化为0.因为0表示0号雷达,是有效的.....
+	m_livox_device.handle = -1;
+
+	//判断参数类型，
+	if(laser_param.type()=="Livox")
+	{
+		//填充雷达设备的广播码
+		t_error = Livox_driver::get_instance_references().Livox_insert_sn_laser(laser_param.sn(), this);
+		//此时不进行驱动层的初始化,在所有雷达创建完成之后,在laser_manager统一初始化.
+		if ( t_error != SUCCESS)
+		{
+			//如果sn码不规范,或者sn码重复都会引发错误,
+			//故障处理,
+			// 方案一:回退.取消创建雷达的操作.释放雷达内存.
+			//方案二:允许创建雷达,但是状态改为故障.禁用后续的功能.
+
+			//以后再写.
+			;
+		}
+	}
+}
+
+CLivoxLaser::~CLivoxLaser()
+{
+	//清空队列
+	m_queue_livox_data.clear_and_delete();
+}
+
+//雷达链接设备，为3个线程添加线程执行函数。
+Error_manager CLivoxLaser::connect_laser()
+{
+	return Laser_base::connect_laser();
+}
+//雷达断开链接，释放3个线程
+Error_manager CLivoxLaser::disconnect_laser()
+{
+	return Laser_base::disconnect_laser();
+}
+//对外的接口函数，负责接受并处理任务单，
+//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
+Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
+{
+	LOG(INFO) << " ---CLivoxLaser::execute_task run  ---"<< 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::execute_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型，
+	if (p_laser_task->get_task_type() != LASER_BASE_SCAN_TASK)
+	{
+		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 "laser task type error ！= LASER_BASE_SCAN_TASK");
+	}
+
+
+
+	//检查雷达状态
+	t_error = check_status();
+	if ( t_error != SUCCESS )
+	{
+		t_result.compare_and_cover_error(t_error);
+	}
+	else
+	{
+		//接受任务，并将任务的状态改为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.error_manager_reset(Error_code::POINTER_IS_NULL,
+										 Error_level::MINOR_ERROR,
+										 "execute_task mp_task_point_cloud is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else if (mp_laser_task->get_task_cloud_lock() == NULL)
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_cloud_lock is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else
+		{
+			//扫描最大帧数,从任务单导入.
+			m_frame_maxnum = mp_laser_task->get_task_frame_maxnum();
+
+			//设置保存文件的路径
+			t_error = set_open_save_path(mp_laser_task->get_task_save_path(), mp_laser_task->get_task_save_flag());
+			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);
+			}
+			else
+			{
+				//将任务的状态改为 TASK_WORKING 处理中
+				mp_laser_task->set_task_statu(TASK_WORKING);
+			}
+		}
+
+		//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
+		{
+			//忽略轻微故障
+			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+				//将任务的状态改为 TASK_ERROR 结束错误
+				mp_laser_task->set_task_statu(TASK_ERROR);
+			}
+			//返回错误码
+			mp_laser_task->compare_and_cover_task_error_manager(t_result);
+
+		}
+	}
+
+	return t_result;
+}
+
+//检查雷达状态,是否正常运行
+Error_manager CLivoxLaser::check_status()
+{
+	if ( get_laser_statu() == LASER_READY && m_livox_device.device_state == K_DEVICE_STATE_CONNECT )
+	{
+		return Error_code::SUCCESS;
+	}
+	else if ( get_laser_statu() == LASER_BUSY && m_livox_device.device_state == K_DEVICE_STATE_SAMPLING )
+	{
+		return Error_manager(Error_code::lIVOX_STATUS_BUSY, Error_level::MINOR_ERROR,
+							 " CLivoxLaser::check_status error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::lIVOX_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " CLivoxLaser::check_status error ");
+	}
+	return Error_code::SUCCESS;
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager CLivoxLaser::start_scan()
+{
+	LOG(INFO) << " ---CLivoxLaser::start_scan() ---:"<<this;
+	if ( is_ready() )
+	{
+		//清空livox子类的队列，
+		m_queue_livox_data.clear_and_delete();
+		m_frame_count = 0;
+		Livox_driver::get_instance_references().Livox_driver_start_sample(m_livox_device.handle);
+		return Laser_base::start_scan();
+	}
+	else
+	{
+		return Error_manager(Error_code::LIVOX_START_FAILE, Error_level::MINOR_ERROR,
+							 "Laser_base::start_scan() is not ready ");
+	}
+	return Error_code::SUCCESS;
+}
+//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+Error_manager CLivoxLaser::stop_scan()
+{
+	//防止多次调用
+	if ( m_laser_scan_flag )
+	{
+		LOG(INFO) << " ---CLivoxLaser::stop_scan() ---:"<<this;
+		Livox_driver::get_instance_references().Livox_driver_stop_sample(m_livox_device.handle);
+		return Laser_base::stop_scan();
+	}
+	return Error_code::SUCCESS;
+
+}
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager CLivoxLaser::end_task()
+{
+	stop_scan();
+	m_frame_count = 0;
+	m_queue_livox_data.clear_and_delete();
+	return Laser_base::end_task();
+}
+
+//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
+//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
+bool CLivoxLaser::is_ready()
+{
+	//livox雷达设备的状态，livox sdk后台线程的状态
+	if ( m_laser_statu == LASER_READY && m_livox_device.device_state == K_DEVICE_STATE_CONNECT  )
+	{
+		true;
+	}
+	else
+	{
+		false;
+	}
+}
+
+//判断是否扫描完成
+bool CLivoxLaser::is_scan_complete()
+{
+	//雷达的采集帧数判断，直接比较 扫描当前帧数 扫描最大帧数
+	return m_frame_count >= m_frame_maxnum;
+}
+
+//往数据缓存里面添加雷达数据,
+Error_manager CLivoxLaser::push_livox_data(Binary_buf* p_binary_buf)
+{
+	if(m_queue_livox_data.push(p_binary_buf))
+	{
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::lIVOX_CANNOT_PUSH_DATA, Error_level::MINOR_ERROR,
+							 " CLivoxLaser::push_livox_data error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//雷达帧数计数增加
+Error_manager CLivoxLaser::add_livox_frame(int add_count)
+{
+	m_frame_count += add_count;
+	return Error_code::SUCCESS;
+}
+
+
+
+
+
+
+
+
+
+
+
+void CLivoxLaser::set_livox_device(uint8_t handle, DeviceState device_state , DeviceInfo info  )
+{
+	m_livox_device.handle = handle;
+	m_livox_device.device_state = device_state;
+	m_livox_device.info = info;
+}
+
+void CLivoxLaser::set_handle(uint8_t handle)
+{
+	m_livox_device.handle = handle;
+
+}
+
+void CLivoxLaser::set_device_state(DeviceState device_state)
+{
+	m_livox_device.device_state = device_state;
+}
+CLivoxLaser::DeviceState CLivoxLaser::get_device_state()
+{
+	return m_livox_device.device_state;
+}
+
+
+void CLivoxLaser::set_device_info(DeviceInfo info)
+{
+	m_livox_device.info = info;
+
+}
+
+
+
+
+
+
+
+
+//接受二进制消息的功能函数，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，
+// 纯虚函数，必须由子类重载，
+Error_manager CLivoxLaser::receive_buf_to_queue()
+{
+	Binary_buf* tp_livox_buf = NULL;
+	if (m_queue_livox_data.try_pop(tp_livox_buf))
+	{
+		//tp_livox_buf此时是有内存的, 是 livox_data_callback 里面new出来的
+		//Binary_buf 内存的管理权限    从 m_queue_livox_data 直接进入到 m_queue_laser_data,
+		//数据最终在 Laser_base::thread_transform()  transform_buf_to_points  释放.
+		if(m_queue_laser_data.push(tp_livox_buf))
+		{
+			return Error_code::SUCCESS;
+		}
+		else
+		{
+			//这里要销毁 tp_livox_buf 的内存
+			delete(tp_livox_buf);
+			return Error_manager(Error_code::LASER_QUEUE_ERROR, Error_level::MINOR_ERROR,
+								 " CLivoxLaser::receive_buf_to_queue() error ");
+		}
+	}
+	else
+	{
+		return NO_DATA;// m_queue_livox_data 没有数据并不意味着程序就出错了,
+	}
+	return Error_code::SUCCESS;
+}
+//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
+// 纯虚函数，必须由子类重载，
+Error_manager CLivoxLaser::transform_buf_to_points()
+{
+	Error_manager t_error;
+	Binary_buf* tp_binaty_buf=NULL;
+	//从队列中取出缓存。 tp_binaty_buf的内存 从链表取出时，就自带内存了。需要手动释放。
+	if(m_queue_laser_data.try_pop(tp_binaty_buf))
+	{
+		//livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
+		LivoxRawPoint *tp_Livox_data = (LivoxRawPoint *)tp_binaty_buf->get_buf();
+		//计算这一帧数据有多少个三维点。
+		int t_count = tp_binaty_buf->get_length() / (sizeof(LivoxRawPoint));
+		//转变三维点格式，并存入vector。
+		for (int i = 0; i < t_count; ++i)
+		{
+			//三维点的坐标变换，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
+			CPoint3D point_source;
+			point_source.x = tp_Livox_data[i].x;
+			point_source.y = tp_Livox_data[i].y;
+			point_source.z = tp_Livox_data[i].z;
+			CPoint3D point_destination = transform_by_matrix(point_source);
+
+			//保存雷达扫描 三维点云的最终结果。
+			if(m_save_flag)
+			{
+				char buf[64] = {0};
+				sprintf(buf, "%f %f %f\n", point_destination.x, point_destination.y, point_destination.z);
+				m_points_log_tool.write(buf, strlen(buf));
+			}
+
+			//LivoxRawPoint 转为 pcl::PointXYZ
+			//int32_t 转 double。不要信号强度
+			pcl::PointXYZ  pointXYZ;
+			pointXYZ.x = point_destination.x/1000.0;
+			pointXYZ.y = point_destination.y/1000.0;
+			pointXYZ.z = point_destination.z/1000.0;
+			//注:单位毫米转为米, 最终数据单位统一为米.....
+			t_error =  mp_laser_task->task_push_point(&pointXYZ);
+			if ( t_error != Error_code::SUCCESS )
+			{
+				delete tp_binaty_buf;
+				return t_error;
+			}
+		}
+		//统计扫描点个数。
+		m_points_count += t_count;
+
+		delete tp_binaty_buf;
+	}
+	else
+	{
+		return NO_DATA;// m_queue_laser_data 没有数据并不意味着程序就出错了,
+	}
+	return Error_code::SUCCESS;
+}

laser/LivoxLaser.h

@@ -0,0 +1,104 @@
+#ifndef __LIVOX_MID_40_LIDAR__H
+#define __LIVOX_MID_40_LIDAR__H
+#include "Laser.h"
+#include "livox_def.h"
+#include "livox_sdk.h"
+#include <map>
+
+//大疆livox雷达，从Laser_base继承。
+class CLivoxLaser :public Laser_base
+{
+public:
+	//雷达设备状态，livox管理底层sdk，后台线程的工作状态
+	typedef enum
+	{
+		K_DEVICE_STATE_DISCONNECT 		= 0,		//雷达设备状态	断开连接
+		K_DEVICE_STATE_CONNECT 			= 1,		//雷达设备状态	连接正常
+		K_DEVICE_STATE_SAMPLING 		= 2,		//雷达设备状态	正在扫描
+		K_DEVICE_STATE_FAULT			= 3,		//雷达设备状态	故障
+	} DeviceState;
+
+	//雷达设备信息，
+	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();
+
+	//雷达链接设备，为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_status();
+	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+	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();
+
+	//判断是否扫描完成
+	virtual bool is_scan_complete();
+
+	//往数据缓存里面添加雷达数据,
+	virtual Error_manager push_livox_data(Binary_buf* p_binary_buf);
+
+	//雷达帧数计数增加
+	virtual Error_manager add_livox_frame(int add_count);
+
+
+public://set and get
+	void set_livox_device(uint8_t handle, DeviceState device_state = K_DEVICE_STATE_DISCONNECT, DeviceInfo info = DeviceInfo() );
+
+	void set_handle(uint8_t handle);
+
+	void set_device_state(DeviceState device_state);
+	CLivoxLaser::DeviceState get_device_state();
+
+	void set_device_info(DeviceInfo info);
+
+
+protected:
+	//接受二进制消息的功能函数，并将其存入 m_queue_laser_data, 每次只转化一个CBinaryData，
+	// 纯虚函数，必须由子类重载，
+	virtual Error_manager receive_buf_to_queue();
+	//将二进制消息转化为三维点云的功能函数，并将其存入 任务单 , 每次只转化一个CBinaryData，
+	// 纯虚函数，必须由子类重载，
+	virtual Error_manager transform_buf_to_points();
+
+
+
+protected:
+	DeviceItem								m_livox_device;//雷达设备信息,handle在这里面.
+	unsigned int							m_frame_count;//扫描当前帧数
+	unsigned int							m_frame_maxnum;//扫描最大帧数
+	Thread_safe_queue<Binary_buf*>			m_queue_livox_data;//雷达数据
+	//注意了:雷达每帧数据都包含很多个三维点.
+	//例如 livox雷达,每帧数据对应一个Binary_buf.对应100个三维点.
+	//1000帧就是十万个三维点.
+
+};
+
+#endif
+

laser/LivoxLaser.puml

@@ -0,0 +1,230 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+<<<<<<< HEAD
+title  CLivoxLaser 大疆livox雷达
+=======
+title  CLivoxLaser
+>>>>>>> origin/hl
+
+
+
+
+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

laser/LogFiles.cpp

@@ -0,0 +1,145 @@
+
+#include "LogFiles.h"
+#include <string.h>
+
+const string CLogFiles::m_strFileNameLoglaserBinary1 = "laser1.data";
+const string CLogFiles::m_strFileNameLoglaserValue1 = "laser1.txt";
+const string CLogFiles::m_strFileNameLogPoints1 = "points1.txt";
+
+const string CLogFiles::m_strFileNameLoglaserBinary2 = "laser2.data";
+const string CLogFiles::m_strFileNameLoglaserValue2 = "laser2.txt";
+const string CLogFiles::m_strFileNameLogPoints2 = "points2.txt";
+
+
+
+CLogFile::CLogFile()
+{
+
+}
+
+
+CLogFile::~CLogFile()
+{
+	if (m_file)
+	{
+        m_lock.lock();
+		m_file.close();
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::open(const char *_Filename, bool binary /* = false */)
+{
+	if (!m_file.is_open())
+	{
+        m_lock.lock();
+		if (binary)
+			m_file.open(_Filename, ios_base::out | ios_base::binary);
+		else
+			m_file.open(_Filename, ios_base::out);
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write(const char *_Str,	streamsize _Count)
+{
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+		m_file.write(_Str, _Count);
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write_double(double val)
+{
+	char buffer[512] = { 0 };
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+        sprintf(buffer, "%f ", val);
+		m_file.write(buffer, strlen(buffer));
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::write_int(int val)
+{
+	char buffer[512] = { 0 };
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+        sprintf(buffer, "%d ", val);
+		m_file.write(buffer, strlen(buffer));
+        m_lock.unlock();
+	}
+}
+
+void CLogFile::close()
+{
+	if (m_file.is_open())
+	{
+        m_lock.lock();
+		m_file.close();
+        m_lock.unlock();
+	}
+}
+
+/****************************************************************************/
+
+CLogFiles::CLogFiles()
+	: m_bBinary(true)
+	, m_bValue(true)
+	, m_bPoints(true)
+{
+
+}
+
+CLogFiles::~CLogFiles()
+{
+	close_project();
+}
+
+void CLogFiles::close_project()
+{
+	m_logLaserBinary1.close();
+	m_logLaserValue1.close();
+	m_logPoints1.close();
+
+	m_logLaserBinary2.close();
+	m_logLaserValue2.close();
+	m_logPoints2.close();
+}
+
+void CLogFiles::new_project(const char * path)
+{
+	close_project();
+
+	if (m_bBinary) 
+	{
+		string filepath1 = string(path) + m_strFileNameLoglaserBinary1;
+		m_logLaserBinary1.open(filepath1.c_str(), true);
+
+		string filepath3 = string(path) + m_strFileNameLoglaserBinary2;
+		m_logLaserBinary2.open(filepath3.c_str(), true);
+	}
+
+	if (m_bValue)
+	{
+		string filepath1 = string(path) + m_strFileNameLoglaserValue1;
+		m_logLaserValue1.open(filepath1.c_str(), true);
+
+		string filepath3 = string(path) + m_strFileNameLoglaserValue2;
+		m_logLaserValue2.open(filepath3.c_str(), true);
+	}
+
+	if (m_bPoints)
+	{
+		string filepath2 = string(path) + m_strFileNameLogPoints1;
+		m_logPoints1.open(filepath2.c_str(), true);
+
+		string filepath4 = string(path) + m_strFileNameLogPoints2;
+		m_logPoints2.open(filepath4.c_str(), true);
+	}
+	
+}

laser/LogFiles.h

@@ -0,0 +1,73 @@
+#ifndef __LOG__FILE__HH
+#define __LOG__FILE__HH
+#include <string>
+#include <fstream>
+#include <mutex>
+
+using namespace std;
+
+class CLogFile
+{
+public:
+	CLogFile();
+	~CLogFile();
+
+	void open(const char *_Filename, bool binary = false);
+	void write(const char *_Str, streamsize _Count);
+	void close();
+	inline bool is_open() const { return m_file.is_open(); };
+
+	void write_double(double val);
+	void write_int(int val);
+
+public:
+	fstream m_file;
+
+public:
+    std::mutex m_lock;
+};
+
+
+class CLogFiles
+{
+public:
+	CLogFiles();
+	~CLogFiles();
+
+	void set_config(bool binary = true, bool value = true, bool points = true)
+	{
+		m_bBinary = binary;
+		m_bValue = value;
+		m_bPoints = points;
+	}
+
+	void new_project(const char * path);
+	void close_project();
+
+private:
+	bool m_bBinary;
+	bool m_bValue;
+	bool m_bPoints;
+
+
+public:		  
+	CLogFile m_logLaserBinary1;
+	CLogFile m_logLaserValue1;
+	CLogFile m_logPoints1;
+		
+	CLogFile m_logLaserBinary2;
+	CLogFile m_logLaserValue2;
+	CLogFile m_logPoints2;
+
+
+public:
+	static const string m_strFileNameLoglaserBinary1;
+	static const string m_strFileNameLoglaserValue1;
+	static const string m_strFileNameLogPoints1;
+
+	static const string m_strFileNameLoglaserBinary2;
+	static const string m_strFileNameLoglaserValue2;
+	static const string m_strFileNameLogPoints2;
+};
+
+#endif

laser/Point2D.cpp

@@ -0,0 +1,24 @@
+
+#include "Point2D.h"
+
+
+CPoint2D::CPoint2D(double x, double y)
+{
+	this->x = x;
+	this->y = y;
+}
+
+
+CPoint2D::~CPoint2D()
+{
+}
+
+double CPoint2D::GetDistance(CPoint2D &pt1, CPoint2D &pt2)
+{
+	return sqrt((pt1.x - pt2.x)*(pt1.x - pt2.x) + (pt1.y - pt2.y)*(pt1.y - pt2.y));
+}
+
+double CPoint2D::GetDistance(CPoint2D &pt)
+{
+	return sqrt((x - pt.x)*(x - pt.x) + (y - pt.y)*(y - pt.y));
+}

laser/Point2D.h

@@ -0,0 +1,35 @@
+#ifndef POINT_2D__HH__
+#define POINT_2D__HH__
+#include <math.h>
+
+#ifndef PI
+#define PI 3.141592653589793238462643383279502f   
+#endif
+
+#ifndef DEGREES
+#define DEGREES 0.01745329251994329576923690768489f                           
+#endif
+
+class CPoint2D
+{
+public:
+	double x, y;
+
+public:
+	CPoint2D(double x = 0, double y = 0);
+	virtual ~CPoint2D();
+	
+	inline double GetDistance(CPoint2D &pt);
+	static double GetDistance(CPoint2D &pt1, CPoint2D &pt2);
+};
+
+class CPointPolar2D
+{
+public:
+	double dist, theta;
+
+public:
+	CPointPolar2D(double d = 0, double t = 0) : dist(.0), theta(.0) { dist = d; theta = t; };
+	virtual ~CPointPolar2D() {};
+};
+#endif

laser/Point3D.cpp

@@ -0,0 +1,34 @@
+
+#include "Point3D.h"
+
+
+CPoint3D::CPoint3D()
+{
+	x = y = z = 0;
+}
+
+
+CPoint3D::~CPoint3D()
+{
+
+}
+
+CPoint3D::CPoint3D(double c1, double c2, double c3)
+{
+    x = c1;      y = c2;      z = c3;
+}
+
+
+
+CPoint3D &CPoint3D::operator=(const CPoint3D& pt)
+{
+	x = pt.x;   z = pt.z;   y = pt.y;
+    return *this;
+}
+
+void CPoint3D::Translate(double cx, double cy, double cz)
+{
+    x += cx;
+    y += cy;
+    z += cz;
+}

laser/Point3D.h

@@ -0,0 +1,19 @@
+#ifndef __POINT_3D__HH
+#define __POINT_3D__HH
+class CPoint3D
+{
+public:
+	double x, y, z;
+
+	CPoint3D();
+	~CPoint3D();
+    CPoint3D(double c1, double c2, double c3);
+    CPoint3D& operator=(const CPoint3D& pt);
+	CPoint3D(const CPoint3D& pt)
+	{
+		*this = pt;
+	}
+    void Translate(double cx, double cy, double cz); //ת��
+};
+
+#endif

laser/laser_manager.cpp

@@ -0,0 +1,534 @@
+
+
+
+#include "./laser_manager.h"
+
+#include "../tool/proto_tool.h"
+#include "../laser/laser_parameter.pb.h"
+#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()
+{
+	laser_manager_uninit();
+}
+
+//初始化 雷达 管理模块。如下三选一
+Error_manager Laser_manager::laser_manager_init()
+{
+	return  laser_manager_init_from_protobuf(LASER_PARAMETER_PATH);
+}
+//初始化 雷达 管理模块。从文件读取
+Error_manager Laser_manager::laser_manager_init_from_protobuf(std::string prototxt_path)
+{
+	Laser_proto::Laser_parameter_all t_laser_parameters;
+
+	if(!  proto_tool::read_proto_param(prototxt_path,t_laser_parameters) )
+	{
+		return Error_manager(LASER_MANAGER_READ_PROTOBUF_ERROR,MINOR_ERROR,"Laser_manager read_proto_param  failed");
+	}
+
+	return laser_manager_init_from_protobuf(t_laser_parameters);
+}
+//初始化 雷达 管理模块。从protobuf读取
+Error_manager Laser_manager::laser_manager_init_from_protobuf(Laser_proto::Laser_parameter_all& laser_parameters)
+{
+	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;
+
+	//创建大疆雷达
+	int laser_cout=laser_parameters.laser_parameters_size();
+	m_laser_vector.resize(laser_cout);
+	for(int i=0;i<laser_parameters.laser_parameters_size();++i)
+	{
+		//创建雷达, CreateLaser 会为雷达实例new内存
+		m_laser_vector[i]=LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(),i,
+													  laser_parameters.laser_parameters(i));
+		if(m_laser_vector[i]!=NULL)
+		{
+			if(m_laser_vector[i]->connect_laser()!=SUCCESS)
+			{
+				char description[255]={0};
+				sprintf(description,"Laser %d connect failed...",i);
+				return Error_manager(LASER_CONNECT_FAILED,MINOR_ERROR,description);
+			}
+		}
+	}
+	//查询是否有livox雷达,初始化 Livox_driver
+	for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
+	{
+		std::string type = laser_parameters.laser_parameters(i).type();
+		if (type.find("Livox") != type.npos)
+		{
+			t_error = Livox_driver::get_instance_references().Livox_driver_init() ;
+			if ( t_error != Error_code::SUCCESS )
+			{
+				return t_error;
+			}
+			break;
+		}
+	}
+
+	m_laser_manager_status = LASER_MANAGER_READY;
+
+	//启动雷达管理模块的内部线程。默认wait。
+	m_laser_manager_condition.reset(false, false, false);
+	mp_laser_manager_thread = new std::thread(&Laser_manager::thread_work, this);
+
+	return Error_code::SUCCESS;
+}
+//反初始化 雷达 管理模块。
+Error_manager Laser_manager::laser_manager_uninit()
+{
+	LOG(INFO) << " ---laser_manager_uninit run--- "<< this;
+	//关闭线程
+	if (mp_laser_manager_thread)
+	{
+		m_laser_manager_condition.kill_all();
+	}
+	//回收线程的资源
+	if (mp_laser_manager_thread)
+	{
+		mp_laser_manager_thread->join();
+		delete mp_laser_manager_thread;
+		mp_laser_manager_thread = NULL;
+	}
+
+	//回收雷达的内存
+	for (int i = 0; i < m_laser_number; ++i)
+	{
+
+		delete(m_laser_vector[i]);
+	}
+	m_laser_manager_status = LASER_MANAGER_UNKNOW;
+	m_laser_number = 0;
+	m_laser_vector.clear();
+	m_laser_manager_working_flag = false;
+	//反初始化 Livox_driver
+	Livox_driver::get_instance_references().Livox_driver_uninit();
+
+	//回收下发雷达任务单
+	for (map<int, Laser_task*>::iterator iter  = m_laser_task_map.begin(); iter != m_laser_task_map.end(); )
+	{
+
+		delete(iter->second);
+		iter = m_laser_task_map.erase(iter);
+		//注：erase 删除当前 iter 之后返回下一个节点，当前的 iter 无效化，
+		//for循环不用 ++iter
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+//对外的接口函数，负责接受并处理任务单，
+//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+Error_manager Laser_manager::execute_task(Task_Base* p_laser_task)
+{
+	LOG(INFO) << " ---Laser_manager::execute_task run---  "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//检查指针
+	if (p_laser_task == NULL) {
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "Laser_manager::execute_task failed, POINTER_IS_NULL");
+	}
+	//检查任务类型，
+	if (p_laser_task->get_task_type() != LASER_MANGER_SCAN_TASK)
+	{
+		return Error_manager(Error_code::LASER_MANAGER_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
+							 "laser task type error ！= LASER_MANGER_SCAN_TASK ");
+	}
+
+	//检查接收方的状态
+	t_error = check_status();
+	if ( t_error != SUCCESS )
+	{
+		t_result.compare_and_cover_error(t_error);
+	}
+	else
+	{
+		//接受任务，并将任务的状态改为TASK_SIGNED已签收
+		mp_laser_manager_task = (Laser_manager_task *) p_laser_task;
+		mp_laser_manager_task->set_task_statu(TASK_SIGNED);
+
+		//检查消息内容是否正确，
+		//检查三维点云指针
+		if (mp_laser_manager_task->get_task_point_cloud_map() == NULL)
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_point_cloud is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else if ( mp_laser_manager_task->get_task_cloud_lock() == NULL )
+		{
+			t_error.error_manager_reset(Error_code::POINTER_IS_NULL,
+										Error_level::MINOR_ERROR,
+										"execute_task mp_task_cloud_lock is null");
+			t_result.compare_and_cover_error(t_error);
+		}
+		else
+		{
+			//启动雷达管理模块，的核心工作线程
+			m_laser_manager_status = LASER_MANAGER_ISSUED_TASK;
+			m_laser_manager_working_flag = true;
+			m_laser_manager_condition.notify_all(true);
+			//通知 thread_work 子线程启动。
+
+			//将任务的状态改为 TASK_WORKING 处理中
+			mp_laser_manager_task->set_task_statu(TASK_WORKING);
+		}
+
+		//return 之前要填充任务单里面的错误码.
+		if (t_result != Error_code::SUCCESS)
+		{
+			//忽略轻微故障
+			if ( t_result.get_error_level() >= Error_level::MINOR_ERROR)
+			{
+				//将任务的状态改为 TASK_ERROR 结束错误
+				mp_laser_manager_task->set_task_statu(TASK_ERROR);
+			}
+			//返回错误码
+			mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
+		}
+	}
+
+	return t_result;
+
+}
+
+//检查雷达状态,是否正常运行
+Error_manager Laser_manager::check_status()
+{
+	if ( m_laser_manager_status == LASER_MANAGER_READY )
+	{
+		return Error_code::SUCCESS;
+	}
+	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,
+							 " Laser_manager::check_status error ");
+	}
+	else
+	{
+		return Error_manager(Error_code::LASER_MANAGER_STATUS_ERROR, Error_level::MINOR_ERROR,
+							 " Laser_manager::check_status error ");
+	}
+	return Error_code::SUCCESS;
+}
+//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+Error_manager Laser_manager::end_task()
+{
+	LOG(INFO) << " ---Laser_manager::end_task run---"<< this;
+
+	//关闭子线程
+	m_laser_manager_working_flag=false;
+	m_laser_manager_condition.notify_all(false);
+
+
+	//释放下发的任务单
+	laser_task_map_clear_and_delete();
+
+	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
+	if(mp_laser_manager_task !=NULL)
+	{
+		//判断任务单的错误等级，
+		if ( mp_laser_manager_task->get_task_error_manager().get_error_level() < Error_level::MINOR_ERROR)
+		{
+			if ( m_laser_manager_status == LASER_MANAGER_ISSUED_TASK  || m_laser_manager_status == LASER_MANAGER_WAIT_REPLY )
+			{
+				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR，则恢复待机，可以接受下一次的任务单
+				m_laser_manager_status = LASER_MANAGER_READY;
+			}
+			//else 状态不变
+
+			//强制改为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);
+		}
+	}
+//	std::cout << "------Laser_manager::end_task()------."<<mp_laser_manager_task->get_task_error_manager().to_string() << std::endl;
+	return Error_code::SUCCESS;
+}
+
+//取消任务单，由发送方提前取消任务单
+Error_manager Laser_manager::cancel_task()
+{
+	//需要先取消子任务.
+	map<int, Laser_task*>::iterator map_iter;
+	for (  map_iter = m_laser_task_map.begin(); map_iter != m_laser_task_map.end(); ++map_iter)
+	{
+			m_laser_vector[map_iter->first]->cancel_task();
+			map_iter->second->set_task_statu(TASK_DEAD);
+	}
+
+	end_task();
+	//强制改为 TASK_DEAD，不管它当前在做什么。
+	mp_laser_manager_task->set_task_statu(TASK_DEAD);
+
+	return Error_code::SUCCESS;
+}
+
+
+//释放下发的任务单
+Error_manager Laser_manager::laser_task_map_clear_and_delete()
+{
+	for ( map<int, Laser_task*>::iterator map_iter2 = m_laser_task_map.begin(); map_iter2 != m_laser_task_map.end(); )
+	{
+		//销毁下发的任务单。
+		delete(map_iter2->second);
+		map_iter2 = m_laser_task_map.erase(map_iter2);
+		//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
+		//for循环不用 ++map_iter2
+	}
+	return Error_code::SUCCESS;
+}
+
+//判断是否为待机，如果已经准备好，则可以执行任务。
+bool Laser_manager::is_ready()
+{
+	return m_laser_manager_status == LASER_MANAGER_READY;
+}
+
+
+
+//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+Error_manager Laser_manager::issued_task(int laser_index)
+{
+	Error_manager t_error;
+
+	if ( laser_index < 0 || laser_index >= m_laser_number )
+	{
+		return Error_manager(Error_code::LASER_MANAGER_LASER_INDEX_ERRPR, Error_level::MINOR_ERROR,
+							 " issued_task laser_index  error ");
+	}
+
+	//查重
+	map<int, Laser_task*>::iterator map_iter = m_laser_task_map.find(laser_index);
+	if ( map_iter != m_laser_task_map.end() )
+	{
+		//如果重复，则不做任何处理,视为无效行为.
+		//返回轻微错误.仅仅提示作用.
+		return Error_manager(Error_code::LASER_MANAGER_LASER_INDEX_REPEAT, Error_level::NEGLIGIBLE_ERROR,
+							" issued_task  laser_index repeart ");
+	}
+	else
+	{
+		//创建雷达扫描任务,
+		// 这里为下发雷达任务分配内存，后续要记得回收。
+		Laser_task* t_laser_task=new Laser_task();
+		mp_laser_manager_task->trans_to_laser_task(t_laser_task,laser_index,m_laser_vector[laser_index],
+												   TASK_CREATED,std::chrono::milliseconds(7000));
+		//保存雷达扫描任务 到 m_laser_task_map
+		m_laser_task_map[laser_index] = t_laser_task;
+
+		//发送任务单给雷达
+		t_error = Task_command_manager::get_instance_references().execute_task(t_laser_task);
+	}
+
+
+
+	if(t_error!=SUCCESS)
+	{
+		;
+		//这里的故障处理很复杂，以后再写。
+	}
+
+	return t_error;
+
+}
+
+//故障处理,下发任务的故障处理.
+Error_manager Laser_manager::troubleshoot_for_issued_task(Error_manager error, int laser_index)
+{
+	if (error == LASER_MANAGER_LASER_INDEX_ERRPR || error == LASER_MANAGER_LASER_INDEX_REPEAT   )
+	{
+	    //雷达索引错误,降级为可忽略的故障,然后返回轻微故障.
+		error.set_error_level_down(NEGLIGIBLE_ERROR);
+		// 此时没有创建子任务,后面等待任务返回时会忽略掉.
+		return error;
+	}
+	if (error == POINTER_IS_NULL || error == LIVOX_TASK_TYPE_ERROR   )
+	{
+		//雷达对象接受任务,发现任务单下发错误.应该是整个任务通信模块错误了.将主任务错误码升级
+		error.set_error_level_up(CRITICAL_ERROR);
+		return error;
+	}
+	//其他情况雷达对象会签收任务,之后出错会填充到任务单里面的错误码.
+	//即使接收方的状态不对,也会签收任务,然后把错误填充到任务单里面的错误码.
+	//后续在等待答复时,会统一处理子任务单所有的错误.
+
+
+	return error;
+}
+
+//mp_laser_manager_thread 线程执行函数，
+//thread_work 内部线程负责分发扫描任务给下面的雷达，并且回收汇总雷达的数据
+void Laser_manager::thread_work()
+{
+	LOG(INFO) << " mp_laser_manager_thread start "<< this;
+	Error_manager t_error;
+	Error_manager t_result;
+
+	//雷达管理的独立线程，负责控制管理所有的雷达。
+	while (m_laser_manager_condition.is_alive())
+	{
+		m_laser_manager_condition.wait();
+		if ( m_laser_manager_condition.is_alive() )
+		{
+			std::this_thread::yield();
+			//重新循环必须清除错误码.
+			t_error.error_manager_clear_all();
+			t_result.error_manager_clear_all();
+
+			if ( mp_laser_manager_task == NULL )
+			{
+				m_laser_manager_status = LASER_MANAGER_FAULT;
+				m_laser_manager_working_flag = false;
+				m_laser_manager_condition.notify_all(false);
+			}
+			//第一步
+			//下发任务, 雷达管理模块给子雷达下发扫描任务.
+			else if(m_laser_manager_status == LASER_MANAGER_ISSUED_TASK)
+			{
+				//分发扫描任务给下面的雷达
+				if ( mp_laser_manager_task->get_select_all_laser_flag() )
+				{
+					//下发任务给所有的雷达
+					for (int i = 0; i < m_laser_number; ++i)
+					{
+						//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+						t_error = issued_task(i);
+						if ( t_error != SUCCESS)
+						{
+							//下发子任务的故障处理
+							t_error = troubleshoot_for_issued_task(t_error, i);
+							t_result.compare_and_cover_error(t_error);
+						}
+					}
+				}
+				else
+				{
+					//下发任务给选中的雷达
+					std::vector<int> & t_select_laser_id_vector = mp_laser_manager_task->get_select_laser_id_vector();
+					for (int i = 0; i < t_select_laser_id_vector.size(); ++i)
+					{
+						//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+						t_error = issued_task(t_select_laser_id_vector[i]);
+						if ( t_error != SUCCESS)
+						{
+							//下发子任务的故障处理
+							t_error = troubleshoot_for_issued_task(t_error, i);
+							t_result.compare_and_cover_error(t_error);
+						}
+					}
+				}
+
+				//故障汇总, 只处理2级以上的故障
+				if(t_result.get_error_level() >= MINOR_ERROR)
+				{
+					m_laser_manager_status = LASER_MANAGER_FAULT;
+					mp_laser_manager_task->set_task_statu(TASK_ERROR);
+					//因为故障,而提前结束任务.
+					mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
+					end_task();
+				}
+				else
+				{
+					//下发任务之后，修改状态为等待答复。
+					m_laser_manager_status = LASER_MANAGER_WAIT_REPLY;
+				}
+			}
+			//第二步
+			//等待答复, 注意不能写成死循环阻塞,
+			//子任务没完成就直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive()),
+			// 这样线程仍然被 m_laser_manager_condition 控制
+			else if(m_laser_manager_status == LASER_MANAGER_WAIT_REPLY)
+			{
+				map<int, Laser_task*>::iterator map_iter;
+				for (  map_iter = m_laser_task_map.begin(); map_iter != m_laser_task_map.end(); ++map_iter)
+				{
+
+					if ( map_iter->second->is_task_end() == false)
+					{
+						if ( map_iter->second->is_over_time() )
+						{
+							//超时处理。取消任务。
+							m_laser_vector[map_iter->first]->cancel_task();
+							map_iter->second->set_task_statu(TASK_DEAD);
+							t_error.error_manager_reset(Error_code::LASER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
+														" LASER_TASK is_over_time ");
+							map_iter->second->set_task_error_manager(t_error);
+
+							continue;
+							//本次子任务超时死亡.直接进行下一个
+						}
+						else
+						{
+							//如果任务还在执行, 那就等待任务继续完成，等1ms
+							//std::this_thread::sleep_for(std::chrono::milliseconds(1));
+							std::this_thread::yield();
+							break;
+							//注意了:这里break之后,map_iter不会指向 m_laser_task_map.end()
+							//这就表示仍然有子任务单没有结束.那就继续等待,,,直到所有任务完成或者超时.
+						}
+
+					}
+					//else 任务完成就判断下一个
+				}
+
+				//如果下发的任务全部结束，那么汇总所有的结果, 并销毁下发的任务单。
+				if(map_iter == m_laser_task_map.end() )
+				{
+					for ( map<int, Laser_task*>::iterator map_iter2 = m_laser_task_map.begin(); map_iter2 != m_laser_task_map.end(); )
+					{
+						//数据汇总，
+						//由于 pcl::PointCloud<pcl::PointXYZ>::Ptr mp_task_point_cloud; 是所有任务单共用的，所以不用处理了。自动就在一起。
+
+						//错误汇总
+						t_result.compare_and_cover_error(map_iter2->second->get_task_error_manager());
+
+						//销毁下发的任务单。
+						delete(map_iter2->second);
+						map_iter2 = m_laser_task_map.erase(map_iter2);
+						//注：erase 删除当前map_iter2之后返回下一个节点，当前的map_iter2无效化，
+						//for循环不用 ++map_iter2
+					}
+
+					m_laser_manager_working_flag = false;
+					mp_laser_manager_task->compare_and_cover_task_error_manager(t_result);
+					//正常结束任务
+					end_task();
+				}
+				//else 直接通过, 然后重新进入  while (m_laser_manager_condition.is_alive())
+			}
+		}
+	}
+	LOG(INFO) << " mp_laser_manager_thread end :"<<this;
+	return;
+}
+
+
+

laser/laser_manager.h

@@ -0,0 +1,129 @@
+
+/*
+ * Laser_manager 是雷达模块的总管理类。
+ * Laser_manager 作为一个单例，全局可以使用。 get_instance 系列的函数可获取唯一的实例。
+ * 它作为一个独立的模块，不和外部有直接的联系。
+ * 对外只有一个接口函数 execute_task 用以执行 任务单 Laser_manager_task
+ *
+ * Laser_manager 模块内部包含了所有的雷达指针。
+ * 还自带一个线程，用来控制雷达。
+ *
+ * 注意了:
+ * std::vector<Laser_base*>     m_laser_vector;		//雷达的对象实例，内存由本类管理
+ * Livox_driver 单例  大疆雷达的底层驱动
+ * 这2个平级, 都归 Laser_manager 控制管理.
+ *
+ * */
+
+
+
+#ifndef LASER_MANAGER_H
+#define LASER_MANAGER_H
+
+#include "../error_code/error_code.h"
+#include "../tool/singleton.h"
+#include "../tool/thread_condition.h"
+
+#include "../laser/Laser.h"
+#include "../laser/laser_manager_task.h"
+
+
+#define LASER_PARAMETER_PATH "../setting/laser.prototxt"
+
+class Laser_manager:public Singleton<Laser_manager>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<Laser_manager>;
+
+public:
+	//雷达管理模块的工作状态
+	enum Laser_manager_status
+	{//default LASER_MANAGER_UNKNOW = 0
+	    LASER_MANAGER_UNKNOW               	= 0,    //未知
+	    LASER_MANAGER_READY               	= 1,    //准备，待机
+		LASER_MANAGER_ISSUED_TASK			= 2,	//工作下发任务
+		LASER_MANAGER_WAIT_REPLY			= 3,	//工作等待答复
+		LASER_MANAGER_FAULT					= 4,	//故障
+	};
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	Laser_manager();
+public:
+	// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	Laser_manager(const Laser_manager&)=delete;
+	Laser_manager& operator =(const Laser_manager&)= delete;
+	~Laser_manager();
+
+
+
+public: //API 对外接口
+	//初始化 雷达 管理模块。如下三选一
+	Error_manager laser_manager_init();
+	//初始化 雷达 管理模块。从文件读取
+	Error_manager laser_manager_init_from_protobuf(std::string prototxt_path);
+	//初始化 雷达 管理模块。从protobuf读取
+	Error_manager laser_manager_init_from_protobuf(Laser_proto::Laser_parameter_all& laser_parameters);
+	//反初始化 雷达 管理模块。
+	Error_manager laser_manager_uninit();
+
+
+	//对外的接口函数，负责接受并处理任务单，
+	//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+	Error_manager execute_task(Task_Base* p_laser_task);
+	//检查雷达状态,是否正常运行
+	Error_manager check_status();
+	//结束任务单，里面会根据任务的故障等级修正雷达管理模块的状态和任务单的状态
+	Error_manager end_task();
+	//取消任务单，由发送方提前取消任务单
+	Error_manager cancel_task();
+
+	//释放下发的任务单
+	Error_manager laser_task_map_clear_and_delete();
+
+	//判断是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+
+public://get or set member variable
+
+
+protected:
+	//下发给子雷达任务。这里为下发雷达任务分配内存, 后续要记得回收 m_laser_task_map 。
+	Error_manager issued_task(int laser_index);
+
+	//故障处理,下发任务的故障处理.
+	Error_manager troubleshoot_for_issued_task(Error_manager error, int laser_index);
+
+	//mp_laser_manager_thread 线程执行函数，
+	//thread_work 内部线程负责分发扫描任务给下面的雷达，并且回收汇总雷达的数据
+	void thread_work();
+
+
+
+
+
+protected://member variable
+
+	//子雷达
+	int 										m_laser_number;					//雷达的个数
+	std::vector<Laser_base*>                    m_laser_vector;					//雷达的对象实例，内存由本类管理
+	//注意：里面的Laser_base*是在init里面new出来的，析构之前必须使用ununit来销毁
+
+	Laser_manager_status						m_laser_manager_status;			//雷达管理模块的工作状态
+	std::atomic<bool>                			m_laser_manager_working_flag;          	//雷达管理的工作使能标志位
+	map<int, Laser_task*>						m_laser_task_map;						//雷达管理下发给雷达子模块的任务map，内存由本类管理
+
+
+	std::thread*        						mp_laser_manager_thread;   		//雷达管理的线程指针，内存由本类管理
+	Thread_condition							m_laser_manager_condition;		//雷达管理的条件变量
+
+	//雷达管理的任务单指针，实际内存由发送方控制管理，//接受任务后，指向新的任务单
+	Laser_manager_task *  						mp_laser_manager_task;        		//雷达管理任务单的指针，内存由发送方管理。
+
+private:
+
+};
+
+
+
+
+#endif //LASER_MANAGER_H

laser/laser_manager_task.cpp

@@ -0,0 +1,301 @@
+
+/*
+ * Laser_manager_task 是雷达管理的任务单。
+ * 外部通过它对 Laser_manager 下发任务。
+ *
+//用法：上级直接调用 Laser_manager 的接口函数 execute_task ，并将 Laser_manager_task 类作为参数传递
+// Laser_manager 类则按照 Laser_manager_task 的功能码和指定的参数，执行相应的功能
+//并将结果填充到 Laser_manager_task ，返回给上级。
+ *
+ *
+ * */
+
+#include "laser_manager_task.h"
+
+
+//构造函数，构造函数锁定任务类型为 LASER_MANGER_SCAN_TASK ，后续不允许更改
+Laser_manager_task::Laser_manager_task()
+{
+//构造函数锁定任务类型为 LASER_MANGER_SCAN_TASK，后续不允许更改
+	m_task_type = LASER_MANGER_SCAN_TASK;
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_frame_maxnum = 0;
+	m_task_save_flag = false;//默认不保存，false
+	m_task_save_path.clear();
+	mp_task_point_cloud_map = NULL;
+	mp_task_cloud_lock=NULL;
+	m_cloud_aggregation_flag = false;
+
+	m_select_all_laser_flag = true;
+	m_select_laser_id_vector.clear();
+}
+
+//析构函数
+Laser_manager_task::~Laser_manager_task()
+{
+
+}
+
+//初始化任务单，必须初始化之后才可以使用，（必选参数）
+// input:p_task_cloud_lock 三维点云的数据保护锁
+// output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Laser_manager_task::task_init(std::mutex* p_task_cloud_lock,
+											std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map)
+{
+	if(p_task_point_cloud_map == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Laser_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_frame_maxnum = 0;
+	m_task_save_flag = false;
+	m_task_save_path.clear();
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+	m_cloud_aggregation_flag = false;
+
+	m_select_all_laser_flag = true;
+	m_select_laser_id_vector.clear();
+
+	return Error_code::SUCCESS;
+}
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_frame_maxnum 点云的采集帧数最大值
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_point_cloud 三维点云容器的智能指针
+//    input:m_is_select_all_laser 是否选取所有的雷达
+//    input:select_laser_number_vector 选取的指定雷达id
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Laser_manager_task::task_init(Task_statu task_statu,
+											std::string task_statu_information,
+											void* p_tast_receiver,
+											std::chrono::milliseconds task_over_time,
+											bool task_save_flag,
+											std::string task_save_path,
+											std::mutex* p_task_cloud_lock,
+											std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map,
+											bool cloud_aggregation_flag,
+											unsigned int task_frame_maxnum,
+											bool select_all_laser_flag,
+											std::vector<int> select_laser_id_vector
+)
+{
+	if(p_task_point_cloud_map == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Laser_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = task_statu;
+	m_task_statu_information = task_statu_information;
+	mp_tast_receiver = p_tast_receiver;
+	m_task_over_time = task_over_time;
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_frame_maxnum = task_frame_maxnum;
+	m_task_save_flag = task_save_flag;
+	m_task_save_path = task_save_path;
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+	m_cloud_aggregation_flag = cloud_aggregation_flag;
+
+	m_select_all_laser_flag = select_all_laser_flag;
+	m_select_laser_id_vector = select_laser_id_vector;
+
+	return Error_code::SUCCESS;
+}
+
+
+
+//把 Laser_manager_task 转化为 Laser_task，注意 task_statu 从外部载入。
+//output：p_laser_task 雷达扫描任务的指针，
+//input：laser_index 雷达编号
+//input：p_laser_base 接受任务的 子雷达
+//input：task_statu 外部载入的任务状态，默认为 TASK_CREATED
+//input：task_over_time 超时时间，一般要比上级任务的时间短一些
+Error_manager Laser_manager_task::trans_to_laser_task(Laser_task * p_laser_task, int laser_index,Laser_base* p_laser_base,
+													  Task_statu task_statu,
+													  std::chrono::milliseconds task_over_time)
+{
+	if ( p_laser_task ==NULL || p_laser_base == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							" trans_to_laser_task input parameter POINTER_IS_NULL ");
+	}
+
+	//该子任务存放在哪个cloud map
+	int map_index;
+	if ( m_cloud_aggregation_flag  )
+	{
+		//如果汇总, 就全部存放于map[0]
+		map_index = 0;
+	}
+	else
+	{
+		//如果不汇总, 就分开存放于map[laser_index]
+		map_index = laser_index;
+	}
+
+	//查看map[map_index] 是否存在
+	if ( mp_task_point_cloud_map->count(map_index) == 0 )
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+		(*mp_task_point_cloud_map)[map_index] = tp_cloud;
+
+		p_laser_task->task_init(task_statu,
+								m_task_statu_information,
+								p_laser_base,
+								task_over_time,
+								m_task_frame_maxnum,
+								m_task_save_flag,
+								m_task_save_path,
+								mp_task_cloud_lock,
+								tp_cloud
+		);
+	}
+	else
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr tp_cloud = (*mp_task_point_cloud_map)[map_index];
+
+		p_laser_task->task_init(task_statu,
+								m_task_statu_information,
+								p_laser_base,
+								task_over_time,
+								m_task_frame_maxnum,
+								m_task_save_flag,
+								m_task_save_path,
+								mp_task_cloud_lock,
+								tp_cloud
+		);
+	}
+
+	return Error_code::SUCCESS;
+}
+
+
+
+//获取 点云的采集帧数最大值
+unsigned int Laser_manager_task::get_task_frame_maxnum()
+{
+	return m_task_frame_maxnum;
+}
+//获取采集的点云保存文件的使能标志位
+bool Laser_manager_task::get_task_save_flag()
+{
+	return m_task_save_flag;
+}
+//获取采集的点云保存路径
+std::string Laser_manager_task::get_task_save_path()
+{
+	return m_task_save_path;
+}
+
+//获取 三维点云容器的锁
+std::mutex* Laser_manager_task::get_task_cloud_lock()
+{
+	return mp_task_cloud_lock;
+}
+//获取 三维点云容器的智能指针map
+std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* Laser_manager_task::get_task_point_cloud_map()
+{
+	return mp_task_point_cloud_map;
+}
+//获取 所有雷达的三维点云是否聚集的标志位
+bool Laser_manager_task::get_cloud_aggregation_flag()
+{
+	return m_cloud_aggregation_flag;
+}
+
+
+
+//设置 点云的采集帧数最大值
+void Laser_manager_task::set_task_frame_maxnum(unsigned int task_frame_maxnum)
+{
+	m_task_frame_maxnum = task_frame_maxnum;
+
+}
+//设置采集的点云保存文件的使能标志位
+void Laser_manager_task::set_task_save_flag(bool task_save_flag)
+{
+	m_task_save_flag=task_save_flag;
+}
+//设置采集的点云保存路径
+void Laser_manager_task::set_task_save_path(std::string task_save_path)
+{
+	m_task_save_path=task_save_path;
+}
+//设置采集的点云保存标志位和路径
+void Laser_manager_task::set_task_save_flag_and_path(bool task_save_flag, std::string task_save_path)
+{
+	m_task_save_flag=task_save_flag;
+	m_task_save_path=task_save_path;
+}
+//设置 三维点云容器的锁
+void Laser_manager_task::set_task_cloud_lock(std::mutex* mp_task_cloud_lock)
+{
+	mp_task_cloud_lock = mp_task_cloud_lock;
+}
+//设置 三维点云容器的智能指针
+void Laser_manager_task::set_task_point_cloud(std::map<int,pcl::PointCloud<pcl::PointXYZ>::Ptr>* p_task_point_cloud_map)
+{
+	mp_task_point_cloud_map = p_task_point_cloud_map;
+}
+//设置 所有雷达的三维点云是否聚集的标志位
+void Laser_manager_task::set_cloud_aggregation_flag(bool cloud_aggregation_flag)
+{
+	m_cloud_aggregation_flag = cloud_aggregation_flag;
+}
+
+//获取 是否选取所有的雷达
+bool Laser_manager_task::get_select_all_laser_flag()
+{
+	return m_select_all_laser_flag;
+}
+//设置 是否选取所有的雷达
+void Laser_manager_task::set_select_all_laser_flag( bool select_all_laser_flag)
+{
+	m_select_all_laser_flag = select_all_laser_flag;
+}
+//获取 被选中的雷达编号。
+std::vector<int>& Laser_manager_task::get_select_laser_id_vector()
+{
+	return m_select_laser_id_vector;
+}
+//设置 被选中的雷达编号。
+void Laser_manager_task::set_select_laser_id_vector(std::vector<int>& select_laser_id_vector)
+{
+	m_select_laser_id_vector = select_laser_id_vector;
+}
+
+
+
+
+
+
+

laser/laser_manager_task.h

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

laser/laser_message.pb.cc

@@ -0,0 +1,518 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: laser_message.proto
+
+#include "laser_message.pb.h"
+
+#include <algorithm>
+
+#include <google/protobuf/stubs/common.h>
+#include <google/protobuf/stubs/port.h>
+#include <google/protobuf/stubs/once.h>
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/wire_format_lite_inl.h>
+#include <google/protobuf/descriptor.h>
+#include <google/protobuf/generated_message_reflection.h>
+#include <google/protobuf/reflection_ops.h>
+#include <google/protobuf/wire_format.h>
+// This is a temporary google only hack
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+#include "third_party/protobuf/version.h"
+#endif
+// @@protoc_insertion_point(includes)
+namespace laser_message {
+class laserMsgDefaultTypeInternal {
+ public:
+  ::google::protobuf::internal::ExplicitlyConstructed<laserMsg>
+      _instance;
+} _laserMsg_default_instance_;
+}  // namespace laser_message
+namespace protobuf_laser_5fmessage_2eproto {
+void InitDefaultslaserMsgImpl() {
+  GOOGLE_PROTOBUF_VERIFY_VERSION;
+
+#ifdef GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  ::google::protobuf::internal::InitProtobufDefaultsForceUnique();
+#else
+  ::google::protobuf::internal::InitProtobufDefaults();
+#endif  // GOOGLE_PROTOBUF_ENFORCE_UNIQUENESS
+  {
+    void* ptr = &::laser_message::_laserMsg_default_instance_;
+    new (ptr) ::laser_message::laserMsg();
+    ::google::protobuf::internal::OnShutdownDestroyMessage(ptr);
+  }
+  ::laser_message::laserMsg::InitAsDefaultInstance();
+}
+
+void InitDefaultslaserMsg() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &InitDefaultslaserMsgImpl);
+}
+
+::google::protobuf::Metadata file_level_metadata[1];
+const ::google::protobuf::EnumDescriptor* file_level_enum_descriptors[1];
+
+const ::google::protobuf::uint32 TableStruct::offsets[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::laser_message::laserMsg, _has_bits_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::laser_message::laserMsg, _internal_metadata_),
+  ~0u,  // no _extensions_
+  ~0u,  // no _oneof_case_
+  ~0u,  // no _weak_field_map_
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::laser_message::laserMsg, laser_status_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::laser_message::laserMsg, queue_data_count_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::laser_message::laserMsg, cloud_count_),
+  GOOGLE_PROTOBUF_GENERATED_MESSAGE_FIELD_OFFSET(::laser_message::laserMsg, id_),
+  0,
+  1,
+  2,
+  3,
+};
+static const ::google::protobuf::internal::MigrationSchema schemas[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+  { 0, 9, sizeof(::laser_message::laserMsg)},
+};
+
+static ::google::protobuf::Message const * const file_default_instances[] = {
+  reinterpret_cast<const ::google::protobuf::Message*>(&::laser_message::_laserMsg_default_instance_),
+};
+
+void protobuf_AssignDescriptors() {
+  AddDescriptors();
+  ::google::protobuf::MessageFactory* factory = NULL;
+  AssignDescriptors(
+      "laser_message.proto", schemas, file_default_instances, TableStruct::offsets, factory,
+      file_level_metadata, file_level_enum_descriptors, NULL);
+}
+
+void protobuf_AssignDescriptorsOnce() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &protobuf_AssignDescriptors);
+}
+
+void protobuf_RegisterTypes(const ::std::string&) GOOGLE_PROTOBUF_ATTRIBUTE_COLD;
+void protobuf_RegisterTypes(const ::std::string&) {
+  protobuf_AssignDescriptorsOnce();
+  ::google::protobuf::internal::RegisterAllTypes(file_level_metadata, 1);
+}
+
+void AddDescriptorsImpl() {
+  InitDefaults();
+  static const char descriptor[] GOOGLE_PROTOBUF_ATTRIBUTE_SECTION_VARIABLE(protodesc_cold) = {
+      "\n\023laser_message.proto\022\rlaser_message\"w\n\010"
+      "laserMsg\0220\n\014laser_status\030\001 \001(\0162\032.laser_m"
+      "essage.laserStatus\022\030\n\020queue_data_count\030\002"
+      " \001(\005\022\023\n\013cloud_count\030\003 \001(\005\022\n\n\002id\030\004 \002(\005*]\n"
+      "\013laserStatus\022\023\n\017eLaserConnected\020\000\022\026\n\022eLa"
+      "serDisconnected\020\001\022\016\n\neLaserBusy\020\002\022\021\n\reLa"
+      "serUnknown\020\003"
+  };
+  ::google::protobuf::DescriptorPool::InternalAddGeneratedFile(
+      descriptor, 252);
+  ::google::protobuf::MessageFactory::InternalRegisterGeneratedFile(
+    "laser_message.proto", &protobuf_RegisterTypes);
+}
+
+void AddDescriptors() {
+  static GOOGLE_PROTOBUF_DECLARE_ONCE(once);
+  ::google::protobuf::GoogleOnceInit(&once, &AddDescriptorsImpl);
+}
+// Force AddDescriptors() to be called at dynamic initialization time.
+struct StaticDescriptorInitializer {
+  StaticDescriptorInitializer() {
+    AddDescriptors();
+  }
+} static_descriptor_initializer;
+}  // namespace protobuf_laser_5fmessage_2eproto
+namespace laser_message {
+const ::google::protobuf::EnumDescriptor* laserStatus_descriptor() {
+  protobuf_laser_5fmessage_2eproto::protobuf_AssignDescriptorsOnce();
+  return protobuf_laser_5fmessage_2eproto::file_level_enum_descriptors[0];
+}
+bool laserStatus_IsValid(int value) {
+  switch (value) {
+    case 0:
+    case 1:
+    case 2:
+    case 3:
+      return true;
+    default:
+      return false;
+  }
+}
+
+
+// ===================================================================
+
+void laserMsg::InitAsDefaultInstance() {
+}
+#if !defined(_MSC_VER) || _MSC_VER >= 1900
+const int laserMsg::kLaserStatusFieldNumber;
+const int laserMsg::kQueueDataCountFieldNumber;
+const int laserMsg::kCloudCountFieldNumber;
+const int laserMsg::kIdFieldNumber;
+#endif  // !defined(_MSC_VER) || _MSC_VER >= 1900
+
+laserMsg::laserMsg()
+  : ::google::protobuf::Message(), _internal_metadata_(NULL) {
+  if (GOOGLE_PREDICT_TRUE(this != internal_default_instance())) {
+    ::protobuf_laser_5fmessage_2eproto::InitDefaultslaserMsg();
+  }
+  SharedCtor();
+  // @@protoc_insertion_point(constructor:laser_message.laserMsg)
+}
+laserMsg::laserMsg(const laserMsg& from)
+  : ::google::protobuf::Message(),
+      _internal_metadata_(NULL),
+      _has_bits_(from._has_bits_),
+      _cached_size_(0) {
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  ::memcpy(&laser_status_, &from.laser_status_,
+    static_cast<size_t>(reinterpret_cast<char*>(&id_) -
+    reinterpret_cast<char*>(&laser_status_)) + sizeof(id_));
+  // @@protoc_insertion_point(copy_constructor:laser_message.laserMsg)
+}
+
+void laserMsg::SharedCtor() {
+  _cached_size_ = 0;
+  ::memset(&laser_status_, 0, static_cast<size_t>(
+      reinterpret_cast<char*>(&id_) -
+      reinterpret_cast<char*>(&laser_status_)) + sizeof(id_));
+}
+
+laserMsg::~laserMsg() {
+  // @@protoc_insertion_point(destructor:laser_message.laserMsg)
+  SharedDtor();
+}
+
+void laserMsg::SharedDtor() {
+}
+
+void laserMsg::SetCachedSize(int size) const {
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+}
+const ::google::protobuf::Descriptor* laserMsg::descriptor() {
+  ::protobuf_laser_5fmessage_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_laser_5fmessage_2eproto::file_level_metadata[kIndexInFileMessages].descriptor;
+}
+
+const laserMsg& laserMsg::default_instance() {
+  ::protobuf_laser_5fmessage_2eproto::InitDefaultslaserMsg();
+  return *internal_default_instance();
+}
+
+laserMsg* laserMsg::New(::google::protobuf::Arena* arena) const {
+  laserMsg* n = new laserMsg;
+  if (arena != NULL) {
+    arena->Own(n);
+  }
+  return n;
+}
+
+void laserMsg::Clear() {
+// @@protoc_insertion_point(message_clear_start:laser_message.laserMsg)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  // Prevent compiler warnings about cached_has_bits being unused
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  if (cached_has_bits & 15u) {
+    ::memset(&laser_status_, 0, static_cast<size_t>(
+        reinterpret_cast<char*>(&id_) -
+        reinterpret_cast<char*>(&laser_status_)) + sizeof(id_));
+  }
+  _has_bits_.Clear();
+  _internal_metadata_.Clear();
+}
+
+bool laserMsg::MergePartialFromCodedStream(
+    ::google::protobuf::io::CodedInputStream* input) {
+#define DO_(EXPRESSION) if (!GOOGLE_PREDICT_TRUE(EXPRESSION)) goto failure
+  ::google::protobuf::uint32 tag;
+  // @@protoc_insertion_point(parse_start:laser_message.laserMsg)
+  for (;;) {
+    ::std::pair< ::google::protobuf::uint32, bool> p = input->ReadTagWithCutoffNoLastTag(127u);
+    tag = p.first;
+    if (!p.second) goto handle_unusual;
+    switch (::google::protobuf::internal::WireFormatLite::GetTagFieldNumber(tag)) {
+      // optional .laser_message.laserStatus laser_status = 1;
+      case 1: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(8u /* 8 & 0xFF */)) {
+          int value;
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   int, ::google::protobuf::internal::WireFormatLite::TYPE_ENUM>(
+                 input, &value)));
+          if (::laser_message::laserStatus_IsValid(value)) {
+            set_laser_status(static_cast< ::laser_message::laserStatus >(value));
+          } else {
+            mutable_unknown_fields()->AddVarint(
+                1, static_cast< ::google::protobuf::uint64>(value));
+          }
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // optional int32 queue_data_count = 2;
+      case 2: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(16u /* 16 & 0xFF */)) {
+          set_has_queue_data_count();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &queue_data_count_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // optional int32 cloud_count = 3;
+      case 3: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(24u /* 24 & 0xFF */)) {
+          set_has_cloud_count();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &cloud_count_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      // required int32 id = 4;
+      case 4: {
+        if (static_cast< ::google::protobuf::uint8>(tag) ==
+            static_cast< ::google::protobuf::uint8>(32u /* 32 & 0xFF */)) {
+          set_has_id();
+          DO_((::google::protobuf::internal::WireFormatLite::ReadPrimitive<
+                   ::google::protobuf::int32, ::google::protobuf::internal::WireFormatLite::TYPE_INT32>(
+                 input, &id_)));
+        } else {
+          goto handle_unusual;
+        }
+        break;
+      }
+
+      default: {
+      handle_unusual:
+        if (tag == 0) {
+          goto success;
+        }
+        DO_(::google::protobuf::internal::WireFormat::SkipField(
+              input, tag, _internal_metadata_.mutable_unknown_fields()));
+        break;
+      }
+    }
+  }
+success:
+  // @@protoc_insertion_point(parse_success:laser_message.laserMsg)
+  return true;
+failure:
+  // @@protoc_insertion_point(parse_failure:laser_message.laserMsg)
+  return false;
+#undef DO_
+}
+
+void laserMsg::SerializeWithCachedSizes(
+    ::google::protobuf::io::CodedOutputStream* output) const {
+  // @@protoc_insertion_point(serialize_start:laser_message.laserMsg)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional .laser_message.laserStatus laser_status = 1;
+  if (cached_has_bits & 0x00000001u) {
+    ::google::protobuf::internal::WireFormatLite::WriteEnum(
+      1, this->laser_status(), output);
+  }
+
+  // optional int32 queue_data_count = 2;
+  if (cached_has_bits & 0x00000002u) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(2, this->queue_data_count(), output);
+  }
+
+  // optional int32 cloud_count = 3;
+  if (cached_has_bits & 0x00000004u) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(3, this->cloud_count(), output);
+  }
+
+  // required int32 id = 4;
+  if (cached_has_bits & 0x00000008u) {
+    ::google::protobuf::internal::WireFormatLite::WriteInt32(4, this->id(), output);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    ::google::protobuf::internal::WireFormat::SerializeUnknownFields(
+        _internal_metadata_.unknown_fields(), output);
+  }
+  // @@protoc_insertion_point(serialize_end:laser_message.laserMsg)
+}
+
+::google::protobuf::uint8* laserMsg::InternalSerializeWithCachedSizesToArray(
+    bool deterministic, ::google::protobuf::uint8* target) const {
+  (void)deterministic; // Unused
+  // @@protoc_insertion_point(serialize_to_array_start:laser_message.laserMsg)
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = _has_bits_[0];
+  // optional .laser_message.laserStatus laser_status = 1;
+  if (cached_has_bits & 0x00000001u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteEnumToArray(
+      1, this->laser_status(), target);
+  }
+
+  // optional int32 queue_data_count = 2;
+  if (cached_has_bits & 0x00000002u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(2, this->queue_data_count(), target);
+  }
+
+  // optional int32 cloud_count = 3;
+  if (cached_has_bits & 0x00000004u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(3, this->cloud_count(), target);
+  }
+
+  // required int32 id = 4;
+  if (cached_has_bits & 0x00000008u) {
+    target = ::google::protobuf::internal::WireFormatLite::WriteInt32ToArray(4, this->id(), target);
+  }
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    target = ::google::protobuf::internal::WireFormat::SerializeUnknownFieldsToArray(
+        _internal_metadata_.unknown_fields(), target);
+  }
+  // @@protoc_insertion_point(serialize_to_array_end:laser_message.laserMsg)
+  return target;
+}
+
+size_t laserMsg::ByteSizeLong() const {
+// @@protoc_insertion_point(message_byte_size_start:laser_message.laserMsg)
+  size_t total_size = 0;
+
+  if (_internal_metadata_.have_unknown_fields()) {
+    total_size +=
+      ::google::protobuf::internal::WireFormat::ComputeUnknownFieldsSize(
+        _internal_metadata_.unknown_fields());
+  }
+  // required int32 id = 4;
+  if (has_id()) {
+    total_size += 1 +
+      ::google::protobuf::internal::WireFormatLite::Int32Size(
+        this->id());
+  }
+  if (_has_bits_[0 / 32] & 7u) {
+    // optional .laser_message.laserStatus laser_status = 1;
+    if (has_laser_status()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::EnumSize(this->laser_status());
+    }
+
+    // optional int32 queue_data_count = 2;
+    if (has_queue_data_count()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->queue_data_count());
+    }
+
+    // optional int32 cloud_count = 3;
+    if (has_cloud_count()) {
+      total_size += 1 +
+        ::google::protobuf::internal::WireFormatLite::Int32Size(
+          this->cloud_count());
+    }
+
+  }
+  int cached_size = ::google::protobuf::internal::ToCachedSize(total_size);
+  GOOGLE_SAFE_CONCURRENT_WRITES_BEGIN();
+  _cached_size_ = cached_size;
+  GOOGLE_SAFE_CONCURRENT_WRITES_END();
+  return total_size;
+}
+
+void laserMsg::MergeFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_merge_from_start:laser_message.laserMsg)
+  GOOGLE_DCHECK_NE(&from, this);
+  const laserMsg* source =
+      ::google::protobuf::internal::DynamicCastToGenerated<const laserMsg>(
+          &from);
+  if (source == NULL) {
+  // @@protoc_insertion_point(generalized_merge_from_cast_fail:laser_message.laserMsg)
+    ::google::protobuf::internal::ReflectionOps::Merge(from, this);
+  } else {
+  // @@protoc_insertion_point(generalized_merge_from_cast_success:laser_message.laserMsg)
+    MergeFrom(*source);
+  }
+}
+
+void laserMsg::MergeFrom(const laserMsg& from) {
+// @@protoc_insertion_point(class_specific_merge_from_start:laser_message.laserMsg)
+  GOOGLE_DCHECK_NE(&from, this);
+  _internal_metadata_.MergeFrom(from._internal_metadata_);
+  ::google::protobuf::uint32 cached_has_bits = 0;
+  (void) cached_has_bits;
+
+  cached_has_bits = from._has_bits_[0];
+  if (cached_has_bits & 15u) {
+    if (cached_has_bits & 0x00000001u) {
+      laser_status_ = from.laser_status_;
+    }
+    if (cached_has_bits & 0x00000002u) {
+      queue_data_count_ = from.queue_data_count_;
+    }
+    if (cached_has_bits & 0x00000004u) {
+      cloud_count_ = from.cloud_count_;
+    }
+    if (cached_has_bits & 0x00000008u) {
+      id_ = from.id_;
+    }
+    _has_bits_[0] |= cached_has_bits;
+  }
+}
+
+void laserMsg::CopyFrom(const ::google::protobuf::Message& from) {
+// @@protoc_insertion_point(generalized_copy_from_start:laser_message.laserMsg)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+void laserMsg::CopyFrom(const laserMsg& from) {
+// @@protoc_insertion_point(class_specific_copy_from_start:laser_message.laserMsg)
+  if (&from == this) return;
+  Clear();
+  MergeFrom(from);
+}
+
+bool laserMsg::IsInitialized() const {
+  if ((_has_bits_[0] & 0x00000008) != 0x00000008) return false;
+  return true;
+}
+
+void laserMsg::Swap(laserMsg* other) {
+  if (other == this) return;
+  InternalSwap(other);
+}
+void laserMsg::InternalSwap(laserMsg* other) {
+  using std::swap;
+  swap(laser_status_, other->laser_status_);
+  swap(queue_data_count_, other->queue_data_count_);
+  swap(cloud_count_, other->cloud_count_);
+  swap(id_, other->id_);
+  swap(_has_bits_[0], other->_has_bits_[0]);
+  _internal_metadata_.Swap(&other->_internal_metadata_);
+  swap(_cached_size_, other->_cached_size_);
+}
+
+::google::protobuf::Metadata laserMsg::GetMetadata() const {
+  protobuf_laser_5fmessage_2eproto::protobuf_AssignDescriptorsOnce();
+  return ::protobuf_laser_5fmessage_2eproto::file_level_metadata[kIndexInFileMessages];
+}
+
+
+// @@protoc_insertion_point(namespace_scope)
+}  // namespace laser_message
+
+// @@protoc_insertion_point(global_scope)

laser/laser_message.pb.h

@@ -0,0 +1,350 @@
+// Generated by the protocol buffer compiler.  DO NOT EDIT!
+// source: laser_message.proto
+
+#ifndef PROTOBUF_laser_5fmessage_2eproto__INCLUDED
+#define PROTOBUF_laser_5fmessage_2eproto__INCLUDED
+
+#include <string>
+
+#include <google/protobuf/stubs/common.h>
+
+#if GOOGLE_PROTOBUF_VERSION < 3005000
+#error This file was generated by a newer version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please update
+#error your headers.
+#endif
+#if 3005000 < GOOGLE_PROTOBUF_MIN_PROTOC_VERSION
+#error This file was generated by an older version of protoc which is
+#error incompatible with your Protocol Buffer headers.  Please
+#error regenerate this file with a newer version of protoc.
+#endif
+
+#include <google/protobuf/io/coded_stream.h>
+#include <google/protobuf/arena.h>
+#include <google/protobuf/arenastring.h>
+#include <google/protobuf/generated_message_table_driven.h>
+#include <google/protobuf/generated_message_util.h>
+#include <google/protobuf/metadata.h>
+#include <google/protobuf/message.h>
+#include <google/protobuf/repeated_field.h>  // IWYU pragma: export
+#include <google/protobuf/extension_set.h>  // IWYU pragma: export
+#include <google/protobuf/generated_enum_reflection.h>
+#include <google/protobuf/unknown_field_set.h>
+// @@protoc_insertion_point(includes)
+
+namespace protobuf_laser_5fmessage_2eproto {
+// Internal implementation detail -- do not use these members.
+struct TableStruct {
+  static const ::google::protobuf::internal::ParseTableField entries[];
+  static const ::google::protobuf::internal::AuxillaryParseTableField aux[];
+  static const ::google::protobuf::internal::ParseTable schema[1];
+  static const ::google::protobuf::internal::FieldMetadata field_metadata[];
+  static const ::google::protobuf::internal::SerializationTable serialization_table[];
+  static const ::google::protobuf::uint32 offsets[];
+};
+void AddDescriptors();
+void InitDefaultslaserMsgImpl();
+void InitDefaultslaserMsg();
+inline void InitDefaults() {
+  InitDefaultslaserMsg();
+}
+}  // namespace protobuf_laser_5fmessage_2eproto
+namespace laser_message {
+class laserMsg;
+class laserMsgDefaultTypeInternal;
+extern laserMsgDefaultTypeInternal _laserMsg_default_instance_;
+}  // namespace laser_message
+namespace laser_message {
+
+enum laserStatus {
+  eLaserConnected = 0,
+  eLaserDisconnected = 1,
+  eLaserBusy = 2,
+  eLaserUnknown = 3
+};
+bool laserStatus_IsValid(int value);
+const laserStatus laserStatus_MIN = eLaserConnected;
+const laserStatus laserStatus_MAX = eLaserUnknown;
+const int laserStatus_ARRAYSIZE = laserStatus_MAX + 1;
+
+const ::google::protobuf::EnumDescriptor* laserStatus_descriptor();
+inline const ::std::string& laserStatus_Name(laserStatus value) {
+  return ::google::protobuf::internal::NameOfEnum(
+    laserStatus_descriptor(), value);
+}
+inline bool laserStatus_Parse(
+    const ::std::string& name, laserStatus* value) {
+  return ::google::protobuf::internal::ParseNamedEnum<laserStatus>(
+    laserStatus_descriptor(), name, value);
+}
+// ===================================================================
+
+class laserMsg : public ::google::protobuf::Message /* @@protoc_insertion_point(class_definition:laser_message.laserMsg) */ {
+ public:
+  laserMsg();
+  virtual ~laserMsg();
+
+  laserMsg(const laserMsg& from);
+
+  inline laserMsg& operator=(const laserMsg& from) {
+    CopyFrom(from);
+    return *this;
+  }
+  #if LANG_CXX11
+  laserMsg(laserMsg&& from) noexcept
+    : laserMsg() {
+    *this = ::std::move(from);
+  }
+
+  inline laserMsg& operator=(laserMsg&& from) noexcept {
+    if (GetArenaNoVirtual() == from.GetArenaNoVirtual()) {
+      if (this != &from) InternalSwap(&from);
+    } else {
+      CopyFrom(from);
+    }
+    return *this;
+  }
+  #endif
+  inline const ::google::protobuf::UnknownFieldSet& unknown_fields() const {
+    return _internal_metadata_.unknown_fields();
+  }
+  inline ::google::protobuf::UnknownFieldSet* mutable_unknown_fields() {
+    return _internal_metadata_.mutable_unknown_fields();
+  }
+
+  static const ::google::protobuf::Descriptor* descriptor();
+  static const laserMsg& default_instance();
+
+  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
+  static inline const laserMsg* internal_default_instance() {
+    return reinterpret_cast<const laserMsg*>(
+               &_laserMsg_default_instance_);
+  }
+  static PROTOBUF_CONSTEXPR int const kIndexInFileMessages =
+    0;
+
+  void Swap(laserMsg* other);
+  friend void swap(laserMsg& a, laserMsg& b) {
+    a.Swap(&b);
+  }
+
+  // implements Message ----------------------------------------------
+
+  inline laserMsg* New() const PROTOBUF_FINAL { return New(NULL); }
+
+  laserMsg* New(::google::protobuf::Arena* arena) const PROTOBUF_FINAL;
+  void CopyFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void MergeFrom(const ::google::protobuf::Message& from) PROTOBUF_FINAL;
+  void CopyFrom(const laserMsg& from);
+  void MergeFrom(const laserMsg& from);
+  void Clear() PROTOBUF_FINAL;
+  bool IsInitialized() const PROTOBUF_FINAL;
+
+  size_t ByteSizeLong() const PROTOBUF_FINAL;
+  bool MergePartialFromCodedStream(
+      ::google::protobuf::io::CodedInputStream* input) PROTOBUF_FINAL;
+  void SerializeWithCachedSizes(
+      ::google::protobuf::io::CodedOutputStream* output) const PROTOBUF_FINAL;
+  ::google::protobuf::uint8* InternalSerializeWithCachedSizesToArray(
+      bool deterministic, ::google::protobuf::uint8* target) const PROTOBUF_FINAL;
+  int GetCachedSize() const PROTOBUF_FINAL { return _cached_size_; }
+  private:
+  void SharedCtor();
+  void SharedDtor();
+  void SetCachedSize(int size) const PROTOBUF_FINAL;
+  void InternalSwap(laserMsg* other);
+  private:
+  inline ::google::protobuf::Arena* GetArenaNoVirtual() const {
+    return NULL;
+  }
+  inline void* MaybeArenaPtr() const {
+    return NULL;
+  }
+  public:
+
+  ::google::protobuf::Metadata GetMetadata() const PROTOBUF_FINAL;
+
+  // nested types ----------------------------------------------------
+
+  // accessors -------------------------------------------------------
+
+  // optional .laser_message.laserStatus laser_status = 1;
+  bool has_laser_status() const;
+  void clear_laser_status();
+  static const int kLaserStatusFieldNumber = 1;
+  ::laser_message::laserStatus laser_status() const;
+  void set_laser_status(::laser_message::laserStatus value);
+
+  // optional int32 queue_data_count = 2;
+  bool has_queue_data_count() const;
+  void clear_queue_data_count();
+  static const int kQueueDataCountFieldNumber = 2;
+  ::google::protobuf::int32 queue_data_count() const;
+  void set_queue_data_count(::google::protobuf::int32 value);
+
+  // optional int32 cloud_count = 3;
+  bool has_cloud_count() const;
+  void clear_cloud_count();
+  static const int kCloudCountFieldNumber = 3;
+  ::google::protobuf::int32 cloud_count() const;
+  void set_cloud_count(::google::protobuf::int32 value);
+
+  // required int32 id = 4;
+  bool has_id() const;
+  void clear_id();
+  static const int kIdFieldNumber = 4;
+  ::google::protobuf::int32 id() const;
+  void set_id(::google::protobuf::int32 value);
+
+  // @@protoc_insertion_point(class_scope:laser_message.laserMsg)
+ private:
+  void set_has_laser_status();
+  void clear_has_laser_status();
+  void set_has_queue_data_count();
+  void clear_has_queue_data_count();
+  void set_has_cloud_count();
+  void clear_has_cloud_count();
+  void set_has_id();
+  void clear_has_id();
+
+  ::google::protobuf::internal::InternalMetadataWithArena _internal_metadata_;
+  ::google::protobuf::internal::HasBits<1> _has_bits_;
+  mutable int _cached_size_;
+  int laser_status_;
+  ::google::protobuf::int32 queue_data_count_;
+  ::google::protobuf::int32 cloud_count_;
+  ::google::protobuf::int32 id_;
+  friend struct ::protobuf_laser_5fmessage_2eproto::TableStruct;
+  friend void ::protobuf_laser_5fmessage_2eproto::InitDefaultslaserMsgImpl();
+};
+// ===================================================================
+
+
+// ===================================================================
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic push
+  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+#endif  // __GNUC__
+// laserMsg
+
+// optional .laser_message.laserStatus laser_status = 1;
+inline bool laserMsg::has_laser_status() const {
+  return (_has_bits_[0] & 0x00000001u) != 0;
+}
+inline void laserMsg::set_has_laser_status() {
+  _has_bits_[0] |= 0x00000001u;
+}
+inline void laserMsg::clear_has_laser_status() {
+  _has_bits_[0] &= ~0x00000001u;
+}
+inline void laserMsg::clear_laser_status() {
+  laser_status_ = 0;
+  clear_has_laser_status();
+}
+inline ::laser_message::laserStatus laserMsg::laser_status() const {
+  // @@protoc_insertion_point(field_get:laser_message.laserMsg.laser_status)
+  return static_cast< ::laser_message::laserStatus >(laser_status_);
+}
+inline void laserMsg::set_laser_status(::laser_message::laserStatus value) {
+  assert(::laser_message::laserStatus_IsValid(value));
+  set_has_laser_status();
+  laser_status_ = value;
+  // @@protoc_insertion_point(field_set:laser_message.laserMsg.laser_status)
+}
+
+// optional int32 queue_data_count = 2;
+inline bool laserMsg::has_queue_data_count() const {
+  return (_has_bits_[0] & 0x00000002u) != 0;
+}
+inline void laserMsg::set_has_queue_data_count() {
+  _has_bits_[0] |= 0x00000002u;
+}
+inline void laserMsg::clear_has_queue_data_count() {
+  _has_bits_[0] &= ~0x00000002u;
+}
+inline void laserMsg::clear_queue_data_count() {
+  queue_data_count_ = 0;
+  clear_has_queue_data_count();
+}
+inline ::google::protobuf::int32 laserMsg::queue_data_count() const {
+  // @@protoc_insertion_point(field_get:laser_message.laserMsg.queue_data_count)
+  return queue_data_count_;
+}
+inline void laserMsg::set_queue_data_count(::google::protobuf::int32 value) {
+  set_has_queue_data_count();
+  queue_data_count_ = value;
+  // @@protoc_insertion_point(field_set:laser_message.laserMsg.queue_data_count)
+}
+
+// optional int32 cloud_count = 3;
+inline bool laserMsg::has_cloud_count() const {
+  return (_has_bits_[0] & 0x00000004u) != 0;
+}
+inline void laserMsg::set_has_cloud_count() {
+  _has_bits_[0] |= 0x00000004u;
+}
+inline void laserMsg::clear_has_cloud_count() {
+  _has_bits_[0] &= ~0x00000004u;
+}
+inline void laserMsg::clear_cloud_count() {
+  cloud_count_ = 0;
+  clear_has_cloud_count();
+}
+inline ::google::protobuf::int32 laserMsg::cloud_count() const {
+  // @@protoc_insertion_point(field_get:laser_message.laserMsg.cloud_count)
+  return cloud_count_;
+}
+inline void laserMsg::set_cloud_count(::google::protobuf::int32 value) {
+  set_has_cloud_count();
+  cloud_count_ = value;
+  // @@protoc_insertion_point(field_set:laser_message.laserMsg.cloud_count)
+}
+
+// required int32 id = 4;
+inline bool laserMsg::has_id() const {
+  return (_has_bits_[0] & 0x00000008u) != 0;
+}
+inline void laserMsg::set_has_id() {
+  _has_bits_[0] |= 0x00000008u;
+}
+inline void laserMsg::clear_has_id() {
+  _has_bits_[0] &= ~0x00000008u;
+}
+inline void laserMsg::clear_id() {
+  id_ = 0;
+  clear_has_id();
+}
+inline ::google::protobuf::int32 laserMsg::id() const {
+  // @@protoc_insertion_point(field_get:laser_message.laserMsg.id)
+  return id_;
+}
+inline void laserMsg::set_id(::google::protobuf::int32 value) {
+  set_has_id();
+  id_ = value;
+  // @@protoc_insertion_point(field_set:laser_message.laserMsg.id)
+}
+
+#ifdef __GNUC__
+  #pragma GCC diagnostic pop
+#endif  // __GNUC__
+
+// @@protoc_insertion_point(namespace_scope)
+
+}  // namespace laser_message
+
+namespace google {
+namespace protobuf {
+
+template <> struct is_proto_enum< ::laser_message::laserStatus> : ::google::protobuf::internal::true_type {};
+template <>
+inline const EnumDescriptor* GetEnumDescriptor< ::laser_message::laserStatus>() {
+  return ::laser_message::laserStatus_descriptor();
+}
+
+}  // namespace protobuf
+}  // namespace google
+
+// @@protoc_insertion_point(global_scope)
+
+#endif  // PROTOBUF_laser_5fmessage_2eproto__INCLUDED

laser/laser_message.proto

@@ -0,0 +1,17 @@
+syntax = "proto2";
+package laser_message;
+
+enum laserStatus
+{
+    eLaserConnected=0;
+    eLaserDisconnected=1;
+    eLaserBusy=2;
+    eLaserUnknown=3;
+}
+message laserMsg
+{
+    optional laserStatus laser_status=1;
+    optional int32 queue_data_count=2;
+    optional int32 cloud_count=3;
+    required int32 id=4;
+}

laser/laser_parameter.proto

@@ -0,0 +1,40 @@
+syntax = "proto2";
+package Laser_proto;
+
+message laser_parameter {
+    optional string laser_ip = 1;
+    optional int64 laser_port = 2;
+    optional int64 laser_port_remote = 3;
+    optional double mat_r00 = 4 [default = 1.0];
+    optional double mat_r01 = 5 [default = 1.0];
+    optional double mat_r02 = 6 [default = 1.0];
+    optional double mat_r03 = 7 [default = 1.0];
+    optional double mat_r10 = 8 [default = 1.0];
+    optional double mat_r11 = 9 [default = 1.0];
+    optional double mat_r12 = 10 [default = 1.0];
+    optional double mat_r13 = 11 [default = 1.0];
+    optional double mat_r20 = 12 [default = 1.0];
+    optional double mat_r21 = 13 [default = 1.0];
+    optional double mat_r22 = 14 [default = 1.0];
+    optional double mat_r23 = 15 [default = 1.0];
+    optional double axis_x_theta = 16;
+    optional double axis_y_theta = 17;
+    optional double axis_z_theta = 18;
+    optional double translation_x = 19;
+    optional double translation_y = 20;
+    optional double translation_z = 21;
+
+    optional double install_angle = 22 [default = 0.0];
+    optional bool scan_direction = 23 [default = true];
+    optional string sn = 24;
+    optional int64 frame_num = 25 [default = 3000];
+    optional string type = 26 [default = ""];
+    optional bool is_save_banary=27 [default=false];
+    optional bool is_save_txt=28 [default=true];
+
+}
+
+message Laser_parameter_all
+{
+    repeated laser_parameter        laser_parameters=1;
+}

laser/laser_task_command.cpp

@@ -0,0 +1,195 @@
+
+//laser_task_command，是雷达任务指令的相关功能
+//功能：用作应用层向laser模块传输任务，的指令消息
+
+//用法：应用层直接调用laser的接口函数，并将Laser_task类作为参数传递
+//CLaser类则按照Laser_task的功能码和指定的参数，执行相应的功能
+//并将结果填充到Laser_task，返回给应用层
+
+#include "laser_task_command.h"
+//构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
+Laser_task::Laser_task()
+{
+    //构造函数锁定任务类型为 LASER_BASE_SCAN_TASK，后续不允许更改
+    m_task_type = LASER_BASE_SCAN_TASK;
+    m_task_statu = TASK_CREATED;
+    m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+    m_task_frame_maxnum = 0;
+	m_task_save_flag = false;//默认不保存，false
+	m_task_save_path.clear();
+    mp_task_point_cloud = NULL;
+    mp_task_cloud_lock = NULL;
+}
+//析构函数
+Laser_task::~Laser_task()
+{
+
+}
+
+//初始化任务单，必须初始化之后才可以使用，（必选参数）
+// input:p_task_cloud_lock 三维点云的数据保护锁
+// output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Laser_task::task_init(std::mutex* p_task_cloud_lock,
+											pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud)
+{
+	if(p_task_point_cloud.get() == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Laser_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = TASK_CREATED;
+	m_task_statu_information.clear();
+	m_task_error_manager.error_manager_clear_all();
+
+	m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
+	m_task_save_flag = false;
+	m_task_save_path.clear();
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
+
+	return Error_code::SUCCESS;
+}
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_statu 任务状态
+//    input:task_statu_information 状态说明
+//    input:tast_receiver 接受对象
+//    input:task_over_time 超时时间
+//    input:task_frame_maxnum 点云的采集帧数最大值
+//    input:task_save_flag 是否保存
+//    input:task_save_path 保存路径
+//    input:p_task_cloud_lock 三维点云的数据保护锁
+//    output:p_task_point_cloud 三维点云容器的智能指针
+//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+Error_manager Laser_task::task_init(Task_statu task_statu,
+											std::string task_statu_information,
+											void* p_tast_receiver,
+											std::chrono::milliseconds task_over_time,
+											unsigned int task_frame_maxnum,
+											bool task_save_flag,
+											std::string task_save_path,
+											std::mutex* p_task_cloud_lock,
+											pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud
+)
+{
+	if(p_task_point_cloud.get() == NULL)
+	{
+		return Error_manager(POINTER_IS_NULL, MINOR_ERROR,
+							 "Laser_task::task_init p_task_point_cloud is null");
+	}
+	if(p_task_cloud_lock==NULL)
+	{
+		return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+							 "Laser_manager_task::task_init p_task_cloud_lock is null");
+	}
+
+	m_task_statu = task_statu;
+	m_task_statu_information = task_statu_information;
+	mp_tast_receiver = p_tast_receiver;
+	m_task_over_time = task_over_time;
+	m_task_error_manager.error_manager_clear_all();
+
+	if ( task_frame_maxnum == 0 )
+	{
+		m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
+	}
+	else
+	{
+		m_task_frame_maxnum = task_frame_maxnum;
+	}
+
+	m_task_save_flag = task_save_flag;
+	m_task_save_path = task_save_path;
+	mp_task_cloud_lock=p_task_cloud_lock;
+	mp_task_point_cloud = p_task_point_cloud;
+
+	return Error_code::SUCCESS;
+}
+
+//push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
+Error_manager Laser_task::task_push_point(pcl::PointXYZ* p_point_xyz)
+{
+    if(mp_task_cloud_lock==NULL)
+    {
+        return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
+                             "push_point laser task input lock is null");
+    }
+    if(mp_task_point_cloud.get()==NULL)
+    {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "Laser_task::task_push_point p_task_point_cloud is null");
+    }
+    //加锁，并添加三维点。
+    mp_task_cloud_lock->lock();
+    mp_task_point_cloud->push_back(*p_point_xyz);
+    mp_task_cloud_lock->unlock();
+    return SUCCESS;
+}
+
+
+
+
+//获取 点云的采集帧数最大值
+unsigned int Laser_task::get_task_frame_maxnum()
+{
+    return m_task_frame_maxnum;
+}
+//获取采集的点云保存文件的使能标志位
+bool Laser_task::get_task_save_flag()
+{
+	return m_task_save_flag;
+}
+//获取采集的点云保存路径
+std::string Laser_task::get_task_save_path()
+{
+    return m_task_save_path;
+}
+//获取 三维点云容器的智能指针
+std::mutex*  Laser_task::get_task_cloud_lock()
+{
+	return mp_task_cloud_lock;
+}
+//获取 三维点云容器的智能指针
+pcl::PointCloud<pcl::PointXYZ>::Ptr Laser_task::get_task_point_cloud()
+{
+    return mp_task_point_cloud;
+}
+
+
+
+
+//设置 点云的采集帧数最大值
+void Laser_task::set_task_frame_maxnum(unsigned int task_frame_maxnum)
+{
+    m_task_frame_maxnum = task_frame_maxnum;
+
+}
+//设置采集的点云保存文件的使能标志位
+void Laser_task::set_task_save_flag(bool task_save_flag)
+{
+	m_task_save_flag=task_save_flag;
+}
+//设置采集的点云保存路径
+void Laser_task::set_task_save_path(std::string task_save_path)
+{
+    m_task_save_path=task_save_path;
+}
+//设置采集的点云保存标志位和路径
+void Laser_task::set_task_save_flag_and_path(bool task_save_flag, std::string task_save_path)
+{
+	m_task_save_flag=task_save_flag;
+	m_task_save_path=task_save_path;
+}
+
+
+
+

laser/laser_task_command.h

@@ -0,0 +1,114 @@
+
+//laser_task_command，是雷达任务指令的相关功能
+//功能：用作应用层向laser模块传输任务，的指令消息
+
+//用法：应用层直接调用laser的接口函数，并将Laser_task类作为参数传递
+//CLaser类则按照Laser_task的功能码和指定的参数，执行相应的功能
+//并将结果填充到Laser_task，返回给应用层
+
+
+#ifndef __LASER_TASK_COMMAND__HH__
+#define __LASER_TASK_COMMAND__HH__
+#include "Point2D.h"
+#include "Point3D.h"
+#include <pcl/point_types.h>
+#include <pcl/common/common.h>
+#include "../error_code/error_code.h"
+
+#include <vector>
+#include <mutex>
+
+#include "../task/task_command_manager.h"
+
+//任务点云的采集帧数最大值，默认值1000
+#define TASK_FRAME_MAXNUM_DEFAULT               1000
+
+//雷达模块的任务指令，从Task_Base继承，
+//补充了雷达专属的数据输入和输出
+class Laser_task:public Task_Base
+{
+public:
+    //构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
+    Laser_task();
+    //析构函数
+    ~Laser_task();
+
+	//初始化任务单，必须初始化之后才可以使用，（必选参数）
+	// input:p_task_cloud_lock 三维点云的数据保护锁
+	// output:p_task_point_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager task_init(std::mutex* p_task_cloud_lock,
+							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
+
+	//初始化任务单，必须初始化之后才可以使用，（可选参数）
+	//    input:task_statu 任务状态
+	//    input:task_statu_information 状态说明
+	//    input:tast_receiver 接受对象
+	//    input:task_over_time 超时时间
+	//    input:task_frame_maxnum 点云的采集帧数最大值
+	//    input:task_save_flag 是否保存
+	//    input:task_save_path 保存路径
+	//    input:p_task_cloud_lock 三维点云的数据保护锁
+	//    output:p_task_pmutexoint_cloud 三维点云容器的智能指针
+	//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
+	Error_manager task_init(Task_statu task_statu,
+							std::string task_statu_information,
+							void* p_tast_receiver,
+							std::chrono::milliseconds task_over_time,
+							unsigned int task_frame_maxnum,
+							bool task_save_flag,
+							std::string task_save_path,
+							std::mutex* p_task_cloud_lock,
+							pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud
+	);
+
+    //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
+    Error_manager task_push_point(pcl::PointXYZ* p_point_xyz);
+
+
+public:
+    //获取 点云的采集帧数最大值
+    unsigned int get_task_frame_maxnum();
+	//获取采集的点云保存文件的使能标志位
+	bool get_task_save_flag();
+    //获取采集的点云保存路径
+    std::string get_task_save_path();
+	//获取 三维点云容器的智能指针
+	std::mutex*  get_task_cloud_lock();
+    //获取 三维点云容器的智能指针
+    pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
+
+
+    //设置 点云的采集帧数最大值
+    void set_task_frame_maxnum(unsigned int task_frame_maxnum);
+	//设置采集的点云保存文件的使能标志位
+	void set_task_save_flag(bool task_save_flag);
+	//设置采集的点云保存路径
+    void set_task_save_path(std::string task_save_path);
+	//设置采集的点云保存标志位和路径
+	void set_task_save_flag_and_path(bool task_save_flag, std::string task_save_path);
+
+
+
+
+protected:
+    //点云的采集帧数最大值，任务输入
+    unsigned int                    m_task_frame_maxnum;
+
+	//雷达保存文件的使能标志位，//默认不保存，false
+	bool		 					m_task_save_flag;
+    //点云保存文件的路径，任务输入
+    std::string                     m_task_save_path;
+
+    //三维点云的数据保护锁，任务输入, (实际上多个雷达会同时往一个pcl点云容器里面填充数据,防止数据冲突,需要对容器加锁.)
+    std::mutex*                     mp_task_cloud_lock;
+    //采集结果，三维点云容器的智能指针，任务输出
+    //这里只是智能指针，实际内存由任务发送方管理管理，初始化时必须保证内存有效。
+    pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
+
+
+};
+
+
+#endif //__LASER_TASK_COMMAND__HH__
+

laser/laser_task_command.puml

@@ -0,0 +1,114 @@
+@startuml
+skinparam classAttributeIconSize 0
+
+<<<<<<< HEAD
+title  Laser_task 雷达模块的任务指令
+=======
+title  binary_buf是二进制缓存
+>>>>>>> origin/hl
+
+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

laser/livox_driver.cpp

@@ -0,0 +1,455 @@
+
+
+/*
+ * livox_driver 大疆雷达的底层驱动
+ *
+ * 本模块作为单例模式,全局使用.
+ * 负责通过调用livox sdk,与livox的后台进行进行对接. 直接控制大疆的雷达设备.
+ *
+
+ */
+
+#include "livox_driver.h"
+
+Livox_driver::Livox_driver()
+{
+	m_init_flag = false;
+}
+
+Livox_driver::~Livox_driver()
+{
+//	因为退出顺序的原因,析构不再自动调用 Livox_driver_uninit ,
+//	而是在推出前手动调用Livox_driver_uninit
+	Livox_driver_uninit();
+}
+
+//livox雷达驱动初始化.
+Error_manager Livox_driver::Livox_driver_init(bool aoto_create_livox_flag)
+{
+	LOG(INFO) << " ---Livox_driver_init run --- "<< this;
+	if (m_init_flag == false)
+	{
+		//Init是livox_sdk.h里面的初始化函数, 此时会创建livox后台线程
+		if (!Init()) {
+			Uninit();
+			LOG(ERROR) << "livox sdk init failed...";
+			return Error_manager(LIVOX_SKD_INIT_FAILED,MINOR_ERROR,"Livox laser init failed...");
+		}
+		else
+		{
+			//显示 livox sdk 版本
+			LivoxSdkVersion _sdkversion;
+			GetLivoxSdkVersion(&_sdkversion);
+			char buf[255] = { 0 };
+			sprintf(buf, "livox sdk init success. \n Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
+			LOG(INFO) << buf;
+
+			m_aoto_create_livox_flag = aoto_create_livox_flag;
+
+			//SetBroadcastCallback 设置广播的回调函数,此时会向局域网发送广播消息.
+			// 收到广播的所有雷达设备都会调用一次livox_device_broadcast_callback,
+			//livox_device_broadcast_callback 函数由livox后台线程来执行.
+			SetBroadcastCallback(livox_device_broadcast_callback);
+
+			//SetDeviceStateUpdateCallback 设置状态更新的回调函数.
+			//雷达设备建立连接或者断开连接,都会调用 livox_device_change_callback
+			//livox_device_change_callback 函数由livox后台线程来执行.
+			SetDeviceStateUpdateCallback(livox_device_change_callback);
+
+			//启动设备底层的扫描线程，该线程在单独在后台运行。(该线程由livox_sdk来维护)
+			//只有在start之后.各种回调函数才能生效.
+			if (!Start())
+			{
+				m_init_flag = false;
+				Uninit();
+				LOG(ERROR) << "livox sdk start failed...";
+				return Error_manager(LIVOX_SKD_INIT_FAILED,MINOR_ERROR,"Livox laser Start failed...");
+			}
+			else
+			{
+				LOG(ERROR) << "livox sdk start success...";
+				m_init_flag = true;
+
+			}
+
+		}
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//livox雷达驱动反初始化.
+Error_manager Livox_driver::Livox_driver_uninit()
+{
+	if ( m_init_flag )
+	{
+		LOG(INFO) << " ---Livox_driver::Livox_driver_uninit() run --- "<< this;
+
+		//回收livox_sdk的相关资源.销毁livox_sdk的后台线程
+		Uninit();
+		m_init_flag = false;
+	}
+
+	return Error_code::SUCCESS;
+}
+
+//插入广播码和雷达实例的映射表
+Error_manager Livox_driver::Livox_insert_sn_laser(std::string sn, CLivoxLaser* p_livox_laser)
+{
+	//雷达的sn码一般为15个字符加一个\0
+	if ( sn.size() != kBroadcastCodeSize-1 )
+	{
+		return Error_manager(Error_code::LIVOX_DRIVER_SN_ERROR, Error_level::MINOR_ERROR,
+							 " Livox_insert_sn_laser error , sn size != 15 ");
+	}
+	//填充雷达设备广播码和雷达实例的映射表.如果重复,直接报错,决不能重复创建雷达.
+	if ( m_sn_laser_map.find(sn) == m_sn_laser_map.end() )
+	{
+		//没找到就新增一个sn--laser映射
+		m_sn_laser_map[sn] = p_livox_laser;
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::LIVOX_DRIVER_SN_REPEAT, Error_level::MINOR_ERROR,
+							 " Livox_insert_sn_laser error , sn repeat  ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//判断 Livox_driver 模块是否正常, 只有正常之后,才能启动或者停止扫描.
+bool Livox_driver::is_ready()
+{
+	return  m_init_flag;
+}
+
+
+//判断是否自动创建雷达,默认为false
+bool Livox_driver::is_aoto_create_livox()
+{
+	return m_aoto_create_livox_flag;
+}
+
+//雷达驱动层开始取样.
+Error_manager Livox_driver::Livox_driver_start_sample(uint8_t handle)
+{
+	if ( is_ready() )
+	{
+		LidarStartSampling(handle, livox_start_sample_callback, NULL);
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::LIVOX_DRIVER_NOT_READY, Error_level::MINOR_ERROR,
+							 " Livox_driver is_ready error ");
+	}
+	return Error_code::SUCCESS;
+}
+//雷达驱动层停止取样.
+Error_manager Livox_driver::Livox_driver_stop_sample(uint8_t handle)
+{
+	if ( is_ready() )
+	{
+		LidarStopSampling(handle, livox_stop_sample_callback, NULL);
+		return Error_code::SUCCESS;
+	}
+	else
+	{
+		return Error_manager(Error_code::LIVOX_DRIVER_NOT_READY, Error_level::MINOR_ERROR,
+							 " Livox_driver is_ready error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+
+
+
+
+
+
+
+//设备广播的回调函数,在 Livox_driver_init 时使用 SetBroadcastCallback() 进行配置.
+//局域网内的所有livox雷达在收到广播之后,都会调用本函数.
+//本函数由livox后台线程来执行. 每个雷达调用一次.
+void Livox_driver::livox_device_broadcast_callback(const BroadcastDeviceInfo *p_info)
+{
+	LOG(INFO) << " ---livox_device_broadcast_callback start--- " ;
+	if (p_info == NULL) {
+		return;
+	}
+
+	//检查设备类型,后续最好确定具体的型号.
+	if ( p_info->dev_type != kDeviceTypeHub )
+	{
+
+		//自动创建雷达实例,就是不从外部参数创建,在驱动层链接设备的时候,按照能够连接上的设备自动创建雷达实例.默认为false
+		if ( Livox_driver::get_instance_references().is_aoto_create_livox() )
+		{
+			//创建雷达啊实例.
+			//为 laser_manager 的 m_laser_vector 分配内存.
+			//以后再写.
+			;
+		}
+		else//默认从外部导入参数来创建雷达. 之前在CLivoxLaser初始化是,就已经填充了 m_sn_laser_map
+		{
+			//按照输入参数来创建雷达.
+			//校验sn码,将雷达与设备链接起来,注意了,要先校验,再连接
+			std::string t_sn = p_info->broadcast_code;
+			std::map<std::string, CLivoxLaser*> & t_sn_laser_map = Livox_driver::get_instance_references().m_sn_laser_map;
+			if ( t_sn_laser_map.find(t_sn) != t_sn_laser_map.end() )
+			{
+				//找到sn了,就连接laser和设备.
+				CLivoxLaser* tp_livox_laser = t_sn_laser_map[t_sn];
+				uint8_t t_handle = 0;
+				//连接雷达设备,连接成功之后,后台进行会分配一个新的handle,默认0到31
+				bool result = AddLidarToConnect(p_info->broadcast_code, &t_handle);
+
+				if (result == kStatusSuccess)
+				{
+					//SetDataCallback 函数会为每个雷达设置数据回调函数,
+					//注意了,livox的后台线程不会立刻调用livox_data_callback,
+					//只有在雷达使用 LidarStartSampling 启动扫描后,当有数据返回时,才会调用livox_data_callback 传回雷达数据.
+					SetDataCallback(t_handle, livox_data_callback, NULL);
+
+					//此时只是单方面连接,状态仍然是 K_DEVICE_STATE_DISCONNECT
+					//需要在 livox_device_change_callback 雷达设备响应连接之后,并确认连接的结果,才会修改雷达连接状态.
+					tp_livox_laser->set_handle(t_handle);
+					tp_livox_laser->set_device_state( CLivoxLaser::K_DEVICE_STATE_DISCONNECT);
+					//填充雷达句柄和雷达实例的映射表. handle是livox后台线程分配的唯一码.一般不会出错.
+					Livox_driver::get_instance_references().m_handle_laser_map[t_handle] = tp_livox_laser;
+				}
+				else
+				{
+					//连接失败.    (一般雷达设备有应答,就会连接成功.)
+					tp_livox_laser->set_handle(-1);
+					tp_livox_laser->set_device_state( CLivoxLaser::K_DEVICE_STATE_FAULT);
+				}
+			}
+		}
+	}
+}
+
+//雷达设备状态改变的回调函数,在 Livox_driver_init 时使用 SetDeviceStateUpdateCallback() 来进行配置
+//雷达设备建立连接或者断开连接,都会调用 livox_device_change_callback 来通知我们雷达状态的改变.
+//本函数由livox后台线程来执行. 每个雷达可能调用多次.
+void Livox_driver::livox_device_change_callback(const DeviceInfo *p_info, DeviceEvent type)
+{
+	LOG(INFO) << " ---livox_device_change_callback start--- " ;
+
+	if (p_info == NULL) {
+		return;
+	}
+
+	uint8_t t_handle = p_info->handle;
+
+	//检查handle的有效性
+	std::map<uint8_t, CLivoxLaser*> & t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
+	if ( t_handle_laser_map.find(t_handle) == t_handle_laser_map.end() )
+	{
+		return;
+	}
+	else
+	{
+		CLivoxLaser* tp_livox_laser = t_handle_laser_map[t_handle];
+		//正常情况下,每个雷达连接都会调用两次,分别为 kEventConnect 和 kEventStateChange
+		switch ( type )
+		{
+			case kEventConnect:
+				//连接成功,    则从断连切换到连接
+				if ( tp_livox_laser->get_device_state() == CLivoxLaser::K_DEVICE_STATE_DISCONNECT )
+				{
+					tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
+					tp_livox_laser->set_device_info(*p_info);
+					//LidarGetExtrinsicParameter 函数会为每个雷达设置 获取欧拉角的回调函数,
+					//获取欧拉角, 并转化为变换矩阵 mp_laser_matrix
+					LidarGetExtrinsicParameter(t_handle, livox_get_extrinsic_parameter_callback,NULL);
+				}
+				break;
+			case kEventDisconnect:
+				//断开连接,		如果不是故障,则切换到断连
+				if ( tp_livox_laser->get_device_state() != CLivoxLaser::K_DEVICE_STATE_FAULT )
+				{
+					tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_DISCONNECT);
+					tp_livox_laser->set_device_info(*p_info);
+				}
+				break;
+			case kEventStateChange:
+				//单纯的刷新雷达设备信息
+				tp_livox_laser->set_device_info(*p_info);
+				break;
+			default:
+
+				break;
+		}
+	}
+
+	//注意了:此时并不启动雷达扫描.
+	//只有当接受到扫描任务之后,由  CLivoxLaser::start_scan() 来调用 LidarStartSampling
+}
+
+//获取欧拉角(笛卡尔坐标)的回调函数。在 livox_device_change_callback 连接成功之后使用 LidarGetExtrinsicParameter 来设置回调函数。
+//雷达接受到查询指令后,会调用 livox_get_extrinsic_parameter_callback 来返回 LidarGetExtrinsicParameterResponse 的指针. 里面就有 欧拉角(笛卡尔坐标)
+//本函数由livox后台线程来执行. 每个雷达只需要调用一次.
+void Livox_driver::livox_get_extrinsic_parameter_callback(uint8_t status,
+														  uint8_t handle,
+														  LidarGetExtrinsicParameterResponse *response,
+														  void *client_data)
+{
+	LOG(INFO) << " ---livox_get_extrinsic_parameter_callback start--- " ;
+
+	if ( status == kStatusSuccess )
+	{
+		//	校验handle的有效性
+		std::map<uint8_t, CLivoxLaser*> & t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
+		if ( t_handle_laser_map.find(handle) == t_handle_laser_map.end() )
+		{
+			return;
+		}
+		else
+		{
+			CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];
+
+			double t_pitch = response->pitch;
+			double t_roll = response->roll;
+			double t_yaw = response->yaw;
+			double t_x = response->x;
+			double t_y = response->y;
+			double t_z = response->z;
+
+			//欧拉角转化为3*4矩阵
+			t_roll = t_roll * M_PI / 180.0;
+			t_pitch = t_pitch * M_PI / 180.0;
+			t_yaw = t_yaw * M_PI / 180.0;
+			double rotate[12] = {
+			std::cos(t_pitch) * std::cos(t_yaw),
+			std::sin(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) - std::cos(t_roll) * std::sin(t_yaw),
+			std::cos(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) + std::sin(t_roll) * std::sin(t_yaw),
+			t_x,
+			std::cos(t_pitch) * std::sin(t_yaw),
+			std::sin(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) + std::cos(t_roll) * std::cos(t_yaw),
+			std::cos(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) - std::sin(t_roll) * std::cos(t_yaw),
+			t_y,
+			-std::sin(t_pitch),
+			std::sin(t_roll) * std::cos(t_pitch),
+			std::cos(t_roll) * std::cos(t_pitch),
+			t_z
+			};
+			//设置变换矩阵
+			tp_livox_laser->set_laser_matrix(rotate, 12);
+		}
+	}
+
+}
+
+//雷达设备数据返回 的回调函数. 在 livox_device_broadcast_callback 连接成功之后使用 SetDataCallback  来设置回调函数。
+//雷达在调用 LidarStartSampling 开始扫描之后,就一直调用 livox_data_callback 来返回数据.
+//雷达在调用 LidarStopSampling 停止扫描之后,就停止扫描.本函数也会停止调用.
+//本函数由livox后台线程来执行. 每帧数据都会调用一次. data里面有多个三维点.
+void Livox_driver::livox_data_callback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser)
+{
+	//livox_data_callback函数会高频率的调用,为了提高2效率,这里就不验证handle的有效性了,这就需要保证前面的正确操作
+	std::map<uint8_t, CLivoxLaser *> &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
+	CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];
+
+	if (data && tp_livox_laser)
+	{
+		//判断是否采集完成
+		if (tp_livox_laser->is_scan_complete())
+		{
+			//按照指定的帧数来采集,到点就停止.
+			tp_livox_laser->stop_scan();
+			//注注注注注意了:stop_scan会调用 LidarStopSampling 通知livox后台线程停止扫描.
+			// 但是这个是有延迟的.会导致数据回调函数 livox_data_callback 仍然会调用3~5次.
+			// 因此这里会反复调用 stop_scan
+			return;
+		}
+		//data实际就是一个数据指针,里面包含data_num个三维点. data_num默认为100个
+		LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
+
+		//把雷达数据填充到livox数据缓存,此时里面是没有解析的原始数据.  data_num默认为100个
+		Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
+		tp_livox_laser->push_livox_data(data_bin);
+
+		tp_livox_laser->add_livox_frame(1);
+
+
+	}
+}
+
+
+//雷达设备启动扫描的回调函数, 在 CLivoxLaser::start_scan() 需要启动扫描的时候使用 LidarStartSampling 来设置回调函数。
+//调用 LidarStartSampling 之后,livox后台进程就直接开始扫描了. 之后就会一直调用 livox_data_callback 来返回数据
+void Livox_driver::livox_start_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data)
+{
+	LOG(INFO) << " ---livox_start_sample_callback start--- " ;
+
+	//	校验handle的有效性
+	std::map<uint8_t, CLivoxLaser *> &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
+	if (t_handle_laser_map.find(handle) == t_handle_laser_map.end())
+	{
+		return;
+	}
+	else
+	{
+		CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];
+
+		if (status == kStatusSuccess)
+		{
+			if (response != 0)
+			{
+				tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
+			}
+			else
+			{
+				//返回成功并且应答没有报错,这里将雷达设备状态改为扫描中.
+				tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_SAMPLING);
+			}
+		}
+		else if (status == kStatusTimeout)
+		{
+			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
+		}
+		else
+		{
+			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_FAULT);
+		}
+	}
+}
+
+//雷达设备启动扫描的回调函数, 在 CLivoxLaser::stop_scan() 或者其他位置 需要停止的时候使用 LidarStopSampling 来设置回调函数。
+//调用 LidarStopSampling 之后,livox后台进程就直接停止扫描了. 也会停止调用 livox_data_callback
+void Livox_driver::livox_stop_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data)
+{
+	LOG(INFO) << " ---livox_stop_sample_callback start--- " ;
+
+//	校验handle的有效性
+	std::map<uint8_t, CLivoxLaser *> &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
+	if (t_handle_laser_map.find(handle) == t_handle_laser_map.end())
+	{
+		return;
+	}
+	else
+	{
+		CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];
+
+		if (status == kStatusSuccess || status == kStatusTimeout)
+		{
+			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
+		}
+		else
+		{
+			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_FAULT);
+		}
+	}
+}
+
+
+
+
+
+
+
+
+
+
+

laser/livox_driver.h

@@ -0,0 +1,114 @@
+
+
+/*
+ * livox_driver 大疆雷达的底层驱动
+ *
+ * 本模块作为单例模式,全局使用.
+ * 负责通过调用livox sdk,与livox的后台进行进行对接. 直接控制大疆的雷达设备.
+ *
+ *
+ * 注:sn码就是 broadcast_code 广播码, 设备识别码,在链接设备的时候使用.
+ *
+ *
+ */
+
+#ifndef __LIVOX_DRIVER_H
+#define __LIVOX_DRIVER_H
+
+#include "Laser.h"
+#include "LivoxLaser.h"
+#include "livox_def.h"
+#include "livox_sdk.h"
+#include <map>
+#include "../tool/singleton.h"
+#include "../error_code/error_code.h"
+
+
+//livox_driver 大疆雷达的底层驱动
+class Livox_driver :public Singleton<Livox_driver>
+{
+	// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<Livox_driver>;
+private:
+	// 父类的构造函数必须保护，子类的构造函数必须私有。
+	Livox_driver();
+public://API functions
+	Livox_driver(const Livox_driver&)=delete;
+	Livox_driver& operator =(const Livox_driver&)= delete;
+	~Livox_driver();
+
+	//livox雷达驱动初始化.
+	Error_manager Livox_driver_init(bool aoto_create_livox_flag = false);
+	//livox雷达驱动反初始化.
+	Error_manager Livox_driver_uninit();
+
+	//插入广播码和雷达实例的映射表
+	Error_manager Livox_insert_sn_laser(std::string sn, CLivoxLaser* p_livox_laser);
+
+	//判断 Livox_driver 模块是否正常, 只有正常之后,才能启动或者停止扫描.
+	bool is_ready();
+	//判断是否自动创建雷达,默认为false
+	bool is_aoto_create_livox();
+
+	//雷达驱动层开始取样.
+	Error_manager Livox_driver_start_sample(uint8_t handle);
+	//雷达驱动层停止取样.
+	Error_manager Livox_driver_stop_sample(uint8_t handle);
+
+
+	//设备广播的回调函数,在 Livox_driver_init 时使用 SetBroadcastCallback() 进行配置.
+	//局域网内的所有livox雷达在收到广播之后,都会调用本函数.
+	//本函数由livox后台线程来执行. 每个雷达调用一次.
+	static void livox_device_broadcast_callback(const BroadcastDeviceInfo *info);
+
+	//雷达设备状态改变的回调函数,在 Livox_driver_init 时使用 SetDeviceStateUpdateCallback() 来进行配置
+	//雷达设备建立连接或者断开连接,都会调用 livox_device_change_callback 来通知我们雷达状态的改变.
+	//本函数由livox后台线程来执行. 每个雷达可能调用多次.
+	static void livox_device_change_callback(const DeviceInfo *info, DeviceEvent type);
+
+	//获取欧拉角(笛卡尔坐标)的回调函数。在 livox_device_change_callback 连接成功之后使用 LidarGetExtrinsicParameter 来设置回调函数。
+	//雷达接受到查询指令后,会调用 livox_get_extrinsic_parameter_callback 来返回 LidarGetExtrinsicParameterResponse 的指针. 里面就有 欧拉角(笛卡尔坐标)
+	//本函数由livox后台线程来执行. 每个雷达只需要调用一次.
+	static void livox_get_extrinsic_parameter_callback(uint8_t status,
+													   uint8_t handle,
+													   LidarGetExtrinsicParameterResponse *response,
+													   void *client_data);
+
+	//雷达设备数据返回 的回调函数. 在 livox_device_broadcast_callback 连接成功之后使用 SetDataCallback  来设置回调函数。
+	//雷达在调用 LidarStartSampling 开始扫描之后,就一直调用 livox_data_callback 来返回数据.
+	//雷达在调用 LidarStopSampling 停止扫描之后,就停止扫描.本函数也会停止调用.
+	//本函数由livox后台线程来执行. 每帧数据都会调用一次. data里面有多个三维点.
+	static void livox_data_callback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser);
+
+	//雷达设备启动扫描的回调函数, 在 CLivoxLaser::start_scan() 需要启动扫描的时候使用 LidarStartSampling 来设置回调函数。
+	//调用 LidarStartSampling 之后,livox后台进程就直接开始扫描了. 之后就会一直调用 livox_data_callback 来返回数据
+	static void livox_start_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data);
+
+	//雷达设备启动扫描的回调函数, 在 CLivoxLaser::stop_scan() 或者其他位置 需要停止的时候使用 LidarStopSampling 来设置回调函数。
+	//调用 LidarStopSampling 之后,livox后台进程就直接停止扫描了. 也会停止调用 livox_data_callback
+	static void livox_stop_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data);
+
+
+public://get or set member variable
+
+
+protected://member variable
+
+	bool 								m_init_flag;				//是否初始化的标记位.
+	bool 								m_aoto_create_livox_flag;	//自动创建雷达
+//	注:livox大疆雷达允许先初始化Livox_driver,然后在局域网连接所有的livox设备之后,再根据连接数量去创建雷达实例对象
+
+	std::map<std::string, CLivoxLaser*> m_sn_laser_map;				//广播码和雷达实例的映射表. CLivoxLaser*内存由laser_manager管理
+	std::map<uint8_t, CLivoxLaser*> 	m_handle_laser_map;		//句柄和雷达实例的映射表. CLivoxLaser*内存由laser_manager管理
+
+
+
+private:
+
+};
+
+
+
+
+
+#endif //__LIVOX_DRIVER_H

locate/cnn3d_segmentation.cpp

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

locate/cnn3d_segmentation.h

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

locate/locate_manager.cpp

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

locate/locate_manager.h

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

locate/locate_manager_task.cpp

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

locate/locate_manager_task.h

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

locate/locate_parameter.proto

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

locate/pointSIFT_API.h

@@ -0,0 +1,52 @@
+#pragma once
+#include <string>
+#include <mutex>
+#ifndef LIB_SIFT_API
+#ifdef LIB_EXPORTS
+#if defined(_MSC_VER)
+#define LIB_SIFT_API __declspec(dllexport)
+#else
+#define LIB_SIFT_API __attribute__((visibility("default")))
+#endif
+#else
+#if defined(_MSC_VER)
+#define LIB_SIFT_API
+#else
+#define LIB_SIFT_API
+#endif
+#endif
+#endif
+
+/*
+ * PointSift 点云分割类
+ * 加载PointSift网络,分割点云
+ */
+
+class LIB_SIFT_API PointSifter
+{
+public:
+    //初始化PointSift
+    //point_num:pointsift输入点数
+    //cls_num:PointSift输出类别数
+	PointSifter(int point_num, int cls_num);
+	~PointSifter();
+	//加载网络参数
+	//meta:tensorflow网络结构定义文件
+	//cpkt:tensorflow网络权重
+	bool Load(std::string meta, std::string cpkt);
+	//预测
+	//data:输入数据,大小为  输入点数*3
+	//output:输出数据,大小为  输入点数*类别数
+	bool Predict(float* data, float* output);
+	//错误原因
+	std::string LastError();
+private:
+	PointSifter();
+protected:
+	std::mutex		m_mutex;
+	std::string		m_error;
+	bool			m_bInit;
+	int				m_point_num;		//点云的数量,默认8192
+	int				m_cls_num;			//拆分点云的类别数,默认2个(只有车轮和车身,  通过box限定范围,可以直接剔除地面和杂物)
+	void*			m_sess;
+};

locate/point_sift_segmentation.cpp

@@ -0,0 +1,603 @@
+#include "point_sift_segmentation.h"
+#include <glog/logging.h>
+#include <fstream>
+
+#include <algorithm>
+#include <pcl/filters//voxel_grid.h>
+#include <pcl/filters/passthrough.h>
+
+#include <time.h>
+#include <sys/time.h>
+#include <chrono>
+using namespace std;
+using namespace chrono;
+
+void save_rgb_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZRGB>& cloud)
+{
+	std::ofstream os;
+	os.open(txt, std::ios::out);
+	for (int i = 0; i < cloud.points.size(); i++)
+	{
+		pcl::PointXYZRGB point = cloud.points[i];
+		char buf[255];
+		memset(buf, 0, 255);
+		sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
+		os.write(buf, strlen(buf));
+	}
+	os.close();
+}
+
+void save_rgb_cloud_txt(std::string txt, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
+{
+	std::ofstream os;
+	os.open(txt, std::ios::out);
+	for (int j = 0; j < cloud_vector.size(); ++j)
+	{
+		for (int i = 0; i < cloud_vector[j]->points.size(); i++)
+		{
+			pcl::PointXYZRGB point = cloud_vector[j]->points[i];
+			char buf[255];
+			memset(buf, 0, 255);
+			sprintf(buf, "%f %f %f %d %d %d\n", point.x, point.y, point.z, point.r, point.g, point.b);
+			os.write(buf, strlen(buf));
+		}
+	}
+	os.close();
+}
+
+
+
+Point_sift_segmentation::Point_sift_segmentation(int point_size, int cls, float freq, pcl::PointXYZ minp, pcl::PointXYZ maxp)
+:PointSifter(point_size, cls)
+{
+	m_point_num = point_size;
+	m_cls_num = cls;
+	m_freq = freq;
+	m_minp = minp;
+	m_maxp = maxp;
+}
+Point_sift_segmentation::Point_sift_segmentation(int point_size,int cls,float freq, Cloud_box cloud_box)
+:PointSifter(point_size, cls)
+{
+	m_point_num = point_size;
+	m_cls_num = cls;
+	m_freq = freq;
+	m_cloud_box_limit = cloud_box;
+}
+
+Point_sift_segmentation::~Point_sift_segmentation()
+{
+}
+
+
+
+
+
+Error_manager Point_sift_segmentation::init(std::string graph, std::string cpkt)
+{
+	if (!Load(graph, cpkt))
+	{
+		std::string error_string="pointSIFT Init ERROR:";
+		error_string+=LastError();
+		return Error_manager(LOCATER_SIFT_INIT_FAILED,MINOR_ERROR,error_string);
+	}
+
+	//创建空数据,第一次初始化后空跑
+	float* cloud_data = (float*)malloc(m_point_num * 3 * sizeof(float));
+	float* output = (float*)malloc(m_point_num * m_cls_num * sizeof(float));
+
+	auto start   = system_clock::now();
+
+	if (false == Predict(cloud_data, output))
+	{
+		free(cloud_data);
+		free(output);
+		std::string error_string="pointSIFT int first predict ERROR:";
+		error_string+=LastError();
+		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,error_string);
+	}
+	else
+	{
+		free(cloud_data);
+		free(output);
+
+	}
+	auto end   = system_clock::now();
+	auto duration = duration_cast<microseconds>(end - start);
+	cout <<  "花费了"
+		 << double(duration.count()) * microseconds::period::num / microseconds::period::den   << "秒" << endl;
+
+
+	return SUCCESS;
+}
+
+
+
+//分割函数,输入点云,输出各类的点云,并保存中间文件方便查看识别结果.
+//cloud:输入点云
+//clouds:输出分类好的多个点云(附带颜色,红色为杂物,绿色为车,白色为地面)(此时里面没有内存)
+Error_manager Point_sift_segmentation::segmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in,
+													std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,
+													bool save_flag, std::string save_dir)
+{
+	if(p_cloud_in.get()==NULL)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud_in->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+
+	Error_manager t_error;
+
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
+	//第一步
+	// 使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
+	t_error = filter_cloud_with_voxel_grid(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
+
+	//第二步
+	// 使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
+	t_error = filter_cloud_with_box(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
+
+	//第三步 /
+	// /通过输入点云,更新区域范围, 范围不一定是box的边界,可能比box要小.
+	t_error = update_region(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
+
+	//第四步
+	// 抽稀点云,	//自己写的算法, 重新生成新的点云.
+	t_error = filter_cloud_with_my_grid(p_cloud_in);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	std::cout << "p_cloud_in = "<<p_cloud_in->size() << std::endl;
+
+	//第五步
+	pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_select(new pcl::PointCloud<pcl::PointXYZ>);
+	//从点云中选出固定数量的随机点, 默认8192个点
+	t_error = filter_cloud_with_select(p_cloud_in, p_cloud_select);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		return t_error;
+	}
+	std::cout << "p_cloud_select = "<<p_cloud_select->size() << std::endl;
+
+	//第六步
+	//把pcl点云转化为float, 因为TensorFlow算法只能识别float
+	//提前分配内存, 后续记得回收....
+	float* p_data_point = (float*)malloc(m_point_num * 3 * sizeof(float));				//8192*3 float, 8192个三维点
+	float* p_data_type = (float*)malloc(m_point_num*m_cls_num*sizeof(float));		//8192*2 float, 8192个类别百分比(车轮和车身2类)
+	memset(p_data_point, 0, m_point_num * 3 * sizeof(float));
+	memset(p_data_type, 0, m_point_num*m_cls_num * sizeof(float));
+	//将点云数据转换到float*
+	t_error = translate_cloud_to_float(p_cloud_select, p_data_point);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		free(p_data_point);
+		free(p_data_type);
+		return t_error;
+	}
+
+	//第七步
+	//tensonflow预测点云, 计算出每个三维点的类别百分比,
+	auto start = system_clock::now();
+	if (!Predict(p_data_point, p_data_type))
+	{
+		free(p_data_point);
+		free(p_data_type);
+		return Error_manager(LOCATER_SIFT_PREDICT_FAILED,MINOR_ERROR,"pointSIFT predict ERROR");
+	}
+	auto end   = system_clock::now();
+	auto duration = duration_cast<microseconds>(end - start);
+	cout <<  "花费了"
+		 << double(duration.count()) * microseconds::period::num / microseconds::period::den   << "秒" << endl;
+	cout <<  double(duration.count()) << " "
+		 <<   microseconds::period::num << " " << microseconds::period::den   << "秒" << endl;
+
+	//第八步
+	//恢复点云,并填充到cloud_vector
+	t_error = recovery_float_to_cloud(p_data_type, p_data_point, cloud_vector);
+	if ( t_error != Error_code::SUCCESS )
+	{
+		free(p_data_point);
+		free(p_data_type);
+		return t_error;
+	}
+
+	//第九步
+	//保存点云数据
+	if ( save_flag )
+	{
+		t_error = save_cloud(cloud_vector,save_dir);
+		if ( t_error != Error_code::SUCCESS )
+		{
+			free(p_data_point);
+			free(p_data_type);
+			return t_error;
+		}
+	}
+
+
+	//第十步
+	free(p_data_point);
+	free(p_data_type);
+
+	return Error_code::SUCCESS;
+}
+
+
+
+//使用体素网格, 抽稀点云,	//将点云空间分为小方格,每个小方格只保留一个点.
+Error_manager Point_sift_segmentation::filter_cloud_with_voxel_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+	if(p_cloud.get()==0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	//体素网格 下采样
+	pcl::VoxelGrid<pcl::PointXYZ> vgfilter;
+	vgfilter.setInputCloud(p_cloud);
+	vgfilter.setLeafSize(0.02f, 0.02f, 0.02f);
+	vgfilter.filter(*p_cloud);
+	return Error_code::SUCCESS;
+}
+//使用box限定点云范围,只保留box范围内部的点, 剔除外部的点云
+Error_manager Point_sift_segmentation::filter_cloud_with_box(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+	if(p_cloud.get()==0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	//限制 x y
+	pcl::PassThrough<pcl::PointXYZ> passX;
+	pcl::PassThrough<pcl::PointXYZ> passY;
+	pcl::PassThrough<pcl::PointXYZ> passZ;
+	passX.setInputCloud(p_cloud);
+	passX.setFilterFieldName("x");
+	passX.setFilterLimits(m_cloud_box_limit.x_min, m_cloud_box_limit.x_max);
+	passX.filter(*p_cloud);
+
+	passY.setInputCloud(p_cloud);
+	passY.setFilterFieldName("y");
+	passY.setFilterLimits(m_cloud_box_limit.y_min, m_cloud_box_limit.y_max);
+	passY.filter(*p_cloud);
+
+	passZ.setInputCloud(p_cloud);
+	passZ.setFilterFieldName("z");
+	passZ.setFilterLimits(m_cloud_box_limit.z_min, m_cloud_box_limit.z_max);
+	passZ.filter(*p_cloud);
+	return Error_code::SUCCESS;
+}
+//通过输入点云,更新边界区域范围
+Error_manager Point_sift_segmentation::update_region(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+	pcl::PointXYZ min_point,max_point;
+	pcl::getMinMax3D(*p_cloud,min_point,max_point);
+
+	if(max_point.x<=min_point.x || max_point.y<=min_point.y)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_BOX_PARAMETER_FAILED,MINOR_ERROR,
+							 "Point sift update_region errror :m_maxp.x<=m_minp.x || m_maxp.y<=m_minp.y ");
+	}
+	m_minp = min_point;
+	m_maxp = max_point;
+	return Error_code::SUCCESS;
+}
+//抽稀点云,	//自己写的算法, 重新生成新的点云.
+Error_manager Point_sift_segmentation::filter_cloud_with_my_grid(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud)
+{
+
+	if(p_cloud.get()==NULL)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	Error_manager t_error;
+
+	//可以按照输入点云的数量, 然后动态修改 m_freq , 让其在抽稀之后,点云数量接近8192
+	//以后在写.
+
+	//按照m_freq的分辨率, 将点云拆分为很多网格
+	//网格大小 m_freq 的3次方, 网格总数 grid_number
+	int t_grid_length = int((m_maxp.x - m_minp.x) / m_freq) + 1;
+	int t_grid_width = int((m_maxp.y - m_minp.y) / m_freq) + 1;
+	int t_grid_height = int((m_maxp.z - m_minp.z) / m_freq) + 1;
+	int t_grid_number = t_grid_length * t_grid_width * t_grid_height;
+//	std::cout << " t_grid_length = "<<t_grid_length << " t_grid_width = "<<t_grid_width<< " t_grid_height = "<<t_grid_height<< std::endl;
+//	std::cout << "---------------------------"<< std::endl;
+
+	//创建网格->三维点的map, 每个网格里面只保留一个点,
+	map<int, pcl::PointXYZ> grid_point_map;
+	//遍历输入点云, 将每个点的索引编号写入对应的网格.
+	for (int i = 0; i < p_cloud->size(); ++i)
+	{
+		//找到对应的网格.
+		pcl::PointXYZ point = p_cloud->points[i];
+		int id_x = (point.x - m_minp.x) / m_freq;
+		int id_y = (point.y - m_minp.y) / m_freq;
+		int id_z = (point.z - m_minp.z) / m_freq;
+
+//		std::cout << "------------point:"<< point << std::endl;
+//		std::cout << "------------m_minp:"<< m_minp << std::endl;
+//		std::cout << "------------m_freq:"<< m_freq << std::endl;
+//		std::cout << " id_x = "<<id_x << " id_y = "<<id_y<< " id_y = "<<id_y<< std::endl;
+
+		if (id_x < 0 || id_x >= t_grid_length || id_y < 0 || id_y >= t_grid_width || id_z < 0 || id_z >= t_grid_height)
+		{
+			//无效点.不要.
+			continue;
+		}
+		grid_point_map[id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width] = point;
+		//注:如果有多个点属于同一个网格, 那个后者就会覆盖前者, 最终保留一个点.
+	}
+
+	//检查map
+	if ( grid_point_map.size() >0 )
+	{
+		//清空点云, 然后将map里面的点 重新加入p_cloud
+		p_cloud->clear();
+		for ( auto iter = grid_point_map.begin(); iter != grid_point_map.end(); iter++  )
+		{
+			p_cloud->push_back(iter->second);
+		}
+	}
+	else
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_GRID_ERROR, Error_level::MINOR_ERROR,
+							 " filter_cloud_with_my_grid  error ");
+	}
+
+/*
+	//创建网格的数据, 内存大小为t_grid_number, 储存内容为是否存在点云的标记位.
+	bool* tp_grid = (bool*)malloc(t_grid_number*sizeof(bool));
+	memset(tp_grid, 0, t_grid_number*sizeof(bool));
+	//遍历输入点云, 将每个点映射到输入网格, 每个网格最多保留一个点,多余的删除.
+	for(pcl::PointCloud<pcl::PointXYZ>::iterator iter = p_cloud->begin(); iter !=p_cloud->end(); )
+	{
+		//找到对应的网格.
+		int id_x = (iter->x - m_minp.x) / m_freq;
+		int id_y = (iter->y - m_minp.y) / m_freq;
+		int id_z = (iter->z - m_minp.z) / m_freq;
+		if (id_x < 0 || id_x >= t_grid_length || id_y < 0 || id_y >= t_grid_width || id_z < 0 || id_z >= t_grid_height)
+		{
+			//无效点, 直接删除
+			iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
+		}
+		else if(*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) == true)
+		{
+			//标记位为true,就表示这个网格已经有点了, 那么直接删除后者,保留前者
+			iter = p_cloud->erase(iter);//删除当前节点,自动指向下一个节点
+		}
+		else
+		{
+			*(tp_grid + id_x + id_y*t_grid_length + id_z*t_grid_length*t_grid_width) = true;
+			iter++;
+		}
+	}
+	free(tp_grid);
+	*/
+
+	return Error_code::SUCCESS;
+}
+
+//从点云中选出固定数量的随机点, 默认8192个点
+Error_manager Point_sift_segmentation::filter_cloud_with_select(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_out)
+{
+	if(p_cloud_in.get()==NULL || p_cloud_out.get()==NULL)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud_in->size()<=0)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_EMPTY,MINOR_ERROR,"locate_sift input cloud empty");
+	}
+	Error_manager t_error;
+
+	//从输入点云 p_cloud_in 里面选出 m_point_num 个点, 然后加入 p_cloud_out
+	//输入个数大于8192
+	if (p_cloud_in->size() > m_point_num)
+	{
+		//将点云随机打乱
+		std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
+		//选出输入点云前面8192个点.
+		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + m_point_num);
+	}
+	//输入个数小鱼8192
+	else if (p_cloud_in->size() < m_point_num)
+	{
+		int add = m_point_num - p_cloud_in->size();
+		//注意了, 输入点云不能少于4096, 否则点云分布太少, 会严重影响到TensorFlow的图像识别.
+		if (add > p_cloud_in->size())
+		{
+			return Error_manager(Error_code::LOCATER_SIFT_CLOUD_VERY_LITTLE, Error_level::MINOR_ERROR,
+								" filter_cloud_with_select  p_cloud_in->size() very little ");
+		}
+		//将点云随机打乱
+		std::random_shuffle(p_cloud_in->points.begin(), p_cloud_in->points.end());
+		//选出输入点云8192个点, 不够的在加一次,
+		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.begin() + add);
+		p_cloud_out->points.insert(p_cloud_out->points.begin(), p_cloud_in->points.begin(), p_cloud_in->points.end());
+	}
+	//输入个数等于8192
+	else
+	{
+		//此时就不需要打乱了, 直接全部复制.
+		p_cloud_out->points.assign(p_cloud_in->points.begin(), p_cloud_in->points.end());
+	}
+
+	if (p_cloud_out->points.size() != m_point_num)
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_SELECT_ERROR, Error_level::MINOR_ERROR,
+							" Point_sift_segmentation::filter_cloud_with_select  error ");
+	}
+	return Error_code::SUCCESS;
+}
+
+//将点云数据转换到float*
+Error_manager Point_sift_segmentation::translate_cloud_to_float(pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud_in, float* p_data_out)
+{
+	if(p_cloud_in.get()==NULL)
+	{
+		return Error_manager(LOCATER_SIFT_INPUT_CLOUD_UNINIT,MINOR_ERROR,"sift input cloud uninit");
+	}
+	if(p_cloud_in->size() != m_point_num)
+	{
+		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+							" Point_sift_segmentation::translate_cloud_to_float p_cloud_in->size() error ");
+	}
+
+	if ( p_data_out == NULL )
+	{
+	    return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+	    					" translate_cloud_to_float  p_data_out POINTER_IS_NULL ");
+	}
+
+	for (int i = 0; i < m_point_num; ++i)
+	{
+		pcl::PointXYZ point = p_cloud_in->points[i];
+		*(p_data_out + i * 3) = point.x;
+		*(p_data_out + i * 3+1) = point.y;
+		*(p_data_out + i * 3+2) = point.z ;
+		//注:三维点云的数据 不用做缩放和平移,
+		// 这个在雷达扫描时就进行了标定和转化, 雷达传输过来的坐标就已经是公共坐标系了, (单位米)
+	}
+	return Error_code::SUCCESS;
+}
+
+//恢复点云, 将float*转换成点云
+//p_data_type:输入点云对应的类别,大小为  点数*类别
+//p_data_point:输入点云数据(xyz)
+//cloud_vector::输出带颜色的点云vector
+Error_manager Point_sift_segmentation::recovery_float_to_cloud(float* p_data_type, float* p_data_point, std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector)
+{
+	if ( p_data_type == NULL || p_data_point == NULL )
+	{
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 " recovery_float_to_cloud  p_data_type or  p_data_point  POINTER_IS_NULL ");
+	}
+
+	//为cloud_vector 添加m_cls_num个点云, 提前分配内存
+	for (int k = 0; k < m_cls_num; ++k)
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
+		cloud_vector.push_back(cloud_rgb);
+	}
+
+	//遍历data数据,
+	for (int i = 0; i < m_point_num; ++i)
+	{
+		pcl::PointXYZRGB t_point;
+		t_point.x = *(p_data_point + i * 3);
+		t_point.y = *(p_data_point + i * 3 + 1);
+		t_point.z = *(p_data_point + i * 3 + 2);
+		if (t_point.x > m_maxp.x || t_point.x<m_minp.x
+			|| t_point.y>m_maxp.y || t_point.y<m_minp.y
+			|| t_point.z>m_maxp.z || t_point.z < m_minp.z)
+		{
+			continue;
+		}
+
+		//当前点 的类别百分比的指针
+		float* tp_type_percent = p_data_type + i*m_cls_num;
+		//当前点 的类别, 初始化为0
+		int t_cls = 0;
+		//当前点 的类别, 百分比的最大值
+		float t_type_percent_max = tp_type_percent[0];
+		//循环比较, 找出百分比最大的类别
+		for (int j = 1; j < m_cls_num; j++) //前面已经把第0个用来初始化了, 后续从第下一个开始比较
+		{
+			if (tp_type_percent[j] > t_type_percent_max)
+			{
+				t_type_percent_max = tp_type_percent[j];
+				t_cls = j;
+			}
+		}
+
+		//根据点的类别, 添加颜色, 并且分别加入到各自的点云里面
+		switch ( t_cls )
+		{
+		    case 0: //第0类, 绿色, 轮胎
+				t_point.r = 0;
+				t_point.g = 255;
+				t_point.b = 0;
+		        break;
+		    case 1://第1类, 白色, 车身
+				t_point.r = 255;
+				t_point.g = 255;
+				t_point.b = 255;
+				break;
+//			case 2:
+//				;
+//				break;
+			default:
+
+		        break;
+		}
+		cloud_vector[t_cls]->push_back(t_point);
+	}
+
+	//校验点云的数量, 要求size>0
+	if(cloud_vector[0]->size() <=0)
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_WHEEL_POINT, Error_level::MINOR_ERROR,
+							" recovery_float_to_cloud NO_WHEEL_POINT ");
+	}
+	if(cloud_vector[1]->size() <=0)
+	{
+		return Error_manager(Error_code::LOCATER_SIFT_PREDICT_NO_CAR_POINT, Error_level::MINOR_ERROR,
+							 " recovery_float_to_cloud NO_CAR_POINT ");
+	}
+
+
+	return Error_code::SUCCESS;
+}
+
+//保存点云数据
+Error_manager Point_sift_segmentation::save_cloud(std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>& cloud_vector,std::string save_dir)
+{
+	if(cloud_vector.size()>0) {
+		std::string sift_in = save_dir + "/SIFT_cls_0.txt";
+		save_rgb_cloud_txt(sift_in, *cloud_vector[0]);
+	}
+	if(cloud_vector.size()>1) {
+		std::string sift_in = save_dir + "/SIFT_cls_1.txt";
+		save_rgb_cloud_txt(sift_in, *cloud_vector[1]);
+	}
+
+	if(cloud_vector.size()>2) {
+		std::string sift_in = save_dir + "/SIFT_cls_2.txt";
+		save_rgb_cloud_txt(sift_in, *cloud_vector[2]);
+	}
+
+	std::string sift_in = save_dir + "/SIFT_TOTAL.txt";
+	save_rgb_cloud_txt(sift_in, cloud_vector);
+
+	return Error_code::SUCCESS;
+}

locate/point_sift_segmentation.h

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

locate/tf_wheel_3Dcnn_api.h

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

task/task_base.cpp

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

task/task_base.h

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

task/task_base.puml

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

task/task_command_manager.cpp

@@ -0,0 +1,85 @@
+
+
+
+#include "task_command_manager.h"
+#include "../laser/Laser.h"
+#include "../laser/laser_manager.h"
+#include "../locate/locate_manager.h"
+
+//对外的接口函数，所有的任务发送方，都必须使用该函数。
+//execute_task在内部解析了Task_Base里面的Task_type，然后转发给具体某个模块的实例对象。
+//input：p_task_base 任务单，基类的指针，指向子类的实例，（多态）
+Error_manager Task_command_manager::execute_task(Task_Base* p_task_base)
+{
+	Error_manager t_error;
+	void * tp_tast_receiver = p_task_base->get_tast_receiver();
+	switch ( p_task_base->get_task_type() )
+	{
+	    case UNKNOW_TASK:
+			t_error.error_manager_reset(Error_code::TASK_TYPE_IS_UNKNOW, Error_level::MINOR_ERROR,
+								" p_task_base->get_task_type() is  UNKNOW_TASK ");
+	        break;
+		case LASER_MANGER_SCAN_TASK:
+			if ( tp_tast_receiver != NULL )
+			{
+				t_error = ((Laser_manager*)tp_tast_receiver)->execute_task(p_task_base);
+			}
+			else
+			{
+				t_error = Laser_manager::get_instance_references().execute_task(p_task_base);
+			}
+			break;
+		case LASER_BASE_SCAN_TASK:
+			if ( tp_tast_receiver != NULL )
+			{
+				t_error = ((Laser_base*)tp_tast_receiver)->execute_task(p_task_base);
+			}
+			else
+			{
+				return Error_manager(Error_code::TASK_NO_RECEIVER, Error_level::MINOR_ERROR,
+									" Task_command_manager::execute_task have not receiver  ");
+	        }
+	        break;
+		case LOCATE_MANGER_TASK:
+			if ( tp_tast_receiver != NULL )
+			{
+				t_error = ((Locate_manager*)tp_tast_receiver)->execute_task(p_task_base);
+			}
+			else
+			{
+				t_error = Locate_manager::get_instance_references().execute_task(p_task_base);
+			}
+			break;
+		case PLC_TASK:
+			;
+			break;
+		case WJ_TASK:
+			;
+			break;
+	    default:
+			t_error.error_manager_reset(Error_code::TASK_TYPE_IS_UNKNOW, Error_level::MINOR_ERROR,
+										" p_task_base->get_task_type() is  UNKNOW_TASK ");
+	        break;
+	}
+
+	return t_error;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

task/task_command_manager.h

@@ -0,0 +1,42 @@
+/*
+ * task_command_manager 是任务单的总管理，单例模式
+ * 负责管理任务单的派送和转发
+ * 所有的任务发送方都只需要调用 task_command_manager.get_instance_references().execute_task(p_task_base)
+ * 然后task_command_manager去找到对应的接受对象，来调用该对象的接口函数。 例如Laser_base::execute_task
+ * 这样发送方和接收方不直接绑定，双方完全独立。
+ * 没有在实例里面保存对方的回调函数或者对象指针
+ *
+ * */
+
+#ifndef TASK_COMAND_MANAGER_H
+#define TASK_COMAND_MANAGER_H
+#include <string>
+#include "../error_code/error_code.h"
+#include "../task/task_base.h"
+#include "../tool/singleton.h"
+
+
+class Task_command_manager:public Singleton<Task_command_manager>
+{
+// 子类必须把父类设定为友元函数，这样父类才能使用子类的私有构造函数。
+	friend class Singleton<Task_command_manager>;
+public:
+// 必须关闭拷贝构造和赋值构造，只能通过 get_instance 函数来进行操作唯一的实例。
+	Task_command_manager(const Task_command_manager&)=delete;
+	Task_command_manager& operator =(const Task_command_manager&)= delete;
+	~Task_command_manager()=default;
+private:
+// 父类的构造函数必须保护，子类的构造函数必须私有。
+	Task_command_manager()=default;
+
+public:
+	//对外的接口函数，所有的任务发送方，都必须使用该函数。
+	//execute_task在内部解析了Task_Base里面的Task_type，然后转发给具体某个模块的实例对象。
+	//input：p_task_base 任务单，基类的指针，指向子类的实例，（多态）
+	Error_manager execute_task(Task_Base* p_task_base);
+};
+
+
+
+#endif //TASK_COMAND_MANAGER_H
+

task/task_command_manager.puml

@@ -0,0 +1,12 @@
+@startuml
+@startuml
+skinparam classAttributeIconSize 0
+
+
+title  task_command_manager 任务单管理类
+
+
+
+
+
+@enduml

tool/ThreadPool.h

@@ -0,0 +1,189 @@
+/*
+ * ThreadPool 线程池,
+ *
+ * */
+
+
+
+//例如
+// ThreadPool thread_pool(4);
+// std::future<int> x =  thread_pool.enqueue( []{return 0;} );
+// std::cout << x.get() << std::endl;
+
+//例如
+/*
+ThreadPool pool(4);
+std::vector< std::future<int> > results;
+
+for(int i = 0; i < 8; ++i) {
+results.emplace_back(
+pool.enqueue([i] {
+std::cout << "hello " << i << std::endl;
+std::this_thread::sleep_for(std::chrono::seconds(1));
+std::cout << "world " << i << std::endl;
+return i*i;
+})
+);
+}
+
+for(auto && result: results)
+std::cout << result.get() << ' ';
+std::cout << std::endl;
+return 0;
+*/
+
+#ifndef THREAD_POOL_H
+#define THREAD_POOL_H
+
+#include <vector>
+#include <queue>
+#include <memory>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <future>
+#include <functional>
+#include <stdexcept>
+
+class ThreadPool {
+public:
+	//构造函数, 会自动初始化 threads_size 数量的线程
+    ThreadPool(size_t threads_size);
+
+	//往线程池添加执行任务, 之后会唤醒一个线程去执行他.
+	//input: F&& f  			函数指针(函数名)
+	//input: Args&&... args		函数的参数, 自定义
+    template<class F, class... Args>
+    auto enqueue(F&& f, Args&&... args) 
+        -> std::future<typename std::result_of<F(Args...)>::type>;
+
+
+    ~ThreadPool();
+
+	//判断线程池是否超载
+    bool thread_is_full_load();
+
+private:
+    // 线程数组
+    std::vector< std::thread > workers;
+	//每个线程的工作状态, true:线程正在执行任务,  false:线程空闲等待
+	std::vector< bool > working_flag_vector;
+    // 任务函数 队列, 里面存入的是任务函数的指针
+    std::queue< std::function<void()> > tasks;
+    
+    // 线程锁和条件变量
+    std::mutex queue_mutex;
+    std::condition_variable condition;
+
+    //终止标志位
+    bool stop;
+};
+
+//构造函数, 会自动初始化 threads_size 数量的线程
+inline ThreadPool::ThreadPool(size_t threads)
+    :   stop(false)
+{
+	//每个线程的工作状态
+	for(size_t i = 0;i<threads;++i)
+	{
+		working_flag_vector.push_back(false);
+	}
+
+	//初始化 threads_size 数量的线程
+    for(size_t i = 0;i<threads;++i)
+        workers.emplace_back(
+            [i,this]   //每个线程的执行的基本函数,
+            {
+                for(;;)
+                {
+                    std::function<void()> task;
+
+					{
+						std::unique_lock<std::mutex> lock(this->queue_mutex);
+						this->working_flag_vector[i] = false;//线程等待
+						this->condition.wait(lock,
+											 [this]    //线程等待的判断函数
+											 { return this->stop || !this->tasks.empty(); });
+						if (this->stop )//&& this->tasks.empty()) //这里修改了, 不需要把任务池都执行完才退出, stop之后就可以退了.
+						{
+							return;//只有在终止标志位true, 那么就退出线程执行函数
+						}
+						this->working_flag_vector[i] = true;//线程工作
+						//从 任务池 里面取出 执行函数
+						task = std::move(this->tasks.front());
+						this->tasks.pop();
+					}
+
+					//运行执行函数
+                    task();
+                }
+            }
+        );
+}
+
+
+//往线程池添加执行任务, 之后会唤醒一个线程去执行他.
+//input: F&& f  			函数指针(函数名)
+//input: Args&&... args		函数的参数, 自定义
+//注注注注注意了:::::  res是enqueue的返回值, 由于线程异步, 使用future, 可以返回未来的一个值,
+// 在子线程执行完成之后, 将结果返回给外部主线程
+// 外部主线程 调用时, 必须使用 std::future<return_type> 格式去接受
+
+
+template<class F, class... Args>
+auto ThreadPool::enqueue(F&& f, Args&&... args) 
+    -> std::future<typename std::result_of<F(Args...)>::type>
+{
+    using return_type = typename std::result_of<F(Args...)>::type;
+
+    auto task = std::make_shared< std::packaged_task<return_type()> >(
+            std::bind(std::forward<F>(f), std::forward<Args>(args)...)
+        );
+
+
+    std::future<return_type> res = task->get_future();
+    {
+        std::unique_lock<std::mutex> lock(queue_mutex);
+
+        // don't allow enqueueing after stopping the pool
+        if(stop)
+            throw std::runtime_error("enqueue on stopped ThreadPool");
+
+        tasks.emplace([task](){ (*task)(); });
+    }
+    condition.notify_one();
+    return res;
+}
+
+// the destructor joins all threads
+inline ThreadPool::~ThreadPool()
+{
+    {
+        std::unique_lock<std::mutex> lock(queue_mutex);
+        stop = true;
+    }
+    condition.notify_all();
+    for(std::thread &worker: workers)
+	{  worker.join();}
+
+
+}
+
+//判断线程池是否超载
+bool ThreadPool::thread_is_full_load()
+{
+	//只要有一个线程wait, 那么就认为没有超载,
+	std::unique_lock<std::mutex> lock(queue_mutex);
+	bool result = true;
+	for(bool t_working_flag: working_flag_vector)
+	{
+		if ( !t_working_flag )
+		{
+			result = false;
+		}
+	}
+	return result;
+}
+
+
+#endif