LidarMeasure/laser/Laser.h

#ifndef __LASER__HH__
#define __LASER__HH__
#include "Point2D.h"
#include "Point3D.h"
#include "LogFiles.h"
#include <glog/logging.h>
#include "../tool/StdCondition.h"
#include "laser_parameter.pb.h"
#include "laser_message.pb.h"
#include "../error_code/error_code.h"
#include "laser_task_command.h"


//雷达消息的类型
//在通信消息的前面一部分字符串，表示这条消息的类型。
//在解析消息的时候，先解析前面的消息类型，来判断这条消息的功用
enum DATA_type
{
	eStart          =0,
	eReady          =1,
	eData           =2,
	eStop           =3,
	eHerror         =4,
	eUnknow         =5,
};

//通信消息的二进制数据，
//这里用字符串，来存储雷达的通信消息的原始数据
//CBinaryData的内容格式：消息类型 + 消息数据
class CBinaryData
{
public:
	CBinaryData();
	CBinaryData(const CBinaryData& data);
	~CBinaryData();
	CBinaryData(const char* buf, int len, DATA_type type= eUnknow);
    CBinaryData& operator=(const CBinaryData& data);
	bool operator==(const char* str);
	const char* operator+(int n);
	CBinaryData& operator+(CBinaryData& data);
	char& operator[](int n);
	char*		Data()const;
	int			Length()const;

protected:
	char*			m_buf;
	int				m_length;
};


#include <queue>  
#include <memory>  
#include <mutex>  
#include <condition_variable>

//通信消息的安全队列，用来存储通信消息的二进制数据的容器
template<typename T>
class threadsafe_queue
{
private:
	mutable std::mutex mut;
	std::queue<T> data_queue;
	std::condition_variable data_cond;
public:
	threadsafe_queue() {}
	threadsafe_queue(threadsafe_queue const& other)
	{
		std::lock_guard<std::mutex> lk(other.mut);
		data_queue = other.data_queue;
	}
	~threadsafe_queue()
	{
		while (!empty())
		{
			try_pop();
		}
	}
	size_t size()
	{
		return data_queue.size();
	}
	void push(T new_value)//��Ӳ���  
	{
		std::lock_guard<std::mutex> lk(mut);
		data_queue.push(new_value);
		data_cond.notify_one();
	}
	void wait_and_pop(T& value)//ֱ����Ԫ�ؿ���ɾ��Ϊֹ  
	{
		std::unique_lock<std::mutex> lk(mut);
		data_cond.wait(lk, [this] {return !data_queue.empty(); });
		value = data_queue.front();
		data_queue.pop();
	}
	std::shared_ptr<T> wait_and_pop()
	{
		std::unique_lock<std::mutex> lk(mut);
		data_cond.wait(lk, [this] {return !data_queue.empty(); });
		std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
		data_queue.pop();
		return res;
	}
	//ֻ���� �� pop
	bool front(T& value)
	{
		std::lock_guard<std::mutex> lk(mut);
		if (data_queue.empty())
			return false;
		value = data_queue.front();
		return true;
	}

	bool try_pop(T& value)//������û�ж���Ԫ��ֱ�ӷ���  
	{
		if (data_queue.empty())
			return false;
		std::lock_guard<std::mutex> lk(mut);
		value = data_queue.front();
		data_queue.pop();
		return true;
	}
	std::shared_ptr<T> try_pop()
	{
		std::lock_guard<std::mutex> lk(mut);
		if (data_queue.empty())
			return std::shared_ptr<T>();
		std::shared_ptr<T> res(std::make_shared<T>(data_queue.front()));
		data_queue.pop();
		return res;
	}
	bool empty() const
	{
		std::lock_guard<std::mutex> lk(mut);
		return data_queue.empty();
	}
	void clear()
	{
		while (!empty()) {
			try_pop();
		}
	}
};


///////////////////////////////////////////////

//雷达的工作状态，
//在start和stop中要切换状态
enum eLaserStatu
{
    eLaser_ready        =0,	        //雷达正常待机，空闲
    eLaser_busy         =1,			//雷达正在工作，正忙
    eLaser_disconnect   =2,	        //雷达断连
    eLaser_error        =3,         //雷达错误
};

//回调函数，当有事件触发是，便会自动调用回调函数，
//typedef从定义，将其简化为PointCallBack，方便后面多次使用
typedef void (*PointCallBack)(CPoint3D ,  void* );

//雷达的基类，不能直接使用，必须子类继承
class Laser_base
{
public:
    //唯一的构造函数，按照设备名称和雷达参数来创建实例。
    //input：id： 雷达设备的id，（唯一索引）
    //input：laser_param：雷达的参数，
    //注：利用protobuf创建stLaserCalibParam类，然后从文件读取参数
	Laser_base(int id,Laser_proto::laser_parameter laser_param);
	virtual ~Laser_base();
	///��������Դ
	virtual bool Connect();
	virtual void Disconnect();

	//对外的接口函数，负责接受并处理任务单，
	//input：p_laser_task 雷达任务单，必须是子类，并且任务正确。
    virtual Error_manager execute_task(Task_Base* p_laser_task);
    //检查雷达状态,是否正常运行
    Error_manager check_error();

	///��ʼ��ֹͣ�ɼ�
	virtual bool Start();
	virtual bool Stop();
	///���õ��Ʊ任���󣨱궨������
	void SetMetrix(double* data);
	///���û�ȡ��ά��ص�����
	void SetPointCallBack(PointCallBack fnc,void* pointer);

public:
	///�������ݴ洢·��
	void			SetSaveDir(std::string strDir,bool bSave=true);
	///��ѯ�״��Ƿ����
    bool			IsReady() { return (GetStatu() == eLaser_ready && m_queue_laser_data.size()==0); }

    virtual eLaserStatu     GetStatu(){return m_statu;}
	int				ID() { return m_id; }
protected:
	////��ȡԭʼ���ݰ�
	virtual bool RecvData(CBinaryData& data) = 0;
	////����ԭʼ���ݰ��ɵ���
	virtual DATA_type Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)=0;
	void thread_recv();
	void thread_toXYZ();
	////���Ʊ任
	virtual CPoint3D transfor(CPoint3D point);
protected:
    static void threadPublish(Laser_base* laser);
    void PublishMsg();
protected:
	std::thread*		m_ThreadRcv;
	std::thread*		m_ThreadPro;
	std::thread*        m_ThreadPub;
	StdCondition		m_bThreadRcvRun;
	StdCondition		m_bThreadProRun;
	int					m_id;
	int                 m_points_count;

	std::mutex          m_scan_lock;

	bool                m_bscan_start;
	eLaserStatu			m_statu;
	bool				m_bSave_file;
    Laser_proto::laser_parameter  m_laser_param;////����

    //���ݴ������
	threadsafe_queue<CBinaryData*>		m_queue_laser_data;     // ���ݶ���
	CBinaryData							m_last_data;			//��һ��������δ�������		
	double*								m_dMatrix;              
	PointCallBack						m_point_callback_fnc;
	void*								m_point_callback_pointer;


    //���ݴ洢
	CLogFile		m_binary_log_tool;										//�洢������
	CLogFile		m_pts_log_tool;										//�洢����
	std::string		m_pts_save_path;
	StdCondition	m_bStart_capture;

    //任务单的指针，实际内存由应用层管理，
    //接受任务后，指向新的任务单
    Laser_task *  mp_laser_task;
};


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