LaserMeasure_linux/laser/Laser.h

#ifndef __LASER__HH__
#define __LASER__HH__
#include "Point2D.h"
#include "Point3D.h"
#include "LogFiles.h"
#include "src/CalibParam.pb.h"
#include "src/StdCondition.h"



enum DATA_type
{
	eStart=0
	,eReady
	,eData
	,eStop
	,eHerror
	,eUnknow
};
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();
		}
	}
};


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


enum eLaserStatu
{
	eLaser_connect=0,
	eLaser_ready ,	//空闲状态
	eLaser_busy			//扫描中
	, eLaser_disconnect	//断线

};


typedef void (*PointCallBack)(CPoint3D ,  void* );
class CLaser
{
public:
	CLaser(int id,Automatic::stLaserCalibParam laser_param);
	virtual ~CLaser();
	///连接数据源
	virtual bool Connect();
	virtual void Disconnect();
	///开始、停止采集
	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 m_statu == eLaser_ready || m_statu==eLaser_connect; }
    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:
	std::thread*		m_ThreadRcv;
	std::thread*		m_ThreadPro;
	StdCondition		m_bThreadRcvRun;
	StdCondition		m_bThreadProRun;
	int					m_id;
	eLaserStatu			m_statu;
	bool				m_bSave_file;
	Automatic::stLaserCalibParam  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;								

};


class LaserRegistory
{
	typedef CLaser*  (*CreateLaserFunc)(int id, Automatic::stLaserCalibParam laser_param);
public:
	LaserRegistory(std::string name, CreateLaserFunc pFun) {
		AddCreator(name, pFun);
	}
	static CLaser* CreateLaser(std::string name, int id,Automatic::stLaserCalibParam 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 CLaser* Create_##NAME##_Laser(int id, Automatic::stLaserCalibParam param)	\
	{																\
		return new C##NAME##Laser(id,param);							\
	}																\
	LaserRegistory g_##NAME##_Laser(#NAME,Create_##NAME##_Laser);


#endif