LidarMeasure/laser/Laser.h

/*
 * 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_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;        		//任务单的指针

};


//雷达的注册类，
//雷达模块可能有多个不同的雷达，这些雷达需要制定不同的子类，从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