20190104,增加laser模块

laser/Laser.cpp

@@ -0,0 +1,431 @@
+#include "Laser.h"
+
+CBinaryData::CBinaryData()
+	:m_buf(0)
+	,m_length(0)
+{
+}
+CBinaryData::CBinaryData(const char* buf, int len, DATA_type type)
+    :m_buf(0),m_length(0)
+{
+	if (len > 0)
+	{
+		m_buf = (char*)malloc(len);
+		memcpy(m_buf, buf, len);
+		m_length = len;
+	}
+}
+CBinaryData::CBinaryData(const CBinaryData& data)
+    :m_buf(0),m_length(0)
+{
+	if (m_buf)
+	{
+		free(m_buf);
+		m_buf = 0;
+		m_length = 0;
+	}
+	if (data.Length() > 0)
+	{
+		m_buf = (char*)malloc(data.Length());
+		memcpy(m_buf, data.Data(), data.Length());
+		m_length = data.Length();
+	}
+}
+CBinaryData::~CBinaryData()
+{
+    if (m_buf && m_length)
+	{
+		free(m_buf);
+		m_length = 0;
+	}
+}
+char* CBinaryData::Data()const {
+	return m_buf;
+}
+int CBinaryData::Length()const {
+	return m_length;
+}
+CBinaryData& CBinaryData::operator=(const CBinaryData& data)
+{
+	if (m_buf)
+	{
+		free(m_buf);
+		m_buf = 0;
+		m_length = 0;
+	}
+	if (data.Length() > 0)
+	{
+		m_buf = (char*)malloc(data.Length());
+		memcpy(m_buf, data.Data(), data.Length());
+		m_length = data.Length();
+	}
+	return *this;
+}
+bool CBinaryData::operator==(const char* str)
+{
+	int length = strlen(str);
+	return  (strncmp((const char*)m_buf, str, length) == 0);
+}
+const char* CBinaryData::operator+(int n)
+{
+	if (n < m_length)
+		return m_buf + n;
+	else
+		return NULL;
+}
+CBinaryData& CBinaryData::operator+(CBinaryData& data)
+{
+	if (data.Length() > 0)
+	{
+		int length = m_length + data.Length();
+		char* buf = (char*)malloc(length);
+		memcpy(buf, m_buf, m_length);
+		memcpy(buf + m_length, data.Data(), data.Length());
+		free(m_buf);
+		m_buf = buf;
+		m_length = length;
+	}
+	return *this;
+}
+char& CBinaryData::operator[](int n)
+{
+	if (n >= 0 && n < m_length)
+		return m_buf[n];
+}
+//////////////
+
+CLaser::CLaser(int id, Automatic::stLaserCalibParam laser_param)
+	:m_ThreadRcv(0)
+	,m_ThreadPro(0)
+	,m_ThreadPub(0)
+	,m_id(id)
+    ,m_points_count(0)
+	,m_statu(eLaser_disconnect)
+	,m_dMatrix(0)
+	, m_point_callback_fnc(0)
+	, m_point_callback_pointer(0)
+	,m_laser_param(laser_param)
+	,m_bSave_file(false)
+	,m_bscan_start(false)
+{
+    mp_laser_task = NULL;
+}
+
+
+CLaser::~CLaser()
+{
+	if (m_dMatrix)
+	{
+		free(m_dMatrix);
+		m_dMatrix = 0;
+	}
+}
+
+bool CLaser::Start()
+{
+    m_bscan_start=true;
+	m_statu = eLaser_busy;
+	m_bStart_capture.Notify(true);
+    m_points_count=0;
+	return true;
+}
+
+bool CLaser::Stop()
+{
+    m_bscan_start=false;
+	m_bStart_capture.Notify(false);
+	m_binary_log_tool.close();
+	m_pts_log_tool.close();
+	m_statu=eLaser_ready;
+
+	//在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
+	if(mp_laser_task !=NULL)
+    {
+	    if(mp_laser_task->get_statu() == TASK_WORKING)
+        {
+            mp_laser_task->set_task_statu(TASK_OVER);
+        }
+    }
+
+	return true;
+}
+
+void CLaser::SetMetrix(double* data)
+{
+	if (m_dMatrix == 0)
+	{
+		m_dMatrix = (double*)malloc(12 * sizeof(double));
+	}
+	memcpy(m_dMatrix, data, 12 * sizeof(double));
+}
+void CLaser::SetPointCallBack(PointCallBack fnc, void* pointer)
+{
+	m_point_callback_fnc = fnc;
+	m_point_callback_pointer = pointer;
+}
+
+bool CLaser::Connect()
+{
+	m_bThreadRcvRun.Notify(true);
+	if(m_ThreadRcv==0)
+		m_ThreadRcv = new std::thread(&CLaser::thread_recv, this);
+	m_ThreadRcv->detach();
+
+	m_bThreadProRun.Notify(true);
+	if(m_ThreadPro==0)
+		m_ThreadPro = new std::thread(&CLaser::thread_toXYZ, this);
+	m_ThreadPro->detach();
+
+    if(m_ThreadPub==0)
+        m_ThreadPub = new std::thread(threadPublish, this);
+    m_ThreadPub->detach();
+
+	return true;
+}
+void CLaser::Disconnect()
+{
+	if (m_ThreadRcv)
+	{
+		delete m_ThreadRcv;
+		m_ThreadRcv = 0;
+	}
+
+    if (m_ThreadPub)
+    {
+        delete m_ThreadPub;
+        m_ThreadPub = 0;
+    }
+
+	if (m_ThreadPro)
+	{
+		delete m_ThreadPro;
+		m_ThreadPro = NULL;
+	}
+	m_statu = eLaser_disconnect;
+}
+
+
+//对外的接口函数，负责接受并处理任务单，
+//input：laser_task 雷达任务单，必须是子类，并且任务正确。（传引用）
+Error_manager CLaser::porform_task(Laser_task* p_laser_task)
+{
+    Error_manager t_error_manager;
+    //检查指针
+    if(p_laser_task == NULL)
+    {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "CLaser::porform_task failed, POINTER_IS_NULL");
+    }
+    else
+    {
+        //接受任务，并将任务的状态改为TASK_SIGNED已签收
+        mp_laser_task = p_laser_task;
+        mp_laser_task->set_task_statu(TASK_SIGNED);
+    }
+
+    //检查消息内容是否正确，
+    //检查点云指针
+    if(p_laser_task->get_task_point3d_cloud_vector() == NULL)
+    {
+        t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
+                                            Error_level::MINOR_ERROR,
+                                            "CLaser::porform_task failed, POINTER_IS_NULL");
+    }
+        //检查任务类型
+    else if(p_laser_task->get_task_type() != LASER_TASK)
+    {
+        t_error_manager.error_manager_reset(Error_code::LASER_TASK_PARAMETER_ERROR,
+                                            Error_level::MINOR_ERROR,
+                                            "CLaser::porform_task failed, task_type is error");
+    }
+    else
+    {
+        //将任务的状态改为 TASK_WORKING 处理中
+        mp_laser_task->set_task_statu(TASK_WORKING);
+
+        //启动雷达扫描
+        Start();
+    }
+
+    //返回错误码
+    if(t_error_manager != Error_code::SUCCESS)
+    {
+        mp_laser_task->set_task_error_manager(t_error_manager);
+    }
+    return t_error_manager;
+}
+
+
+void CLaser::SetSaveDir(std::string strDir,bool bSaveFile)
+{
+	m_bSave_file = bSaveFile;
+	m_pts_log_tool.close();
+	m_binary_log_tool.close();
+
+	if (bSaveFile == false)
+		return;
+
+	m_pts_save_path = strDir;
+	char pts_file[255] = { 0 };
+	sprintf(pts_file, "%s/pts%d.txt", strDir.c_str(), m_id+1);
+	m_pts_log_tool.open(pts_file);
+
+	char bin_file[255] = { 0 };
+	sprintf(bin_file, "%s/laser%d.data", strDir.c_str(), m_id+1);
+	m_binary_log_tool.open(bin_file,true);
+}
+
+void CLaser::thread_recv()
+{
+	while (m_bThreadRcvRun.WaitFor(1))
+	{
+		if (m_bStart_capture.WaitFor(1) == false)
+			continue;
+		CBinaryData* data=new CBinaryData();
+		if (this->RecvData(*data))
+		{
+			m_queue_laser_data.push(data);
+		}
+		else
+		{
+			delete data;
+            data=0;
+		}
+		
+	}
+}
+
+CPoint3D CLaser::transfor(CPoint3D point)
+{
+	if (m_dMatrix)
+	{
+		CPoint3D result;
+		double x = point.x;
+		double y = point.y;
+		double z = point.z;
+		result.x = x * m_dMatrix[0] + y*m_dMatrix[1] + z*m_dMatrix[2] + m_dMatrix[3];
+		result.y = x * m_dMatrix[4] + y*m_dMatrix[5] + z*m_dMatrix[6] + m_dMatrix[7];
+		result.z = x * m_dMatrix[8] + y*m_dMatrix[9] + z*m_dMatrix[10] + m_dMatrix[11];
+		return result;
+	}
+	else
+	{
+		return point;
+	}
+}
+
+void CLaser::thread_toXYZ()
+{
+	while (m_bThreadProRun.WaitFor(1))
+	{
+		if (m_bStart_capture.WaitFor(1) == false)
+		{
+			m_queue_laser_data.clear();
+			continue;
+		}
+        CBinaryData* pData=NULL;
+        bool bPop = m_queue_laser_data.try_pop(pData);
+        DATA_type type = eUnknow;
+        if (bPop || m_last_data.Length() != 0)
+        {
+            std::vector<CPoint3D> cloud;
+            if (bPop)
+            {
+                if (pData == NULL)
+                    continue;
+                m_binary_log_tool.write(pData->Data(), pData->Length());
+                type= this->Data2PointXYZ(pData, cloud);
+            }
+            else
+            {
+                type = this->Data2PointXYZ(NULL, cloud);
+            }
+            if (type == eUnknow)
+            {
+                delete pData;
+                continue;
+            }
+
+            if (type == eData || type == eStart || type == eStop)
+            {
+                for (int i = 0; i < cloud.size(); ++i)
+                {
+                    CPoint3D point = transfor(cloud[i]);
+                    if (m_point_callback_fnc)
+                        m_point_callback_fnc(point, m_point_callback_pointer);
+                    if(m_bSave_file) {
+                        char buf[64] = {0};
+                        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
+                        m_pts_log_tool.write(buf, strlen(buf));
+                    }
+
+                    //此时cloud为雷达采集数据的结果，转换后的三维点云。
+                    //将每个点存入任务单里面的点云容器。
+                    mp_laser_task->get_task_point3d_cloud_vector()->push_back(point);
+                }
+                m_points_count+=cloud.size();
+                if (type == eStop)
+                {
+                    if(m_bSave_file) {
+                        m_pts_log_tool.close();
+                        m_binary_log_tool.close();
+                    }
+                    m_statu = eLaser_ready;
+
+                    //在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
+                    if(mp_laser_task !=NULL)
+                    {
+                        if(mp_laser_task->get_statu() == TASK_WORKING)
+                        {
+                            mp_laser_task->set_task_statu(TASK_OVER);
+                        }
+                    }
+                }
+
+            }
+            else if (type == eReady)
+            {
+                if(m_bSave_file) {
+                    m_pts_log_tool.close();
+                    m_binary_log_tool.close();
+                }
+                m_statu = eLaser_ready;
+            }
+
+
+            delete pData;
+        }
+		
+	}
+}
+
+
+#include "unistd.h"
+void CLaser::threadPublish(CLaser *laser)
+{
+    if(laser==0) return ;
+    while(laser->m_bThreadRcvRun.WaitFor(1))
+    {
+        laser->PublishMsg();
+        usleep(1000*300);
+    }
+}
+
+#include "../common.h"
+void CLaser::PublishMsg()
+{
+    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_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());
+}

laser/Laser.h

@@ -0,0 +1,274 @@
+#ifndef __LASER__HH__
+#define __LASER__HH__
+#include "Point2D.h"
+#include "Point3D.h"
+#include "LogFiles.h"
+#include "../src/CalibParam.pb.h"
+#include "../src/StdCondition.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 CLaser
+{
+public:
+    //唯一的构造函数，按照设备名称和雷达参数来创建实例。
+    //input：id： 雷达设备的id，（唯一索引）
+    //input：laser_param：雷达的参数，
+    //注：利用protobuf创建stLaserCalibParam类，然后从文件读取参数
+	CLaser(int id,Automatic::stLaserCalibParam laser_param);
+	virtual ~CLaser();
+	///��������Դ
+	virtual bool Connect();
+	virtual void Disconnect();
+
+	//对外的接口函数，负责接受并处理任务单，
+	//input：p_laser_task 雷达任务单，必须是子类，并且任务正确。
+    virtual Error_manager porform_task(Laser_task* p_laser_task);
+
+	///��ʼ��ֹͣ�ɼ�
+	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(CLaser* 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;
+	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;
+
+    //任务单的指针，实际内存由应用层管理，
+    //接受任务后，指向新的任务单
+    Laser_task *  mp_laser_task;
+};
+
+
+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

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,275 @@
+
+#include "LivoxLaser.h"
+#include "../common.h"
+
+RegisterLaser(Livox)
+
+CLivoxLaser::DeviceItem CLivoxLaser::g_devices[kMaxLidarCount] = { 0 };
+
+std::map<uint8_t, std::string>	CLivoxLaser::g_handle_sn= std::map<uint8_t, std::string>();
+std::map<std::string, uint8_t>	CLivoxLaser::g_sn_handle = std::map<std::string, uint8_t>();
+std::map<std::string, CLivoxLaser*> CLivoxLaser::g_sn_laser= std::map<std::string, CLivoxLaser*>();
+CLivoxLaser*						CLivoxLaser::g_all_laser[kMaxLidarCount] = { 0 };
+unsigned int  CLivoxLaser::g_count[kMaxLidarCount] = { 0 };
+
+void CLivoxLaser::UpdataHandle()
+{
+	std::string sn = m_laser_param.sn();
+	if (g_sn_handle.find(sn) != g_sn_handle.end())
+	{
+		m_handle = g_sn_handle[sn];
+	}
+
+}
+
+
+bool CLivoxLaser::IsScanComplete()
+{
+	return g_count[m_handle] >= m_frame_maxnum;
+}
+
+void CLivoxLaser::InitLivox()
+{
+	static bool g_init = false;
+	if (g_init == false)
+	{
+		if (!Init()) {
+			LOG(INFO) << "livox sdk init failed...";
+		}
+		else
+		{
+			LivoxSdkVersion _sdkversion;
+			GetLivoxSdkVersion(&_sdkversion);
+			char buf[255] = { 0 };
+			sprintf(buf, "Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
+			LOG(INFO) << buf;
+			SetBroadcastCallback(OnDeviceBroadcast);
+			SetDeviceStateUpdateCallback(OnDeviceChange);
+			g_init = true;
+		}
+	}
+}
+
+CLivoxLaser::CLivoxLaser(int id, Automatic::stLaserCalibParam laser_param)
+	:CLaser(id, laser_param)
+{
+	
+	m_frame_maxnum = laser_param.frame_num();
+	if(laser_param.type()=="Livox")
+		g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
+
+	InitLivox();
+}
+
+
+
+
+CLivoxLaser::~CLivoxLaser()
+{
+}
+
+void CLivoxLaser::OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
+	CLivoxLaser* laser = (CLivoxLaser*)data;
+
+	if (status == kStatusSuccess) {
+		if (response != 0) {
+			g_devices[handle].device_state = kDeviceStateConnect;
+		}
+	}
+	else if (status == kStatusTimeout) {
+		g_devices[handle].device_state = kDeviceStateConnect;
+	}
+}
+
+void CLivoxLaser::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
+{
+
+	if (info == NULL) {
+		return;
+	}
+	
+	uint8_t handle = info->handle;
+	if (handle >= kMaxLidarCount) {
+		return;
+	}
+	/*std::cout<<"-----------------------------------------------------------------"<<std::endl;
+    std::cout<<" OnDeviceChange  handle :   "<<info->broadcast_code<<"  type : "<<type <<"  device_state : "
+                <<g_devices[handle].device_state<<std::endl;*/
+
+	if (type == kEventConnect) {
+		//QueryDeviceInformation(handle, OnDeviceInformation, NULL);
+		if (g_devices[handle].device_state == kDeviceStateDisconnect)
+		{
+			g_devices[handle].device_state = kDeviceStateConnect;
+			g_devices[handle].info = *info;
+		}
+	}
+	else if (type == kEventDisconnect) {
+		g_devices[handle].device_state = kDeviceStateDisconnect;
+	}
+	else if (type == kEventStateChange) {
+		g_devices[handle].info = *info;
+	}
+
+	if (g_devices[handle].device_state == kDeviceStateConnect)
+	{
+		
+		if (g_devices[handle].info.state == kLidarStateNormal) {
+			if (g_devices[handle].info.type == kDeviceTypeHub) {
+				HubStartSampling(OnSampleCallback, NULL);
+			}
+			else {
+				LidarStartSampling(handle, OnSampleCallback, NULL);
+			}
+			g_devices[handle].device_state = kDeviceStateSampling;
+		}
+	}
+}
+void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
+{
+	if (info == NULL) {
+		return;
+	}
+
+	//printf("Receive Broadcast Code %s\n", info->broadcast_code);
+	LOG(INFO) << " broadcast  sn : " << info->broadcast_code;
+	bool result = false;
+	uint8_t handle = 0;
+	result = AddLidarToConnect(info->broadcast_code, &handle);
+	
+	if (result == kStatusSuccess) {
+		/** Set the point cloud data for a specific Livox LiDAR. */
+		SetDataCallback(handle, LidarDataCallback, NULL);
+		g_devices[handle].handle = handle;
+		g_devices[handle].device_state = kDeviceStateDisconnect;
+
+		std::string sn = info->broadcast_code;
+		if (g_handle_sn.find(handle) != g_handle_sn.end())
+			g_handle_sn[handle] = sn;
+		else
+			g_handle_sn.insert(std::make_pair(handle,sn));
+
+		if (g_sn_handle.find(sn) != g_sn_handle.end())
+			g_sn_handle[sn] = handle;
+		else
+			g_sn_handle.insert(std::make_pair(sn,handle));
+		
+	}
+
+}
+
+void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data)
+{
+	///?????????????? ?????????handle
+	CLivoxLaser* livox = g_all_laser[handle];
+	if (livox == 0)
+	{
+		if (g_handle_sn.find(handle) != g_handle_sn.end())
+		{
+			std::string sn = g_handle_sn[handle];
+			if (g_sn_laser.find(sn) != g_sn_laser.end())
+			{
+				livox = g_sn_laser[sn];
+				g_all_laser[handle] = livox;
+				if (livox)
+					livox->UpdataHandle();
+			}
+		}
+	}
+
+	
+	if (data && livox)
+	{
+		if (livox->m_bStart_capture.WaitFor(1))
+		{
+			if (livox->IsScanComplete())
+			{
+				livox->Stop();
+				return;
+			}
+			if (g_count[handle] >= livox->m_frame_maxnum)
+				return;
+			uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
+			LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
+			
+			CBinaryData* data_bin = new CBinaryData((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
+			livox->m_queue_livox_data.push(data_bin);
+			g_count[handle]++;
+			
+		}
+        else if(livox->m_statu!=eLaser_ready)
+		{
+			//?????????????????
+			livox->m_statu = eLaser_ready;
+            usleep(10*1000);
+		}
+	}
+	
+}
+
+eLaserStatu CLivoxLaser::GetStatu()
+{
+    if(g_devices[m_handle].device_state == kDeviceStateConnect||
+            g_devices[m_handle].device_state == kDeviceStateSampling)
+    {
+        m_statu=eLaser_ready;
+    }
+    if(g_devices[m_handle].device_state == kDeviceStateDisconnect)
+    {
+        m_statu=eLaser_disconnect;
+    }
+    return m_statu;
+}
+
+bool CLivoxLaser::Connect()
+{
+	return CLaser::Connect();
+}
+void CLivoxLaser::Disconnect() 
+{
+	CLaser::Disconnect();
+}
+bool CLivoxLaser::Start()
+{
+	LOG(INFO) << " livox start :"<<this; 
+	//???????????
+	m_queue_livox_data.clear();
+	g_count[m_handle] = 0;
+	return CLaser::Start();
+}
+bool CLivoxLaser::Stop()
+{
+	return CLaser::Stop();
+}
+
+bool CLivoxLaser::RecvData(CBinaryData& data)
+{
+	CBinaryData* livox_data;
+	if (m_queue_livox_data.try_pop(livox_data))
+	{
+		data = *livox_data;
+		delete livox_data;
+		return true;
+	}
+	return false;
+}
+DATA_type CLivoxLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
+{
+	LivoxRawPoint *p_point_data = (LivoxRawPoint *)pData->Data();
+	int count = pData->Length() / (sizeof(LivoxRawPoint));
+	
+	if (count <= 0)
+		return eUnknow;
+	for (int i = 0; i < count; ++i)
+	{
+		LivoxRawPoint point = p_point_data[i];
+		CPoint3D pt;
+		pt.x = point.x;
+		pt.y = point.y;
+		pt.z = point.z;
+		points.push_back(pt);
+	}
+	return eData;
+}
+
+

laser/LivoxLaser.h

@@ -0,0 +1,64 @@
+#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>
+
+
+class CLivoxLaser :public CLaser
+{
+protected:
+	typedef enum {
+		kDeviceStateDisconnect = 0,
+		kDeviceStateConnect = 1,
+		kDeviceStateSampling = 2,
+	} DeviceState;
+	typedef struct {
+		uint8_t handle;
+		DeviceState device_state;
+		DeviceInfo info;
+	} DeviceItem;
+
+
+public:
+	CLivoxLaser(int id, Automatic::stLaserCalibParam laser_param);
+	~CLivoxLaser();
+
+	virtual bool Connect();
+	virtual void Disconnect();
+	virtual bool Start();
+	virtual bool Stop();
+
+protected:
+	virtual bool RecvData(CBinaryData& data);
+	virtual DATA_type Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points);
+	virtual bool IsScanComplete();
+	virtual void UpdataHandle();
+	virtual eLaserStatu GetStatu();
+	
+protected:
+	static void InitLivox();
+	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;
+	threadsafe_queue<CBinaryData*>			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];
+
+
+};
+
+#endif
+

laser/LivoxMid100Laser.cpp

@@ -0,0 +1,100 @@
+
+#include "LivoxMid100Laser.h"
+#include "../common.h"
+
+RegisterLaser(LivoxMid100)
+
+
+CLivoxMid100Laser::CLivoxMid100Laser(int id, Automatic::stLaserCalibParam laser_param)
+	:CLivoxLaser(id, laser_param)
+{
+	m_frame_maxnum = laser_param.frame_num();
+	if (laser_param.type() == "LivoxMid100")
+	{
+		std::string sn = laser_param.sn();
+		std::string sn1 = sn, sn2 = sn, sn3 = sn;
+		sn1 += "1";
+		sn2 += "2";
+		sn3 += "3";
+		g_sn_laser.insert(std::make_pair(sn1, this));
+		g_sn_laser.insert(std::make_pair(sn2, this));
+		g_sn_laser.insert(std::make_pair(sn3, this));
+	}
+}
+
+void CLivoxMid100Laser::UpdataHandle()
+{
+	std::string sn = m_laser_param.sn();
+	std::string sn1 = sn, sn2 = sn, sn3 = sn;
+	sn1 += "1";
+	sn2 += "2";
+	sn3 += "3";
+	if (g_sn_handle.find(sn1) != g_sn_handle.end())
+	{
+		m_handle1 = g_sn_handle[sn1];
+	}
+	if (g_sn_handle.find(sn2) != g_sn_handle.end())
+	{
+		m_handle2 = g_sn_handle[sn2];
+	}
+	if (g_sn_handle.find(sn3) != g_sn_handle.end())
+	{
+		m_handle3 = g_sn_handle[sn3];
+	}
+}
+bool CLivoxMid100Laser::IsScanComplete()
+{
+	return g_count[m_handle1] >= m_frame_maxnum && g_count[m_handle2] >= m_frame_maxnum
+		&& g_count[m_handle2] >= m_frame_maxnum;
+}
+
+eLaserStatu CLivoxMid100Laser::GetStatu()
+{
+    bool cond1=g_devices[m_handle1].device_state == kDeviceStateConnect ||
+               g_devices[m_handle1].device_state == kDeviceStateSampling;
+    bool cond2=g_devices[m_handle2].device_state == kDeviceStateConnect ||
+               g_devices[m_handle2].device_state == kDeviceStateSampling;
+    bool cond3=g_devices[m_handle3].device_state == kDeviceStateConnect ||
+               g_devices[m_handle3].device_state == kDeviceStateSampling;
+    if (cond1 && cond2 && cond3) {
+        if(m_queue_laser_data.size()==0 && m_bscan_start==false)
+            m_statu = eLaser_ready;
+        else
+            m_statu = eLaser_busy;
+    }
+    else {
+        m_statu = eLaser_disconnect;
+    }
+
+
+    return m_statu;
+
+}
+
+bool CLivoxMid100Laser::Connect()
+{
+    return CLivoxLaser::Connect();
+}
+void CLivoxMid100Laser::Disconnect()
+{
+    CLivoxLaser::Disconnect();
+}
+
+bool CLivoxMid100Laser::Stop()
+{
+    return CLivoxLaser::Stop();
+}
+bool CLivoxMid100Laser::Start()
+{
+		LOG(INFO) << " livoxMid100 start :" << this;
+		//������ݣ���ʼ
+		m_queue_livox_data.clear();
+		g_count[m_handle1] = 0;
+		g_count[m_handle2] = 0;
+		g_count[m_handle3] = 0;
+		return CLaser::Start();
+}
+
+CLivoxMid100Laser::~CLivoxMid100Laser()
+{
+}

laser/LivoxMid100Laser.h

@@ -0,0 +1,29 @@
+#pragma once
+#include "LivoxLaser.h"
+#include "livox_def.h"
+#include "livox_sdk.h"
+#include <map>
+class CLivoxMid100Laser : public CLivoxLaser
+{
+
+public:
+	CLivoxMid100Laser(int id, Automatic::stLaserCalibParam laser_param);
+	~CLivoxMid100Laser();
+
+    virtual bool Connect();
+    virtual void Disconnect();
+    virtual bool Start();
+    virtual bool Stop();
+    virtual eLaserStatu GetStatu();
+
+protected:
+	virtual bool IsScanComplete();
+	virtual void UpdataHandle();
+
+protected:
+	uint8_t		m_handle1;
+	uint8_t		m_handle2;
+	uint8_t		m_handle3;
+};
+
+

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,18 @@
+#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_task_command.cpp

@@ -0,0 +1,128 @@
+
+//laser_task_command，是雷达任务指令的相关功能
+//功能：用作应用层向laser模块传输任务，的指令消息
+
+//用法：应用层直接调用laser的接口函数，并将Laser_task类作为参数传递
+//CLaser类则按照Laser_task的功能码和指定的参数，执行相应的功能
+//并将结果填充到Laser_task，返回给应用层
+
+#include "laser_task_command.h"
+//构造函数
+Laser_task::Laser_task()
+{
+    m_task_type = UNKNOW_TASK;
+    m_task_statu = TASK_CREATED;
+    //m_task_statu_information默认为空
+
+    m_task_points_number = 0;
+    pm_task_point3d_cloud_vector = NULL;
+    //m_task_error_manager 默认为空
+}
+//析构函数
+Laser_task::~Laser_task()
+{
+
+}
+
+//初始化任务单，必须初始化之后才可以使用，（必选参数）
+//input:task_type 任务类型
+// input:task_statu任务状态
+// output:p_task_point3d_cloud_vector三维点云容器指针
+//注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
+Error_manager Laser_task::task_init(Task_type task_type,
+                                    Task_statu task_statu,
+                                    std::vector<CPoint3D> * p_task_point3d_cloud_vector)
+{
+    if(p_task_point3d_cloud_vector == NULL)
+    {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "Laser_task::task_init failed");
+    }
+    m_task_type = task_type;
+    m_task_statu = task_statu;
+    m_task_statu_information.clear();
+
+    m_task_points_number = 0;
+    pm_task_point3d_cloud_vector = p_task_point3d_cloud_vector;
+    m_task_error_manager.error_manager_clear_all();
+
+    return Error_code::SUCCESS;
+}
+
+//初始化任务单，必须初始化之后才可以使用，（可选参数）
+//    input:task_type 任务类型
+//    input:task_statu任务状态
+//    input:task_statu_information状态说明
+//    input:task_points_number点云的采集数量
+//    output:p_task_point3d_cloud_vector三维点云容器指针
+//注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
+Error_manager Laser_task::task_init(Task_type task_type,
+                                    Task_statu task_statu,
+                                    std::string & task_statu_information,
+                                    int task_points_number,
+                                    std::vector<CPoint3D> * p_task_point3d_cloud_vector)
+{
+    if(p_task_point3d_cloud_vector == NULL)
+    {
+        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+                             "Laser_task::task_init failed");
+    }
+    m_task_type = task_type;
+    m_task_statu = task_statu;
+    m_task_statu_information = task_statu_information;
+
+    m_task_points_number = task_points_number;
+    pm_task_point3d_cloud_vector = p_task_point3d_cloud_vector;
+    m_task_error_manager.error_manager_clear_all();
+
+    return Error_code::SUCCESS;
+}
+
+//获取 点云的采集数量
+int Laser_task::get_task_points_number()
+{
+    return m_task_points_number;
+}
+//获取 三维点云容器
+std::vector<CPoint3D> * Laser_task::get_task_point3d_cloud_vector()
+{
+    return pm_task_point3d_cloud_vector;
+}
+//获取 错误码
+Error_manager Laser_task::get_task_error_manager()
+{
+    return m_task_error_manager;
+}
+
+//设置 任务状态
+void Laser_task::set_task_statu(Task_statu task_statu)
+{
+    m_task_statu = task_statu;
+}
+//设置 任务状态说明
+void Laser_task::set_task_statu_information(std::string & task_statu_information)
+{
+    m_task_statu_information = task_statu_information;
+}
+//设置 点云的采集数量
+void Laser_task::set_task_points_number(int task_points_number)
+{
+    m_task_points_number = task_points_number;
+}
+//设置 三维点云容器
+void Laser_task::set_task_point3d_cloud_vector(std::vector<CPoint3D> * p_task_point3d_cloud_vector)
+{
+    pm_task_point3d_cloud_vector = p_task_point3d_cloud_vector;
+}
+//设置 错误码
+void Laser_task::set_task_error_manager(Error_manager & error_manager)
+{
+    m_task_error_manager = error_manager;
+}
+
+
+
+
+
+
+

laser/laser_task_command.h

@@ -0,0 +1,85 @@
+
+//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 "../error_code/error_code.h"
+
+#include <vector>
+
+
+#include "../task/task_command_manager.h"
+
+//雷达模块的任务指令，从Task_Base继承，
+//补充了雷达专属的数据输入和输出
+class Laser_task:public Task_Base
+{
+public:
+    //构造函数
+    Laser_task();
+    //析构函数
+    ~Laser_task();
+
+    //初始化任务单，必须初始化之后才可以使用，（必选参数）
+    //input:task_type 任务类型
+    // input:task_statu任务状态
+    // output:p_task_point3d_cloud_vector三维点云容器指针
+    //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
+    Error_manager task_init(Task_type task_type,
+                            Task_statu task_statu,
+                            std::vector<CPoint3D> * p_task_point3d_cloud_vector);
+
+    //初始化任务单，必须初始化之后才可以使用，（可选参数）
+    //    input:task_type 任务类型
+    //    input:task_statu任务状态
+    //    input:task_statu_information状态说明
+    //    input:task_points_number点云的采集数量
+    //    output:p_task_point3d_cloud_vector三维点云容器指针
+    //注：task_points_number默认为0，如果为0，则使用laser默认的点云的采集数量
+    Error_manager task_init(Task_type task_type,
+                            Task_statu task_statu,
+                            std::string & task_statu_information,
+                            int task_points_number,
+                            std::vector<CPoint3D> * p_task_point3d_cloud_vector);
+
+    //获取 点云的采集数量
+    int get_task_points_number();
+    //获取 三维点云容器
+    std::vector<CPoint3D> * get_task_point3d_cloud_vector();
+    //获取 错误码
+    Error_manager get_task_error_manager();
+
+    //设置 任务状态
+    void set_task_statu(Task_statu task_statu);
+    //设置 任务状态说明
+    void set_task_statu_information(std::string & task_statu_information);
+    //设置 点云的采集数量
+    void set_task_points_number(int task_points_number);
+    //设置 三维点云容器
+    void set_task_point3d_cloud_vector(std::vector<CPoint3D> * p_task_point3d_cloud_vector);
+    //设置 错误码
+    void set_task_error_manager(Error_manager & error_manager);
+
+protected:
+    //点云的采集数量，任务输入
+    int                             m_task_points_number;
+
+    //采集结果，三维点云容器，任务输出
+    //这里只是指针，实际内存由应用层管理，初始化时必须保证内存有效。
+    std::vector<CPoint3D> *         pm_task_point3d_cloud_vector;
+
+    //错误码，任务故障信息，任务输出
+    Error_manager                   m_task_error_manager;
+};
+
+
+#endif //__LASER_TASK_COMMAND__HH__
+