20200115，修正laser_base主线流程

优化雷达模块

CMakeLists.txt

@@ -47,8 +47,8 @@ add_executable(LidarMeasure ./main.cpp ./error_code/error_code.cpp
         ${LASER_SRC} ${PLC_SRC} ${TERMINOR_SRC} ${LOCATE_SRC} ${TASK_MANAGER_SRC}
 #        ${TOOL_SRC}
         ./tool/binary_buf.cpp
-        tool/thread_condition.h tool/thread_condition.cpp
-        tool/thread_safe_queue.h tool/thread_safe_queue.cpp
+        ./tool/thread_condition.h ./tool/thread_condition.cpp
+        ./tool/thread_safe_queue.h ./tool/thread_safe_queue.cpp
 
         )
 

laser/Laser.cpp

@@ -1,205 +1,72 @@
+
+
 #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];
-}
-//////////////
 
-Laser_base::Laser_base(int id, Laser_proto::laser_parameter 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)
+Laser_base::Laser_base(int laser_id, Laser_proto::laser_parameter laser_param)
+:m_laser_id(laser_id)
 ,m_laser_param(laser_param)
-,m_bSave_file(false)
-,m_bscan_start(false)
+,m_laser_scan_flag(false)
+,m_laser_statu(LASER_DISCONNECT)
+,m_points_count(0)
+,m_save_flag(false)
 {
-    mp_laser_task = NULL;
+	mp_thread_receive = NULL;
+	mp_thread_transform = NULL;
+	mp_thread_publish = NULL;
+	init_laser_matrix();
+	mp_laser_task = NULL;
 }
-
-
 Laser_base::~Laser_base()
 {
-    if (m_dMatrix)
-    {
-        free(m_dMatrix);
-        m_dMatrix = 0;
-    }
+	m_queue_laser_data.termination_queue();
+	close_save_path();
+	m_condition_receive.kill_all();
+	m_condition_transform.kill_all();
+	m_condition_publish.kill_all();
 }
 
-bool Laser_base::Start()
+//雷达链接设备，为3个线程添加线程执行函数。
+Error_manager Laser_base::connect_laser()
 {
-    m_bscan_start=true;
-    m_statu = eLaser_busy;
-    m_bStart_capture.Notify(true);
-    m_points_count=0;
-    return true;
-}
+	m_bThreadRcvRun.notify_all(true);
+	if(m_ThreadRcv==0)
+		m_ThreadRcv = new std::thread(&Laser_base::thread_recv, this);
+	m_ThreadRcv->detach();
 
-bool Laser_base::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 已结束
-    //强制改为TASK_OVER，不管它当前在做什么。
-	if(mp_laser_task !=NULL)
-    {
-        mp_laser_task->set_task_statu(TASK_OVER);
-    }
+	m_bThreadProRun.notify_all(true);
+	if(m_ThreadPro==0)
+		m_ThreadPro = new std::thread(&Laser_base::thread_toXYZ, this);
+	m_ThreadPro->detach();
 
-    return true;
-}
+	if(m_ThreadPub==0)
+		m_ThreadPub = new std::thread(threadPublish, this);
+	m_ThreadPub->detach();
 
-void Laser_base::SetMetrix(double* data)
-{
-    if (m_dMatrix == 0)
-    {
-        m_dMatrix = (double*)malloc(12 * sizeof(double));
-    }
-    memcpy(m_dMatrix, data, 12 * sizeof(double));
+	return Error_code::SUCCESS;
 }
-void Laser_base::SetPointCallBack(PointCallBack fnc, void* pointer)
+//雷达断开链接，释放3个线程
+Error_manager Laser_base::disconnect_laser()
 {
-    m_point_callback_fnc = fnc;
-    m_point_callback_pointer = pointer;
-}
-
-bool Laser_base::Connect()
-{
-    m_bThreadRcvRun.Notify(true);
-    if(m_ThreadRcv==0)
-        m_ThreadRcv = new std::thread(&Laser_base::thread_recv, this);
-    m_ThreadRcv->detach();
-
-    m_bThreadProRun.Notify(true);
-    if(m_ThreadPro==0)
-        m_ThreadPro = new std::thread(&Laser_base::thread_toXYZ, this);
-    m_ThreadPro->detach();
-
-    if(m_ThreadPub==0)
-        m_ThreadPub = new std::thread(threadPublish, this);
-    m_ThreadPub->detach();
-
-    return true;
-}
-void Laser_base::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;
+	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_laser_statu = LASER_DISCONNECT;
+	return Error_code::SUCCESS;
 }
 
 
@@ -208,24 +75,56 @@ void Laser_base::Disconnect()
 //注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
 Error_manager Laser_base::execute_task(Task_Base* p_laser_task)
 {
-    //检查指针
-    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");
-    }
+	//检查指针
+	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_error()
+Error_manager Laser_base::check_laser()
+{
+	return Error_code::SUCCESS;
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager Laser_base::start_scan()
 {
-    return SUCCESS;
+	m_laser_scan_flag=true;
+	m_laser_statu = LASER_BUSY;
+	m_bStart_capture.notify_all(true);
+	m_points_count=0;
+	return Error_code::SUCCESS;
+}
+//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+Error_manager Laser_base::stop_scan()
+{
+	m_laser_scan_flag=false;
+	m_bStart_capture.notify_all(false);
+	m_binary_log_tool.close();
+	m_points_log_tool.close();
+	m_laser_statu=LASER_READY;
+
+	//在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
+	//强制改为TASK_OVER，不管它当前在做什么。
+	if(mp_laser_task !=NULL)
+	{
+		mp_laser_task->set_task_statu(TASK_OVER);
+	}
+
+	return Error_code::SUCCESS;
 }
 
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager Laser_base::end_task()
+{
+	return Error_code::SUCCESS;
+}
 /*
 //对外的接口函数，负责接受并处理任务单，
 //input：laser_task 雷达任务单，必须是子类，并且任务正确。（传引用）
@@ -278,184 +177,264 @@ Error_manager Laser_base::porform_task(Laser_task* p_laser_task)
 }
 */
 
-void Laser_base::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);
+//设置保存文件的路径，并打开文件，
+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
+	{
+		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);
+		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);
+		m_binary_log_tool.open(bin_file,true);
+		return Error_code::SUCCESS;
+	}
 }
-
-void Laser_base::thread_recv()
+//关闭保存文件，推出前一定要执行
+Error_manager Laser_base::close_save_path()
 {
-    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;
-        }
+	m_points_log_tool.close();
+	m_binary_log_tool.close();
+	return Error_code::SUCCESS;
+}
 
-    }
+//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
+bool Laser_base::is_ready()
+{
+	return ( m_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;
 }
 
-CPoint3D Laser_base::transfor(CPoint3D point)
+
+void Laser_base::thread_recv()
 {
-    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;
-    }
+	while (m_bThreadRcvRun.wait_for_millisecond(1))
+	{
+		if (m_bStart_capture.wait_for_millisecond(1) == false)
+			continue;
+		Binary_buf* data=new Binary_buf();
+		if (this->RecvData(*data))
+		{
+			m_queue_laser_data.push(data);
+		}
+		else
+		{
+			delete data;
+			data=0;
+		}
+
+	}
 }
 
+
+
 void Laser_base::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;
-                if(m_bSave_file)
-                {
-                    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_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为雷达采集数据的结果，转换后的三维点云。
-                    //将每个点存入任务单里面的点云容器。
-
-                    if(mp_laser_task!=NULL)
-                    {
-                        Error_manager code= mp_laser_task->task_push_point(pcl::PointXYZ(point.x,point.y,point.z));
-                    }
-                }
-                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;
-        }
-
-    }
+	while (m_bThreadProRun.wait_for_millisecond(1))
+	{
+		if (m_bStart_capture.wait_for_millisecond(1) == false)
+		{
+			m_queue_laser_data.clear();
+			continue;
+		}
+		Binary_buf* pData=NULL;
+		bool bPop = m_queue_laser_data.try_pop(pData);
+		Buf_type type = BUF_UNKNOW;
+		if (bPop || m_last_data.get_length() != 0)
+		{
+			std::vector<CPoint3D> cloud;
+			if (bPop)
+			{
+				if (pData == NULL)
+					continue;
+				if(m_save_flag)
+				{
+					m_binary_log_tool.write(pData->get_buf(), pData->get_length());
+				}
+				type= this->Data2PointXYZ(pData, cloud);
+			}
+			else
+			{
+				type = this->Data2PointXYZ(NULL, cloud);
+			}
+
+
+			if (type == BUF_UNKNOW)
+			{
+				delete pData;
+				continue;
+			}
+
+			if (type == BUF_DATA || type == BUF_START || type == BUF_STOP)
+			{
+				for (int i = 0; i < cloud.size(); ++i)
+				{
+					CPoint3D point = transform_by_matrix(cloud[i]);
+					if(m_save_flag) {
+						char buf[64] = {0};
+						sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
+						m_points_log_tool.write(buf, strlen(buf));
+					}
+
+					//此时cloud为雷达采集数据的结果，转换后的三维点云。
+					//将每个点存入任务单里面的点云容器。
+
+					if(mp_laser_task!=NULL)
+					{
+						Error_manager code= mp_laser_task->task_push_point(pcl::PointXYZ(point.x,point.y,point.z));
+					}
+				}
+				m_points_count+=cloud.size();
+				if (type == BUF_STOP)
+				{
+					if(m_save_flag) {
+						m_points_log_tool.close();
+						m_binary_log_tool.close();
+					}
+					m_laser_statu = LASER_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 == BUF_READY)
+			{
+				if(m_save_flag) {
+					m_points_log_tool.close();
+					m_binary_log_tool.close();
+				}
+				m_laser_statu = LASER_READY;
+			}
+
+
+			delete pData;
+		}
+
+	}
 }
 
 
 #include "unistd.h"
 void Laser_base::threadPublish(Laser_base *laser)
 {
-    if(laser==0) return ;
-    while(laser->m_bThreadRcvRun.WaitFor(1))
-    {
-        laser->PublishMsg();
-        usleep(1000*300);
-    }
+	if(laser==0) return ;
+	while(laser->m_bThreadRcvRun.wait_for_millisecond(1))
+	{
+		laser->PublishMsg();
+		usleep(1000*300);
+	}
 }
 
 void Laser_base::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());
-     */
-}
+	/*
+	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());
+	 */
+}
+
+
+//初始化变换矩阵，设置默认值
+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;
+	}
+}
+//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以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

@@ -1,266 +1,155 @@
+
+
 #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 "../error_code/error_code.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"
 
-//雷达消息的类型
-//在通信消息的前面一部分字符串，表示这条消息的类型。
-//在解析消息的时候，先解析前面的消息类型，来判断这条消息的功用
-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,         //雷达错误
+enum Laser_statu
+{//默认值 LASER_DISCONNECT	=0
+	LASER_DISCONNECT	=0,	        //雷达断连
+	LASER_READY			=1,			//雷达正常待机，空闲
+	LASER_BUSY			=2,	        //雷达正在工作，正忙
+	LASER_ERROR			=3,         //雷达错误
 };
 
-//回调函数，当有事件触发是，便会自动调用回调函数，
-//typedef从定义，将其简化为PointCallBack，方便后面多次使用
-typedef void (*PointCallBack)(CPoint3D ,  void* );
+//雷达变换矩阵的数组长度，默认为12个参数， size = 3 * ( 3 + 1 ) ，旋转加平移，没有缩放
+#define LASER_MATRIX_ARRAY_SIZE 12
 
 //雷达的基类，不能直接使用，必须子类继承
 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();
-
-    //雷达链接设备，为3个线程添加线程执行函数。
-    virtual bool Connect();
-    //雷达断开链接，释放3个线程
-    virtual void Disconnect();
-
+	Laser_base() = delete;
+	Laser_base(const Laser_base& other) = delete;
+	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
+	//input：id： 雷达设备的id，（唯一索引）
+	//input：laser_param：雷达的参数，
+	//注：利用protobuf创建stLaserCalibParam类，然后从文件读取参数
+	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);
-    //检查雷达状态,是否正常运行
-    Error_manager check_error();
-    //雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达扫描方式不同。
-    virtual bool Start();
-    //雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达扫描方式不同。
-    virtual bool Stop();
-
-    //设置转化矩阵，用作三维点的坐标变换，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-    void SetMetrix(double* data);
-    //设置回调函数，外部通过回调函数，来获取雷达的扫描结果
-    void SetPointCallBack(PointCallBack fnc,void* pointer);
+	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:
-    //设置保存为文件的路径
-    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;}
-    //获取雷达id
-    int ID() { return m_id; }
-
+	//设置保存文件的路径，并打开文件，
+	Error_manager set_open_save_path(std::string save_path,bool is_save=true);
+	//关闭保存文件，推出前一定要执行
+	Error_manager close_save_path();
+	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
+	bool is_ready();
+	//获取雷达状态
+	virtual Laser_statu get_laser_statu();
+	//获取雷达id
+	int get_laser_id();
 
 
 protected:
-    //接受二进制消息的功能函数，每次只操作一个CBinaryData
-    // 纯虚函数，必须由子类重载，
-    virtual bool RecvData(CBinaryData& data) = 0;
-    //线程执行函数，将二进制消息存入队列缓存，
-    void thread_recv();
-
-    //将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-    // 纯虚函数，必须由子类重载，
-    virtual DATA_type Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)=0;
-    //线程执行函数，转化并处理三维点云。
-    void thread_toXYZ();
-
-    //三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-    virtual CPoint3D transfor(CPoint3D point);
+	//接受二进制消息的功能函数，每次只接受一个CBinaryData
+	// 纯虚函数，必须由子类重载，
+	virtual bool RecvData(Binary_buf& data) = 0;
+	//线程执行函数，将二进制消息存入队列缓存，
+	void thread_recv();
+	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
+	// 纯虚函数，必须由子类重载，
+	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)=0;
+	//线程执行函数，转化并处理三维点云。
+	void thread_toXYZ();
+	//线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
+	static void threadPublish(Laser_base* laser);
+	//公开发布雷达信息的功能函数，
+	void PublishMsg();
+
 protected:
-    //线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
-    static void threadPublish(Laser_base* laser);
-    //公开发布雷达信息的功能函数，
-    void PublishMsg();
+	//初始化变换矩阵，设置默认值
+	Error_manager init_laser_matrix();
+	//设置变换矩阵，用作三维点的坐标变换，
+	Error_manager set_laser_matrix(double* p_matrix, int size);
 
+	//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
+	virtual CPoint3D transform_by_matrix(CPoint3D point);
 
 
 protected:
-    //线程指针的内存管理，由Connect和Disconnect进行分配和释放。
-    std::thread*		m_ThreadRcv;            //接受缓存的线程指针
-    std::thread*		m_ThreadPro;            //转化缓存的线程指针
-    std::thread*        m_ThreadPub;            //发布信息的线程指针
-    StdCondition		m_bThreadRcvRun;        //接受缓存的线程条件
-    StdCondition		m_bThreadProRun;        //转化缓存的线程条件
-    std::mutex          m_scan_lock;            //雷达线程的锁
-
-    int					m_id;                   //雷达设备id
-    int                 m_points_count;         //雷达采集点的计数
-
-    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;			    //最后一个二进制缓存
-    PointCallBack						m_point_callback_fnc;       //回调函数
-    void*								m_point_callback_pointer;   //回调函数的指针
-
-    //变换矩阵，三维点的坐标变换的矩阵，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-    //构造和初始化是没有分配内存的，只有在SetMetrix函数里面分配实际的内存，
-    double*								m_dMatrix;              //变换矩阵
-
-    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;                //任务单的指针
+	//为了保证多线程的数据安全，修改共享数据必须加锁。  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;             	//雷达状态
+
+	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;          //三维点云的日志工具
+
+
+	Thread_condition	m_bStart_capture;           //线程条件？？？？？？？
+
+
+
+	//线程指针的内存管理，由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;		//发布信息的条件变量
+
+	//线程指针的内存管理，由Connect和Disconnect进行分配和释放。
+	std::thread*		m_ThreadRcv;            //接受缓存的线程指针
+	std::thread*		m_ThreadPro;            //转化缓存的线程指针
+	std::thread*        m_ThreadPub;            //发布信息的线程指针
+	Thread_condition		m_bThreadRcvRun;        //接受缓存的线程条件
+	Thread_condition		m_bThreadProRun;        //转化缓存的线程条件
+
+	//任务单的指针，实际内存由应用层管理，
+	//接受任务后，指向新的任务单
+	Laser_task *  						mp_laser_task;        		//任务单的指针
+
 };
 
 
@@ -279,18 +168,18 @@ public:
 	}
 	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);
-    }
+			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;
-    }
+	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;
+	}
 };
 
 

laser/LivoxLaser.cpp

@@ -11,25 +11,134 @@ std::map<std::string, CLivoxLaser*> CLivoxLaser::g_sn_laser= std::map<std::strin
 CLivoxLaser*						CLivoxLaser::g_all_laser[kMaxLidarCount] = { 0 };
 unsigned int  CLivoxLaser::g_count[kMaxLidarCount] = { 0 };
 
-void CLivoxLaser::UpdataHandle()
+
+
+
+
+CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
+:Laser_base(id, laser_param)
 {
-	std::string sn = m_laser_param.sn();
-	if (g_sn_handle.find(sn) != g_sn_handle.end())
+
+	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()
+{
+
+}
+
+//雷达链接设备，为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)
+{
+	//检查指针
+	if(p_laser_task == NULL)
 	{
-		m_handle = g_sn_handle[sn];
+		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
+							 "CLivoxLaser::execute_task failed, POINTER_IS_NULL");
+	}
+	else
+	{
+		return Error_manager(Error_code::CLASS_BASE_FUNCTION_CANNOT_USE, Error_level::NEGLIGIBLE_ERROR,
+							 "CLivoxLaser::execute_task cannot use");
+	}
+}
+//检查雷达状态,是否正常运行
+Error_manager CLivoxLaser::check_laser()
+{
+	return Error_code::SUCCESS;
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager CLivoxLaser::start_scan()
+{
+	LOG(INFO) << " livox start :"<<this;
+	//???????????
+	m_queue_livox_data.clear();
+	g_count[m_handle] = 0;
+	return Laser_base::start_scan();
+}
+//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+Error_manager CLivoxLaser::stop_scan()
+{
+	return Laser_base::stop_scan();
+}
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager CLivoxLaser::end_task()
+{
+	return Error_code::SUCCESS;
+}
+
+bool CLivoxLaser::RecvData(Binary_buf& data)
+{
+	Binary_buf* livox_data;
+	if (m_queue_livox_data.try_pop(livox_data))
+	{
+		data = *livox_data;
+		delete livox_data;
+		return true;
 	}
+	return false;
 }
+Buf_type CLivoxLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
+{
+	LivoxRawPoint *p_point_data = (LivoxRawPoint *)pData->get_buf();
+	int count = pData->get_length() / (sizeof(LivoxRawPoint));
 
+	if (count <= 0)
+		return BUF_UNKNOW;
+	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 BUF_DATA;
+}
 
-bool CLivoxLaser::IsScanComplete()
+Laser_statu CLivoxLaser::get_laser_statu()
 {
-    //雷达的采集帧数判断，直接比较任务单里面的帧数最大值
-    if(mp_laser_task!=NULL)
-	    return g_count[m_handle] >= mp_laser_task->get_task_frame_maxnum();
-    else
-        return false;
+	if(g_devices[m_handle].device_state == kDeviceStateConnect||
+	   g_devices[m_handle].device_state == kDeviceStateSampling)
+	{
+		m_laser_statu=LASER_READY;
+	}
+	if(g_devices[m_handle].device_state == kDeviceStateDisconnect)
+	{
+		m_laser_statu=LASER_DISCONNECT;
+	}
+	return m_laser_statu;
 }
 
+
+
+
+
+
+
+
+
+
+
+
+
 void CLivoxLaser::InitLivox()
 {
 	static bool g_init = false;
@@ -52,22 +161,22 @@ void CLivoxLaser::InitLivox()
 	}
 }
 
-CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
-	:Laser_base(id, laser_param)
+bool CLivoxLaser::IsScanComplete()
 {
-	
-	m_frame_maxnum = laser_param.frame_num();
-	if(laser_param.type()=="Livox")
-		g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
-
-	InitLivox();
+	//雷达的采集帧数判断，直接比较任务单里面的帧数最大值
+	if(mp_laser_task!=NULL)
+		return g_count[m_handle] >= mp_laser_task->get_task_frame_maxnum();
+	else
+		return false;
 }
 
-
-
-
-CLivoxLaser::~CLivoxLaser()
+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];
+	}
 }
 
 void CLivoxLaser::OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
@@ -89,7 +198,7 @@ void CLivoxLaser::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
 	if (info == NULL) {
 		return;
 	}
-	
+
 	uint8_t handle = info->handle;
 	if (handle >= kMaxLidarCount) {
 		return;
@@ -115,7 +224,7 @@ void CLivoxLaser::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
 
 	if (g_devices[handle].device_state == kDeviceStateConnect)
 	{
-		
+
 		if (g_devices[handle].info.state == kLidarStateNormal) {
 			if (g_devices[handle].info.type == kDeviceTypeHub) {
 				HubStartSampling(OnSampleCallback, NULL);
@@ -138,7 +247,7 @@ void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
 	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);
@@ -155,7 +264,7 @@ void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
 			g_sn_handle[sn] = handle;
 		else
 			g_sn_handle.insert(std::make_pair(sn,handle));
-		
+
 	}
 
 }
@@ -179,117 +288,32 @@ void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32
 		}
 	}
 
-	
+
 	if (data && livox)
 	{
-		if (livox->m_bStart_capture.WaitFor(1))
+		if (livox->m_bStart_capture.wait_for_millisecond(1))
 		{
 			if (livox->IsScanComplete())
 			{
-				livox->Stop();
+				livox->stop_scan();
 				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));
+
+			Binary_buf* data_bin = new Binary_buf((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)
+		else if(livox->m_laser_statu!=LASER_READY)
 		{
 			//?????????????????
-			livox->m_statu = eLaser_ready;
-            usleep(10*1000);
+			livox->m_laser_statu = LASER_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 Laser_base::Connect();
-}
-void CLivoxLaser::Disconnect() 
-{
-	Laser_base::Disconnect();
-}
-bool CLivoxLaser::Start()
-{
-	LOG(INFO) << " livox start :"<<this; 
-	//???????????
-	m_queue_livox_data.clear();
-	g_count[m_handle] = 0;
-	return Laser_base::Start();
-}
-bool CLivoxLaser::Stop()
-{
-	return Laser_base::Stop();
-}
 
-//对外的接口函数，负责接受并处理任务单，
-//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-Error_manager CLivoxLaser::execute_task(Task_Base* p_laser_task)
-{
-    //检查指针
-    if(p_laser_task == NULL)
-    {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "CLivoxLaser::execute_task failed, POINTER_IS_NULL");
-    }
-    else
-    {
-        return Error_manager(Error_code::CLASS_BASE_FUNCTION_CANNOT_USE, Error_level::NEGLIGIBLE_ERROR,
-                             "CLivoxLaser::execute_task cannot use");
-    }
-}
-
-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

@@ -25,25 +25,34 @@ public:
 	CLivoxLaser(int id, Laser_proto::laser_parameter laser_param);
 	~CLivoxLaser();
 
-	virtual bool Connect();
-	virtual void Disconnect();
-	virtual bool Start();
-	virtual bool Stop();
+	//雷达链接设备，为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();
 
-    //对外的接口函数，负责接受并处理任务单，
-    //input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-    //注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-    virtual Error_manager execute_task(Task_Base* p_laser_task);
 
 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();
-	
+	virtual bool RecvData(Binary_buf& data);
+	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
+	virtual Laser_statu get_laser_statu();
+
+
 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);
@@ -52,7 +61,7 @@ protected:
 protected:
 	uint8_t									m_handle;
 	unsigned int							m_frame_maxnum;
-	threadsafe_queue<CBinaryData*>			m_queue_livox_data;
+	Thread_safe_queue<Binary_buf*>			m_queue_livox_data;
 
 	static DeviceItem						g_devices[kMaxLidarCount];
 	

laser/LivoxMid100Laser.cpp

@@ -20,105 +20,40 @@ CLivoxMid100Laser::CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_
         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()
-{
-    //雷达的采集帧数判断，直接比较任务单里面的帧数最大值
-    if(mp_laser_task==NULL)
-    {
-        return false;
-    }
-    else
-    {
-        int frame_count=mp_laser_task->get_task_frame_maxnum();
-        return g_count[m_handle1] >= frame_count && g_count[m_handle2] >= frame_count
-               && g_count[m_handle2] >= frame_count;
-    }
-}
-
-eLaserStatu CLivoxMid100Laser::GetStatu()
+CLivoxMid100Laser::~CLivoxMid100Laser()
 {
-    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()
+//雷达链接设备，为3个线程添加线程执行函数。
+Error_manager CLivoxMid100Laser::connect_laser()
 {
-    //设置点云变换矩阵
-    double matrix[12]={0};
-    matrix[0]=m_laser_param.mat_r00();
-    matrix[1]=m_laser_param.mat_r01();
-    matrix[2]=m_laser_param.mat_r02();
-    matrix[3]=m_laser_param.mat_r03();
-
-    matrix[4]=m_laser_param.mat_r10();
-    matrix[5]=m_laser_param.mat_r11();
-    matrix[6]=m_laser_param.mat_r12();
-    matrix[7]=m_laser_param.mat_r13();
-
-    matrix[8]=m_laser_param.mat_r20();
-    matrix[9]=m_laser_param.mat_r21();
-    matrix[10]=m_laser_param.mat_r22();
-    matrix[11]=m_laser_param.mat_r23();
-    SetMetrix(matrix);
-    return CLivoxLaser::Connect();
+	//设置点云变换矩阵
+	double matrix[12]={0};
+	matrix[0]=m_laser_param.mat_r00();
+	matrix[1]=m_laser_param.mat_r01();
+	matrix[2]=m_laser_param.mat_r02();
+	matrix[3]=m_laser_param.mat_r03();
+
+	matrix[4]=m_laser_param.mat_r10();
+	matrix[5]=m_laser_param.mat_r11();
+	matrix[6]=m_laser_param.mat_r12();
+	matrix[7]=m_laser_param.mat_r13();
+
+	matrix[8]=m_laser_param.mat_r20();
+	matrix[9]=m_laser_param.mat_r21();
+	matrix[10]=m_laser_param.mat_r22();
+	matrix[11]=m_laser_param.mat_r23();
+	set_laser_matrix(matrix, 12);
+	return CLivoxLaser::connect_laser();
 }
-void CLivoxMid100Laser::Disconnect()
+//雷达断开链接，释放3个线程
+Error_manager CLivoxMid100Laser::disconnect_laser()
 {
-    CLivoxLaser::Disconnect();
+	return CLivoxLaser::disconnect_laser();
 }
 
-bool CLivoxMid100Laser::Stop()
-{
-    LOG(INFO)<<" livoxmid100 stoped !!!"<<m_laser_param.sn();
-    return CLivoxLaser::Stop();
-}
-bool CLivoxMid100Laser::Start()
-{
-    LOG(INFO) << " livoxMid100 start :" << m_laser_param.sn();
-    m_queue_livox_data.clear();
-    g_count[m_handle1] = 0;
-    g_count[m_handle2] = 0;
-    g_count[m_handle3] = 0;
-    return Laser_base::Start();
-}
+
 
 //对外的接口函数，负责接受并处理任务单，
 //input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
@@ -156,7 +91,7 @@ Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
 //将任务的状态改为 TASK_WORKING 处理中
         mp_laser_task->set_task_statu(TASK_WORKING);
         std::string save_path = mp_laser_task->get_task_save_path();
-        SetSaveDir(save_path);
+		set_open_save_path(save_path);
 //启动雷达扫描
         Start();
     }
@@ -166,6 +101,89 @@ Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
     }
     return t_error_manager;
 }
-CLivoxMid100Laser::~CLivoxMid100Laser()
+
+//检查雷达状态,是否正常运行
+Error_manager CLivoxMid100Laser::check_laser()
+{
+	return Error_code::SUCCESS;
+}
+
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager CLivoxMid100Laser::start_scan()
+{
+	LOG(INFO) << " livoxMid100 start :" << m_laser_param.sn();
+	m_queue_livox_data.clear();
+	g_count[m_handle1] = 0;
+	g_count[m_handle2] = 0;
+	g_count[m_handle3] = 0;
+	return CLivoxLaser::start_scan();
+}
+//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+Error_manager CLivoxMid100Laser::stop_scan()
 {
+	LOG(INFO)<<" livoxmid100 stoped !!!"<<m_laser_param.sn();
+	return CLivoxLaser::stop_scan();
 }
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager CLivoxMid100Laser::end_task()
+{
+	return Error_code::SUCCESS;
+}
+
+Laser_statu CLivoxMid100Laser::get_laser_statu()
+{
+	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_laser_scan_flag==false)
+			m_laser_statu = LASER_READY;
+		else
+			m_laser_statu = LASER_BUSY;
+	}
+	else {
+		m_laser_statu = LASER_DISCONNECT;
+	}
+
+
+	return m_laser_statu;
+
+}
+
+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()
+{
+	//雷达的采集帧数判断，直接比较任务单里面的帧数最大值
+	if(mp_laser_task==NULL)
+	{
+		return false;
+	}
+	else
+	{
+		int frame_count=mp_laser_task->get_task_frame_maxnum();
+		return g_count[m_handle1] >= frame_count && g_count[m_handle2] >= frame_count
+			   && g_count[m_handle2] >= frame_count;
+	}
+}

laser/LivoxMid100Laser.h

@@ -10,22 +10,31 @@ public:
 	CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param);
 	~CLivoxMid100Laser();
 
-    virtual bool Connect();
-    virtual void Disconnect();
-    virtual bool Start();
-    virtual bool Stop();
+	//雷达链接设备，为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();
 
-    //对外的接口函数，负责接受并处理任务单，
-    //input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-    //注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-    Error_manager execute_task(Task_Base* p_laser_task);
-
-    virtual eLaserStatu GetStatu();
 
+protected:
+	virtual Laser_statu get_laser_statu();
 protected:
 	virtual bool IsScanComplete();
 	virtual void UpdataHandle();
 
+
 protected:
 	uint8_t		m_handle1;
 	uint8_t		m_handle2;

laser/Sick511FileLaser.cpp

@@ -0,0 +1,323 @@
+
+#include "Sick511FileLaser.h"
+#include <unistd.h>
+
+RegisterLaser(Sick511File);
+CSick511FileLaser::CSick511FileLaser(int id, Laser_proto::laser_parameter laser_param)
+:Laser_base(id, laser_param)
+,m_start_read(false)
+{
+}
+
+CSick511FileLaser::~CSick511FileLaser()
+{
+	if (m_stream_read.is_open())
+		m_stream_read.close();
+}
+
+
+
+//雷达链接设备，为3个线程添加线程执行函数。
+Error_manager CSick511FileLaser::connect_laser()
+{
+	std::string ip = m_laser_param.laser_ip();
+	char file[255] = { 0 };
+	sprintf(file, "%s/laser%d.data", ip.c_str(), m_laser_id + 1);
+	if (m_stream_read.is_open())
+	{
+		m_stream_read.close();
+	}
+	m_stream_read.open(file, ios::in | ios::binary);
+	if (!m_stream_read.good())
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							 "m_stream_read.open error ");
+	m_laser_statu = LASER_READY;
+	m_file = file;
+	return Laser_base::connect_laser();
+}
+//雷达断开链接，释放3个线程
+Error_manager CSick511FileLaser::disconnect_laser()
+{
+	if(m_stream_read.is_open())
+		m_stream_read.close();
+	return Laser_base::disconnect_laser();
+}
+//对外的接口函数，负责接受并处理任务单，
+//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
+Error_manager CSick511FileLaser::execute_task(Task_Base* p_laser_task)
+{
+
+}
+//检查雷达状态,是否正常运行
+Error_manager CSick511FileLaser::check_laser()
+{
+
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager CSick511FileLaser::start_scan()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+
+	if (!this->is_ready())
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							" is_ready error ");
+
+	m_start_read = true;
+	return Laser_base::start_scan();
+
+}
+//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+Error_manager CSick511FileLaser::stop_scan()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+	m_start_read = false;
+	return Error_code::SUCCESS;
+}
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager CSick511FileLaser::end_task()
+{
+
+}
+
+
+
+Buf_type CSick511FileLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
+{
+	////ƴ���ϴ�δ�������
+
+	Buf_type type = BUF_UNKNOW;
+	Binary_buf frame;
+
+	if (m_last_data.get_length() > 0)
+	{
+		if (pData)
+			frame = m_last_data + (*pData);
+		else
+			frame = m_last_data;
+		m_last_data = Binary_buf();
+	}
+	else if(pData)
+	{
+		frame = (*pData);
+	}
+	else
+	{
+		return type;
+	}
+
+	int head = FindHead(frame.get_buf(), frame.get_length());
+	int tail = FindTail(frame.get_buf(), frame.get_length());
+
+	if (tail >= 0)
+	{
+		if (tail < frame.get_length() - 1)		//���������
+		{
+			m_last_data = Binary_buf(frame.get_buf() + tail + 1, frame.get_length() - tail - 1);
+		}
+		if (head >= 0 && head < tail)
+		{
+			////���
+			Binary_buf data(frame.get_buf() + head, tail - head + 1);
+			if (data.is_equal_front( "$SLSSTP"))
+				type = BUF_STOP;
+			else if (data.is_equal_front( "$SLSSTA"))
+				type = BUF_START;
+			else if (data.is_equal_front( "$SCLRDY"))
+				type = BUF_READY;
+			else if (data.is_equal_front( "$SHWERR"))
+				type = BUF_ERROR;
+			else if (data.is_equal_front( "$N"))
+				type = BUF_DATA;
+
+			if (type == BUF_DATA)
+			{
+				////��������
+				points.clear();
+				if (data.get_length() <= 29)
+					return type;
+
+				////����
+				unsigned char angle_1 = 0;
+				unsigned char angle_2 = 0;
+				memcpy(&angle_1, (data.get_buf())+26, 1);
+				memcpy(&angle_2, (data.get_buf())+27, 1);
+				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
+				float start_angle = 0.0;
+				float freq = 0.0;
+
+				std::vector<float> distance;
+				if (GetData(&data, distance, freq, start_angle))
+				{
+					float beta = (beta_a)*DEGREES;
+					float sin_beta = sin(beta);
+					float cos_beta = cos(beta);
+					for (int i = 0; i < distance.size(); ++i)
+					{
+						if (distance[i] < 0.001)
+							continue;
+
+						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
+						float sin_alpha = sin(alpha);
+						float cos_alpha = cos(alpha);
+
+						float x = distance[i] * sin_alpha;
+						float y = distance[i] * cos_alpha * sin_beta;
+						float z = distance[i] * cos_alpha * cos_beta;
+
+						points.push_back(CPoint3D(x, y, z));
+					}
+					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
+				}
+
+			}
+			else if (type == BUF_STOP)
+			{
+				m_laser_statu = LASER_READY;
+			}
+		}
+	}
+	else if (head >= 0)
+	{
+		m_last_data = Binary_buf(frame.get_buf() + head, frame.get_length() - head);
+	}
+
+	return type;
+}
+
+bool CSick511FileLaser::RecvData(Binary_buf& data)
+{
+	if (m_start_read == false)
+		return false;
+	if (!m_stream_read.is_open())
+		return false;
+
+	if (m_stream_read.eof())
+	{
+		stop_scan();
+		m_stream_read.close();
+		usleep(100*1000);
+		m_stream_read.open(m_file.c_str(), ios::in | ios::binary);
+	}
+
+	char buf[512] = { 0 };
+	m_stream_read.read(buf, 512);
+	int count = m_stream_read.gcount();
+	if (count > 0)
+	{
+		Binary_buf bin_data(buf, count);
+		data = bin_data;
+		return true;
+	}
+	return false;
+}
+
+int CSick511FileLaser::FindHead(char* buf, int b_len)
+{
+	int i = 0;
+	if (b_len < 2)
+		return NULL;
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len > 10)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
+				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
+				return i;
+		}
+		if (b_len > 7)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7] == '*')
+				return i;
+		}
+	}
+	return -1;
+}
+
+int CSick511FileLaser::FindTail(char* buf, int b_len)
+{
+	int i = 0;
+
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len >= i + 5)
+		{
+			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
+				return i + 4;
+		}
+	}
+	return -9999999;
+}
+
+bool CSick511FileLaser::GetData(Binary_buf* pData, std::vector<float>& distance,
+								float& freq, float& start_angle)
+{
+	struct stData
+	{
+		const char* data;
+		int length;
+	};
+	std::vector<struct stData> strDatas;
+	int start = 0;
+	int end = 0;
+	int LMDscandata_index = -1;
+	for (int i = 0; i < pData->get_length(); ++i)
+	{
+		if ((*pData)[i] == ' ')
+		{
+			end = i;
+			if (end > start)
+			{
+				struct stData strData;
+//				strData.data = (*pData + start);
+				strData.data = ( pData->get_buf() + start);
+				strData.length = end - start;
+
+				strDatas.push_back(strData);
+				if (strncmp(strData.data, "LMDscandata", 11) == 0)
+					LMDscandata_index = strDatas.size() - 1;
+			}
+			end = i + 1;
+			start = end;
+		}
+	}
+
+	if (strDatas.size() > 26 + LMDscandata_index - 1)
+	{
+		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
+		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
+		start_angle = float(start_angle_l)*0.01;
+
+		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
+		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
+		freq = float(freq_l)*0.0001;
+
+		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
+		long count = Str0x2Long(count_str.data, count_str.length);
+		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
+		{
+			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
+			{
+				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
+				distance.push_back(dis);
+			}
+			return true;
+		}
+	}
+	return false;
+}
+long CSick511FileLaser::Str0x2Long(const char* data, int len)
+{
+	long sum = 0;
+	for (int i = 0; i < len; ++i)
+	{
+		char c = data[i];
+		int n = 0;
+		if (c >= 48 && c <= 57)
+			n = c - 48;
+		else if (c >= 65 && c <= 70)
+			n = c - 65 + 10;
+		sum += n*pow(16, len - i - 1);
+	}
+	return sum;
+}

laser/Sick511FileLaser.h

@@ -0,0 +1,50 @@
+/*************************
+sick 511 �״� �ļ����ݽ���
+*************************/
+#ifndef __SICK_511_LASER_FILE__HH
+#define __SICK_511_LASER_FILE__HH
+#include "Laser.h"
+#include <fstream>
+class CSick511FileLaser :
+	public Laser_base
+{
+public:
+	CSick511FileLaser(int id, Laser_proto::laser_parameter laser_param);
+	virtual ~CSick511FileLaser();
+
+	//雷达链接设备，为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();
+
+protected:
+	virtual bool RecvData(Binary_buf& data);
+	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
+
+protected:
+	int FindHead(char* buf, int b_len);
+	int FindTail(char* buf, int b_len);
+protected:
+	virtual bool GetData(Binary_buf* pData, std::vector<float>& distance,
+		float& freq, float& start_angle);
+	long Str0x2Long(const char* data, int len);
+
+protected:
+	std::ifstream	m_stream_read;
+	std::string		m_file;
+	bool			m_start_read;
+	std::mutex		m_mutex;
+};
+#endif

laser/TcpLaser.cpp

@@ -0,0 +1,343 @@
+#include "TcpLaser.h"
+#include <stdlib.h>
+#include <unistd.h>
+
+RegisterLaser(Tcp);
+CTcpLaser::CTcpLaser(int id, Laser_proto::laser_parameter laser_param)
+	:Laser_base(id,laser_param)
+{
+}
+
+CTcpLaser::~CTcpLaser()
+{
+}
+
+
+
+
+//雷达链接设备，为3个线程添加线程执行函数。
+Error_manager CTcpLaser::connect_laser()
+{
+	std::string ip = m_laser_param.laser_ip();
+	int			port = m_laser_param.laser_port();
+	int			remoteport = m_laser_param.laser_port_remote();
+	if (ip == "" || port < 0)
+		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+							" m_laser_param PARAMRTER ERROR ");
+
+	//��ʼ��Socket
+	//WSAStartup(MAKEWORD(1, 1), &m_wsd);
+	//����Socket����
+	m_socket = socket(AF_INET, SOCK_STREAM, 0);
+	if (m_socket <= 0)
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							 " m_laser_param PARAMRTER ERROR ");
+
+	m_send_addr.sin_family = AF_INET;
+	m_send_addr.sin_addr.s_addr = inet_addr(ip.c_str());
+	m_send_addr.sin_port = htons(remoteport);
+
+	if (0 != ::connect(m_socket, (struct sockaddr *)&m_send_addr, sizeof(struct sockaddr)))
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							 " connect m_socket ERROR ");
+	m_last_data_time = clock();
+	return Laser_base::connect_laser();
+}
+//雷达断开链接，释放3个线程
+Error_manager CTcpLaser::disconnect_laser()
+{
+	if (m_socket > 0)
+	{
+		close(m_socket);
+		//	WSACleanup();
+		m_socket = -1;
+	}
+	return Laser_base::disconnect_laser();
+}
+//对外的接口函数，负责接受并处理任务单，
+//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
+Error_manager CTcpLaser::execute_task(Task_Base* p_laser_task)
+{
+
+}
+//检查雷达状态,是否正常运行
+Error_manager CTcpLaser::check_laser()
+{
+
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager CTcpLaser::start_scan()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (!this->is_ready())
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							"  is_ready ERROR ");
+
+	char* sendMsg = "$SLSSTA*0A\r\n";
+	if (Send(sendMsg, strlen(sendMsg)))
+	{
+		return Laser_base::start_scan();
+	}
+	return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+						" Send error ");
+}
+//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+Error_manager CTcpLaser::stop_scan()
+{
+	char sendMsg[] = "$SLSSTP*1B\r\n";
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (Send(sendMsg, strlen(sendMsg)))
+		return Error_code::SUCCESS;
+	return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+						" error ");
+}
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager CTcpLaser::end_task()
+{
+
+}
+
+
+Buf_type CTcpLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
+{
+	////ƴ���ϴ�δ�������
+
+	Buf_type type = BUF_UNKNOW;
+	Binary_buf frame;
+
+	if (m_last_data.get_length() > 0)
+	{
+		if (pData)
+			frame = m_last_data + (*pData);
+		else
+			frame = m_last_data;
+		m_last_data = Binary_buf();
+	}
+	else if (pData)
+	{
+		frame = (*pData);
+	}
+	else
+	{
+		return type;
+	}
+
+	int head = FindHead(frame.get_buf(), frame.get_length());
+	int tail = FindTail(frame.get_buf(), frame.get_length());
+
+	if (tail >= 0)
+	{
+		if (tail < frame.get_length() - 1)		//���������
+		{
+			m_last_data = Binary_buf(frame.get_buf() + tail + 1, frame.get_length() - tail - 1);
+		}
+		if (head >= 0 && head < tail)
+		{
+			////���
+			Binary_buf data(frame.get_buf() + head, tail - head + 1);
+			if (data.is_equal_front("$SLSSTP") )
+				type = BUF_STOP;
+			else if (data.is_equal_front( "$SLSSTA"))
+				type = BUF_START;
+			else if (data.is_equal_front( "$SCLRDY"))
+				type = BUF_READY;
+			else if (data.is_equal_front( "$SHWERR"))
+				type = BUF_ERROR;
+			else if (data.is_equal_front( "$N"))
+				type = BUF_DATA;
+
+			if (type == BUF_DATA)
+			{
+				////��������
+				points.clear();
+				if (data.get_length() <= 29)
+					return type;
+
+				////����
+				unsigned char angle_1 = 0;
+				unsigned char angle_2 = 0;
+				memcpy(&angle_1, (data.get_buf())+26, 1);
+				memcpy(&angle_2, (data.get_buf())+27, 1);
+				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
+				float start_angle = 0.0;
+				float freq = 0.0;
+
+				std::vector<float> distance;
+				if (GetData(&data, distance, freq, start_angle))
+				{
+					float beta = (beta_a)*DEGREES;
+					float sin_beta = sin(beta);
+					float cos_beta = cos(beta);
+					for (int i = 0; i < distance.size(); ++i)
+					{
+						if (distance[i] < 0.001)
+							continue;
+
+						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
+						float sin_alpha = sin(alpha);
+						float cos_alpha = cos(alpha);
+
+						float x = distance[i] * sin_alpha;
+						float y = distance[i] * cos_alpha * sin_beta;
+						float z = distance[i] * cos_alpha * cos_beta;
+
+						points.push_back(CPoint3D(x, y, z));
+					}
+					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
+				}
+
+			}
+		}
+	}
+	else if (head >= 0)
+	{
+		m_last_data = Binary_buf(frame.get_buf() + head, frame.get_length() - head);
+	}
+
+	return type;
+}
+
+bool CTcpLaser::RecvData(Binary_buf& data)
+{
+	char buf[4096] = { 0 };
+	int bytesRead = Recv(buf, 4096);
+	if (bytesRead > 0)
+	{
+		Binary_buf bin_data(buf, bytesRead);
+		data = bin_data;
+		m_last_data_time = clock();
+		return true;
+	}
+	return false;
+}
+
+int CTcpLaser::FindHead(char* buf, int b_len)
+{
+	int i = 0;
+	if (b_len < 2)
+		return NULL;
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len > 10)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
+				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
+				return i;
+		}
+		if (b_len > 7)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7] == '*')
+				return i;
+		}
+	}
+	return -1;
+}
+
+int CTcpLaser::FindTail(char* buf, int b_len)
+{
+	int i = 0;
+
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len >= i + 5)
+		{
+			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
+				return i + 4;
+		}
+	}
+	return -9999999;
+}
+
+bool CTcpLaser::Send(char* sendbuf, int len)
+{
+	int ret = 0;
+	do
+	{
+        ret = send(m_socket, sendbuf, strlen(sendbuf), 0);
+	} while (ret < 0);
+	return ret == len;
+}
+int CTcpLaser::Recv(char* recvbuf, int len)
+{
+	struct sockaddr_in sddr_from;
+	int ret = 0;
+	do
+	{
+        ret = recv(m_socket, recvbuf, 4096, 0);
+	} while (ret < 0);
+	return ret;
+}
+
+bool CTcpLaser::GetData(Binary_buf* pData, std::vector<float>& distance,
+	float& freq, float& start_angle)
+{
+	struct stData
+	{
+		const char* data;
+		int length;
+	};
+	std::vector<struct stData> strDatas;
+	int start = 0;
+	int end = 0;
+	int LMDscandata_index = -1;
+	for (int i = 0; i < pData->get_length(); ++i)
+	{
+		if ((*pData)[i] == ' ')
+		{
+			end = i;
+			if (end > start)
+			{
+				struct stData strData;
+//				strData.data = (*pData + start);
+				strData.data = (pData->get_buf() + start);
+				strData.length = end - start;
+
+				strDatas.push_back(strData);
+				if (strncmp(strData.data, "LMDscandata", 11) == 0)
+					LMDscandata_index = strDatas.size() - 1;
+			}
+			end = i + 1;
+			start = end;
+		}
+	}
+
+	if (strDatas.size() > 26 + LMDscandata_index - 1)
+	{
+		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
+		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
+		start_angle = float(start_angle_l)*0.01;
+
+		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
+		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
+		freq = float(freq_l)*0.0001;
+
+		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
+		long count = Str0x2Long(count_str.data, count_str.length);
+		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
+		{
+			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
+			{
+				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
+				distance.push_back(dis);
+			}
+			return true;
+		}
+	}
+	return false;
+}
+long CTcpLaser::Str0x2Long(const char* data, int len)
+{
+	long sum = 0;
+	for (int i = 0; i < len; ++i)
+	{
+		char c = data[i];
+		int n = 0;
+		if (c >= 48 && c <= 57)
+			n = c - 48;
+		else if (c >= 65 && c <= 70)
+			n = c - 65 + 10;
+		sum += n*pow(16, len - i - 1);
+	}
+	return sum;
+}

laser/TcpLaser.h

@@ -0,0 +1,64 @@
+/*************************
+sick 511 �״� tcpЭ�����ӿ��ư�
+*************************/
+
+#pragma once
+#include "Laser.h"
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <mutex>
+
+#define LASER_DATA_TIME_OUT		10000
+
+class CTcpLaser :
+	public Laser_base
+{
+public:
+	CTcpLaser(int id, Laser_proto::laser_parameter laser_param);
+	~CTcpLaser();
+
+	//雷达链接设备，为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();
+
+protected:
+	virtual bool RecvData(Binary_buf& data);
+	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
+
+
+private:
+	bool	Send(char* sendbuf, int len);                          // ����ָ��
+	int		Recv(char* recvbuf, int len);                           // ��������
+	static int FindHead(char* buf, int b_len);
+	static int FindTail(char* buf, int b_len);
+private:
+	bool GetData(Binary_buf* pData, std::vector<float>& distance,
+		float& freq, float& start_angle);
+	long Str0x2Long(const char* data, int len);
+
+private:
+#pragma region udp��ر���
+	int				m_socket;
+    //WSADATA			m_wsd;
+	struct sockaddr_in m_send_addr;	
+	std::mutex		m_mutex;
+	clock_t			m_last_data_time;
+#pragma endregion
+
+};
+

laser/UdpLaser.cpp

@@ -0,0 +1,352 @@
+
+#include "UdpLaser.h"
+#include <stdlib.h>
+#include <unistd.h>
+
+RegisterLaser(Udp);
+CUdpLaser::CUdpLaser(int id, Laser_proto::laser_parameter laser_param)
+	:Laser_base(id,laser_param)
+{
+}
+
+
+CUdpLaser::~CUdpLaser()
+{
+}
+
+
+
+//雷达链接设备，为3个线程添加线程执行函数。
+Error_manager CUdpLaser::connect_laser()
+{
+	std::string ip = m_laser_param.laser_ip();
+	int			port = m_laser_param.laser_port();
+	int			remoteport = m_laser_param.laser_port_remote();
+	if (ip == "" || port < 0)
+		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
+							" m_laser_param PARAMRTER_ERROR ");
+
+	//��ʼ��Socket
+	//WSAStartup(MAKEWORD(1, 1), &m_wsd);
+	//����Socket����
+	m_socket = socket(AF_INET, SOCK_DGRAM, 0);
+	if (m_socket <= 0)
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							" m_socket PARAMRTER_ERROR ");
+
+	struct sockaddr_in sddr_udp;
+	sddr_udp.sin_family = AF_INET;
+	sddr_udp.sin_addr.s_addr = htons(INADDR_ANY);
+	sddr_udp.sin_port = htons(port);
+
+	m_send_addr.sin_family = AF_INET;
+	m_send_addr.sin_addr.s_addr = inet_addr(ip.c_str());
+	m_send_addr.sin_port = htons(remoteport);
+
+	if(-1==bind(m_socket, (struct sockaddr *)&sddr_udp, sizeof(struct sockaddr)))
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							" bind m_socket  ERROR ");
+
+	return Laser_base::connect_laser();
+}
+//雷达断开链接，释放3个线程
+Error_manager CUdpLaser::disconnect_laser()
+{
+	if (m_socket > 0)
+	{
+
+		close(m_socket);
+		//WSACleanup();
+		m_socket = -1;
+	}
+	return Laser_base::disconnect_laser();
+}
+//对外的接口函数，负责接受并处理任务单，
+//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
+//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
+Error_manager CUdpLaser::execute_task(Task_Base* p_laser_task)
+{
+
+}
+//检查雷达状态,是否正常运行
+Error_manager CUdpLaser::check_laser()
+{
+
+}
+//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
+Error_manager CUdpLaser::start_scan()
+{
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (!this->is_ready())
+		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+							"is_ready error ");
+
+	char* sendMsg = "$SLSSTA*0A\r\n";
+	if (Send(sendMsg, strlen(sendMsg)))
+	{
+		m_laser_statu = LASER_BUSY;
+
+		return Laser_base::start_scan();
+	}
+	return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+						" Send error ");
+}
+//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
+Error_manager CUdpLaser::stop_scan()
+{
+	char sendMsg[] = "$SLSSTP*1B\r\n";
+	std::lock_guard<std::mutex> lk(m_mutex);
+	if (Send(sendMsg, strlen(sendMsg)))
+		return Error_code::SUCCESS;
+	return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
+						" Send error ");
+}
+//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
+Error_manager CUdpLaser::end_task()
+{
+
+}
+
+
+Buf_type CUdpLaser::Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points)
+{
+	////ƴ���ϴ�δ�������
+
+	Buf_type type = BUF_UNKNOW;
+	Binary_buf frame;
+
+	if (m_last_data.get_length() > 0)
+	{
+		if (pData)
+			frame = m_last_data + (*pData);
+		else
+			frame = m_last_data;
+		m_last_data = Binary_buf();
+	}
+	else if (pData)
+	{
+		frame = (*pData);
+	}
+	else
+	{
+		return type;
+	}
+
+	int head = FindHead(frame.get_buf(), frame.get_length());
+	int tail = FindTail(frame.get_buf(), frame.get_length());
+	if (tail >= 0)
+	{
+		if (tail < frame.get_length() - 1)		//���������
+		{
+			m_last_data = Binary_buf(frame.get_buf() + tail + 1, frame.get_length() - tail - 1);
+		}
+		if (head >= 0 && head < tail)
+		{
+			////���
+			Binary_buf data(frame.get_buf() + head, tail - head + 1);
+			if (data.is_equal_front( "$SLSSTP"))
+				type = BUF_STOP;
+			else if (data.is_equal_front( "$SLSSTA"))
+				type = BUF_START;
+			else if (data.is_equal_front( "$SCLRDY"))
+				type = BUF_READY;
+			else if (data.is_equal_front( "$SHWERR"))
+				type = BUF_ERROR;
+			else if (data.is_equal_front( "$N"))
+				type = BUF_DATA;
+
+			if (type == BUF_DATA)
+			{
+				////��������
+				points.clear();
+				if (data.get_length() <= 29)
+					return type;
+
+				////����
+				unsigned char angle_1 = 0;
+				unsigned char angle_2 = 0;
+				memcpy(&angle_1, (data.get_buf())+26, 1);
+				memcpy(&angle_2, (data.get_buf())+27, 1);
+				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
+				float start_angle = 0.0;
+				float freq = 0.0;
+
+				std::vector<float> distance;
+				if (GetData(&data, distance, freq, start_angle))
+				{
+					
+					float beta = (beta_a)*DEGREES;
+					float sin_beta = sin(beta);
+					float cos_beta = cos(beta);
+					for (int i = 0; i < distance.size(); ++i)
+					{
+						if (distance[i] < 0.001)
+							continue;
+
+						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
+						float sin_alpha = sin(alpha);
+						float cos_alpha = cos(alpha);
+
+						float x = distance[i] * sin_alpha;
+						float y = distance[i] * cos_alpha * sin_beta;
+						float z = distance[i] * cos_alpha * cos_beta;
+
+						points.push_back(CPoint3D(x, y, z));
+					}
+					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
+				}
+
+			}
+		}
+	}
+	else if (head >= 0)
+	{
+		m_last_data = Binary_buf(frame.get_buf() + head, frame.get_length() - head);
+	}
+	
+	return type;
+}
+
+bool CUdpLaser::RecvData(Binary_buf& data)
+{
+	char buf[4096 * 10] = { 0 };
+    int bytesRead = Recv(buf, 4096);
+	if (bytesRead > 0)
+	{
+		Binary_buf bin_data(buf, bytesRead);
+		data = bin_data;
+		return true;
+	}
+	return false;
+}
+
+int CUdpLaser::FindHead(char* buf, int b_len)
+{
+	int i = 0;
+	if (b_len < 2)
+		return NULL;
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len > 10)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
+				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
+				return i;
+		}
+		if (b_len >7)
+		{
+			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7]=='*')
+				return i;
+		}
+	}
+	return -1;
+}
+
+int CUdpLaser::FindTail(char* buf, int b_len)
+{
+	int i = 0;
+	
+	for (i = 0; i <= b_len; i++)
+	{
+		if (b_len >=i+ 5)
+		{
+			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
+				return i+4;
+		}
+	}
+	return -9999999;
+}
+
+bool CUdpLaser::Send(char* sendbuf, int len)
+{
+	int ret = 0;
+	do 
+	{
+        ret = sendto(m_socket, sendbuf, strlen(sendbuf), 0, (struct sockaddr*)&m_send_addr, sizeof(struct sockaddr));
+	} while (ret < 0);
+	return ret == len;
+}
+int CUdpLaser::Recv(char* recvbuf, int len)
+{
+	struct sockaddr_in sddr_from;
+    socklen_t clientLen = sizeof(sddr_from);
+	int ret = 0;
+	do
+	{
+        ret = recvfrom(m_socket, recvbuf, 4096, 0, (struct sockaddr*)&sddr_from, &clientLen);
+	} while (ret < 0);
+	return ret;
+}
+
+bool CUdpLaser::GetData(Binary_buf* pData, std::vector<float>& distance,
+	float& freq, float& start_angle)
+{
+	struct stData
+	{
+		const char* data;
+		int length;
+	};
+	std::vector<struct stData> strDatas;
+	int start = 0;
+	int end = 0;
+	int LMDscandata_index = -1;
+	for (int i = 0; i < pData->get_length(); ++i)
+	{
+		if ((*pData)[i] == ' ')
+		{
+			end = i;
+			if (end > start)
+			{
+				struct stData strData;
+//				strData.data = (*pData + start);
+				strData.data = (pData->get_buf() + start);
+				strData.length = end - start;
+				
+				strDatas.push_back(strData);
+				if (strncmp(strData.data, "LMDscandata", 11) == 0)
+					LMDscandata_index = strDatas.size() - 1;
+			}
+			end = i + 1;
+			start = end;
+		}
+	}
+
+	if (strDatas.size() > 26 + LMDscandata_index - 1)
+	{
+		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
+		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
+		start_angle = float(start_angle_l)*0.01;
+
+		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
+		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
+		freq = float(freq_l)*0.0001;
+
+		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
+		long count = Str0x2Long(count_str.data, count_str.length);
+		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
+		{
+			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
+			{
+				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
+				distance.push_back(dis);
+			}
+			return true;
+		}
+	}
+	return false;
+}
+long CUdpLaser::Str0x2Long(const char* data, int len)
+{
+	long sum = 0;
+	for (int i = 0; i < len; ++i)
+	{
+		char c = data[i];
+		int n = 0;
+		if (c >= 48 && c <= 57)
+			n = c - 48;
+		else if (c >= 65 && c <= 70)
+			n = c - 65 + 10;
+		sum += n*pow(16, len - i - 1);
+	}
+	return sum;
+}

laser/UdpLaser.h

@@ -0,0 +1,57 @@
+/*************************
+sick 511 �״� updЭ�����ӿ��ư�
+*************************/
+#pragma once
+#include "Laser.h"
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <mutex>
+class CUdpLaser : public Laser_base
+{
+public:
+	CUdpLaser(int id, Laser_proto::laser_parameter laser_param);
+	virtual ~CUdpLaser();
+
+	//雷达链接设备，为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();
+
+protected:
+	virtual bool RecvData(Binary_buf& data);
+	virtual Buf_type Data2PointXYZ(Binary_buf* pData, std::vector<CPoint3D>& points);
+	
+
+private:
+	bool Send(char* sendbuf, int len);                          // ����ָ��
+	int Recv(char* recvbuf, int len);                           // ��������
+	static int FindHead(char* buf, int b_len);
+	static int FindTail(char* buf, int b_len);
+private:
+	bool GetData(Binary_buf* pData, std::vector<float>& distance,
+		float& freq, float& start_angle);
+	long Str0x2Long(const char* data, int len);
+
+protected:
+#pragma region udp��ر���
+	int				m_socket;
+//	WSADATA			m_wsd;
+	struct sockaddr_in m_send_addr;
+	std::mutex		m_mutex;
+#pragma endregion
+};
+

main.cpp

@@ -35,19 +35,22 @@ bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
 	return success;
 }
 
+bool fun(int a)
+{
+	if ( a==2 )
+	{
+		return true;
+	}
+
+}
 
 int main(int argc,char* argv[])
 {
-	char * asd = "asd";
-	char * zxc = "zxc";
-	char * sdf = "asd";
-	const char * q=NULL;
-	char * p=NULL;
-	cout << (strncmp(asd, sdf, 3) == 0) << endl;
-	cout << (strncmp(asd, zxc, 3) == 0) << endl;
-	cout << (strncmp(asd, q, 3) == 0) << endl;
-	cout << (strncmp(p, sdf, 3) == 0) << endl;
-	cout << (strncmp(p, q, 3) == 0) << endl;
+int a=2;
+cout << fun(a) << endl;
+	 a=1;
+	cout << fun(a) << endl;
+	return 0;
 
 
 
@@ -75,7 +78,7 @@ int main(int argc,char* argv[])
 
 	if (m_laser_vector != NULL)
 	{
-		if (!m_laser_vector->Connect())
+		if ( m_laser_vector->connect_laser() != Error_code::SUCCESS)
 		{
 			char description[255] = {0};
 			sprintf(description, "Laser %d connect failed...", i);

tool/binary_buf.cpp

@@ -2,7 +2,7 @@
 /*
  * binary_buf是二进制缓存
  * 这里用字符串，来存储雷达的通信消息的原始数据
- * binary_buf 的内容格式：消息类型 + 消息数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
  *
  * 例如思科的雷达的消息类型
  * ready->ready->start->data->data->data->stop->ready->ready
@@ -18,13 +18,13 @@
 #include <string>
 #include <string.h>
 
-binary_buf::binary_buf()
+Binary_buf::Binary_buf()
 {
 	mp_buf = NULL;
 	m_length = 0;
 }
 
-binary_buf::binary_buf(const binary_buf& other)
+Binary_buf::Binary_buf(const Binary_buf& other)
 {
 	mp_buf = NULL;
 	m_length = 0;
@@ -37,7 +37,7 @@ binary_buf::binary_buf(const binary_buf& other)
 	}
 }
 
-binary_buf::~binary_buf()
+Binary_buf::~Binary_buf()
 {
 	if ( mp_buf )
 	{
@@ -49,7 +49,7 @@ binary_buf::~binary_buf()
 
 
 //使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
-binary_buf::binary_buf(const char* p_buf, int len)
+Binary_buf::Binary_buf(const char* p_buf, int len)
 {
 	mp_buf = NULL;
 	m_length = 0;
@@ -68,7 +68,7 @@ binary_buf::binary_buf(const char* p_buf, int len)
 }
 
 //重载=，深拷贝，
-binary_buf& binary_buf::operator=(const binary_buf& other)
+Binary_buf& Binary_buf::operator=(const Binary_buf& other)
 {
 	clear();
 
@@ -82,7 +82,7 @@ binary_buf& binary_buf::operator=(const binary_buf& other)
 }
 
 //重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
-binary_buf& binary_buf::operator=(const char* p_buf)
+Binary_buf& Binary_buf::operator=(const char* p_buf)
 {
 	clear();
 
@@ -97,7 +97,7 @@ binary_buf& binary_buf::operator=(const char* p_buf)
 }
 
 //重载+，other追加在this的后面，
-binary_buf& binary_buf::operator+(binary_buf& other)
+Binary_buf& Binary_buf::operator+(Binary_buf& other)
 {
 	if (other.mp_buf != NULL && other.m_length > 0)
 	{
@@ -113,7 +113,7 @@ binary_buf& binary_buf::operator+(binary_buf& other)
 }
 
 //重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
-binary_buf& binary_buf::operator+(const char* p_buf)
+Binary_buf& Binary_buf::operator+(const char* p_buf)
 {
 	if (p_buf != NULL )
 	{
@@ -130,7 +130,7 @@ binary_buf& binary_buf::operator+(const char* p_buf)
 }
 
 //重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
-char& binary_buf::operator[](int n)
+char& Binary_buf::operator[](int n)
 {
 	if (n >= 0 && n < m_length)
 	{
@@ -144,7 +144,7 @@ char& binary_buf::operator[](int n)
 
 
 //判空
-bool binary_buf::is_empty()
+bool Binary_buf::is_empty()
 {
 	if ( mp_buf != NULL && m_length > 0 )
 	{
@@ -157,7 +157,7 @@ bool binary_buf::is_empty()
 }
 
 //清空
-void binary_buf::clear()
+void Binary_buf::clear()
 {
 	if ( mp_buf )
 	{
@@ -169,7 +169,7 @@ void binary_buf::clear()
 
 
 //比较前面部分的buf是否相等，使用 other.m_length 为标准
-bool binary_buf::is_equal_front(const binary_buf& other)
+bool Binary_buf::is_equal_front(const Binary_buf& other)
 {
 	if ( other.mp_buf == NULL || other.m_length <= 0 )
 	{
@@ -201,7 +201,7 @@ bool binary_buf::is_equal_front(const binary_buf& other)
 }
 
 //比较前面部分的buf是否相等，len为0时，使用strlen(buf)为标准，不比较结束符'\0'
-bool binary_buf::is_equal_front(const char* p_buf, int len)
+bool Binary_buf::is_equal_front(const char* p_buf, int len)
 {
 	if ( p_buf == NULL )
 	{
@@ -237,7 +237,7 @@ bool binary_buf::is_equal_front(const char* p_buf, int len)
 }
 
 //比较的buf是否全部相等，
-bool binary_buf::is_equal_all(const binary_buf& other)
+bool Binary_buf::is_equal_all(const Binary_buf& other)
 {
 	if ( other.mp_buf == NULL || other.m_length <= 0 )
 	{
@@ -268,7 +268,7 @@ bool binary_buf::is_equal_all(const binary_buf& other)
 	}
 }
 //比较的buf是否全部相等，不比较结束符'\0'
-bool binary_buf::is_equal_all(const char* p_buf)
+bool Binary_buf::is_equal_all(const char* p_buf)
 {
 	if ( p_buf == NULL )
 	{
@@ -303,12 +303,12 @@ bool binary_buf::is_equal_all(const char* p_buf)
 
 
 
-char*	binary_buf::get_buf()const
+char*	Binary_buf::get_buf()const
 {
 	return mp_buf;
 }
 
-int		binary_buf::get_length()const
+int		Binary_buf::get_length()const
 {
 	return m_length;
 }

tool/binary_buf.h

@@ -2,7 +2,7 @@
 /*
  * binary_buf是二进制缓存
  * 这里用字符串，来存储雷达的通信消息的原始数据
- * binary_buf 的内容格式：消息类型 + 消息数据
+ * Binary_buf 的内容格式：消息类型 + 消息数据
  *
  * 例如思科的雷达的消息类型
  * ready->ready->start->data->data->data->stop->ready->ready
@@ -17,26 +17,38 @@
 #define LIDARMEASURE_BINARY_BUF_H
 
 
-//#include "../error_code/error_code.h"
+//雷达消息的类型
+//在通信消息的前面一部分字符串，表示这条消息的类型。
+//在解析消息的时候，先解析前面的消息类型，来判断这条消息的功用
+enum Buf_type
+{//默认值 BUF_UNKNOW = 0
+	BUF_UNKNOW   		=0,	//未知消息
+	BUF_READY  			=1,	//待机消息
+	BUF_START 			=2,	//开始消息
+	BUF_DATA   			=3,	//数据消息
+	BUF_STOP  			=4,	//结束消息
+	BUF_ERROR   		=5,	//错误消息
+};
+
 
 //二进制缓存，
-class binary_buf
+class Binary_buf
 {
 public:
-	binary_buf();
-	binary_buf(const binary_buf& other);
-	~binary_buf();
+	Binary_buf();
+	Binary_buf(const Binary_buf& other);
+	~Binary_buf();
 
 	//使用参数构造，深拷贝，len为0时，使用strlen(buf)，不存储结束符'\0'
-	binary_buf(const char* p_buf, int len = 0);
+	Binary_buf(const char* p_buf, int len = 0);
 	//重载=，深拷贝，
-	binary_buf& operator=(const binary_buf& other);
+	Binary_buf& operator=(const Binary_buf& other);
 	//重载=，深拷贝，使用strlen(buf)，不存储结束符'\0'
-	binary_buf& operator=(const char* p_buf);
+	Binary_buf& operator=(const char* p_buf);
 	//重载+，other追加在this的后面，
-	binary_buf& operator+(binary_buf& other);
+	Binary_buf& operator+(Binary_buf& other);
 	//重载+，追加在this的后面，使用strlen(buf)，不存储结束符'\0'
-	binary_buf& operator+(const char* p_buf);
+	Binary_buf& operator+(const char* p_buf);
 	//重载[]，允许直接使用数组的形式，直接访问buf的内存。注意，n值必须在0～m_length之间，
 	char& operator[](int n);
 
@@ -46,24 +58,24 @@ public:
 	void clear();
 
 	//比较前面部分的buf是否相等，使用 other.m_length 为标准
-	bool is_equal_front(const binary_buf& other);
+	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);
 
 	//比较的buf是否全部相等，
-	bool is_equal_all(const binary_buf& other);
+	bool is_equal_all(const Binary_buf& other);
 	//比较的buf是否全部相等，不比较结束符'\0'
 	bool is_equal_all(const char* p_buf);
 
 
 
 public:
-	char*		get_buf()const;
-	int			get_length()const;
+	char* get_buf()const;
+	int	get_length()const;
 
 protected:
-	char*			mp_buf;				//二进制缓存指针
-	int				m_length;			//二进制缓存长度
+	char*		mp_buf;				//二进制缓存指针
+	int			m_length;			//二进制缓存长度
 
 private:
 

tool/thread_condition.cpp

@@ -13,7 +13,7 @@
  * 如果只想要线程执行一次，那就使用 notify_all(false, true)
  * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
  *
- * m_kill //是否杀死线程，让线程强制退出，
+ * m_kill_flag //是否杀死线程，让线程强制退出，
  * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
 	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
 	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
@@ -27,13 +27,13 @@
 
 Thread_condition::Thread_condition()
 {
-	m_kill = false;
+	m_kill_flag = false;
 	m_pass_ever = false;
 	m_pass_once = false;
 }
 Thread_condition::~Thread_condition()
 {
-
+	kill_all();
 }
 
 //无限等待，由 is_pass_wait 决定是否阻塞。
@@ -49,7 +49,7 @@ bool Thread_condition::wait()
 //等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
 //return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
 //注意了：线程阻塞期间，是不会return的。
-bool Thread_condition::wait_for(unsigned int millisecond)
+bool Thread_condition::wait_for_millisecond(unsigned int millisecond)
 {
 	std::unique_lock<std::mutex> loc(m_mutex);
 	m_condition_variable.wait_for(loc, std::chrono::milliseconds(millisecond), std::bind(is_pass_wait, this));
@@ -57,18 +57,7 @@ bool Thread_condition::wait_for(unsigned int millisecond)
 	m_pass_once = false;
 	return t_pass;
 }
-//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
-//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
-//注意了：线程阻塞期间，是不会return的。
-template<typename _Rep, typename _Period>
-bool Thread_condition::wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration)
-{
-	std::unique_lock<std::mutex> loc(m_mutex);
-	m_condition_variable.wait_for(loc, std::chrono::duration<_Rep, _Period>(time_duration), std::bind(is_pass_wait, this));
-	bool t_pass = is_pass_wait(this);
-	m_pass_once = false;
-	return t_pass;
-}
+
 
 //唤醒已经阻塞的线程，唤醒一个线程
 //pass_ever 或者 pass_once 为真时，才能唤醒线程。都为假时，线程进入等待。
@@ -98,20 +87,20 @@ void Thread_condition::notify_all(bool pass_ever, bool pass_once)
 void Thread_condition::kill_all()
 {
 	std::unique_lock<std::mutex> loc(m_mutex);
-	m_kill = true;
+	m_kill_flag = true;
 	m_condition_variable.notify_all();
 }
 
-//判断是否或者，return !m_kill
+//判断是否或者，return !m_kill_flag
 bool Thread_condition::is_alive()
 {
-	return !m_kill;
+	return !m_kill_flag;
 }
 
 
-bool Thread_condition::get_kill()
+bool Thread_condition::get_kill_flag()
 {
-	return m_kill;
+	return m_kill_flag;
 }
 bool Thread_condition::get_pass_ever()
 {
@@ -121,9 +110,9 @@ bool Thread_condition::get_pass_once()
 {
 	return m_pass_once;
 }
-void Thread_condition::set_kill(bool kill)
+void Thread_condition::set_kill_flag(bool kill)
 {
-	m_kill = kill;
+	m_kill_flag = kill;
 }
 void Thread_condition::set_pass_ever(bool pass_ever)
 {
@@ -135,7 +124,7 @@ void Thread_condition::set_pass_once(bool pass_once)
 }
 void Thread_condition::reset(bool kill, bool pass_ever, bool pass_once)
 {
-	m_kill = kill;
+	m_kill_flag = kill;
 	m_pass_ever = pass_ever;
 	m_pass_once = pass_once;
 }
@@ -150,6 +139,6 @@ bool Thread_condition::is_pass_wait(Thread_condition * other)
 		throw (other);
 		return false;
 	}
-	return (other->m_kill || other->m_pass_ever || other->m_pass_once);
+	return (other->m_kill_flag || other->m_pass_ever || other->m_pass_once);
 }
 

tool/thread_condition.h

@@ -13,7 +13,7 @@
  * 如果只想要线程执行一次，那就使用 notify_all(false, true)
  * 注：notify_all(false, true)和notify_one(false, true) 一样，只能让其中一个线程执行一次
  *
- * m_kill //是否杀死线程，让线程强制退出，
+ * m_kill_flag //是否杀死线程，让线程强制退出，
  * 外部调用 kill_all() 函数，可以直接通知线程自动退出。
 	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
 	//唤醒所有线程，使其通过等待，但是不能运行功能函数，必须直接return
@@ -47,7 +47,7 @@ public:
 	//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
 	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
 	//注意了：线程阻塞期间，是不会return的。
-	bool wait_for(unsigned int millisecond);
+	bool wait_for_millisecond(unsigned int millisecond);
 
 	//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
 	//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
@@ -69,15 +69,15 @@ public:
 	//		通过等待之后，也要检查kill的状态，如果为真，那么就不能执行功能函数，应该直接return
 	void kill_all();
 
-	//判断是否或者，return !m_kill
+	//判断是否或者，return !m_kill_flag
 	bool is_alive();
 
 public:
 
-	bool get_kill();
+	bool get_kill_flag();
 	bool get_pass_ever();
 	bool get_pass_once();
-	void set_kill(bool kill);
+	void set_kill_flag(bool kill);
 	void set_pass_ever(bool pass_ever);
 	void set_pass_once(bool pass_once);
 	void reset(bool kill = false, bool pass_ever = false, bool pass_once = false);
@@ -87,15 +87,28 @@ protected:
 	//注：m_kill或者m_pass为真时，return true
 	static bool is_pass_wait(Thread_condition * other);
 
-	std::atomic<bool> m_kill;			//是否杀死线程，让线程强制退出，
-	std::atomic<bool> m_pass_ever;		//线程能否直接通过等待，对后面的线程也生效。
-	std::atomic<bool> m_pass_once;		//线程能否直接通过等待，一次（通过一次之后，wait里面自动改为false）
+	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;		//线程的条件变量
+	std::mutex 				m_mutex;				//线程的锁
+	std::condition_variable m_condition_variable;	//线程的条件变量
 
 private:
 
 };
 
+//等待一定的时间（时间单位可调），由 is_pass_wait 决定是否阻塞。
+//return：is_pass_wait的结果，	true：线程直接通过等待，false：线程超时了，然后通过等待。
+//注意了：线程阻塞期间，是不会return的。
+template<typename _Rep, typename _Period>
+bool Thread_condition::wait_for_ex(const std::chrono::duration<_Rep, _Period>& time_duration)
+{
+	std::unique_lock<std::mutex> loc(m_mutex);
+	m_condition_variable.wait_for(loc, std::chrono::duration<_Rep, _Period>(time_duration), std::bind(is_pass_wait, this));
+	bool t_pass = is_pass_wait(this);
+	m_pass_once = false;
+	return t_pass;
+}
+
 #endif //LIDARMEASURE_THREAD_CONDITION_H

tool/thread_safe_queue.cpp

@@ -1,7 +1,7 @@
 
 
 /*
- * （1）这个实现要求构建工具支持C++11的atomic mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
+ * （1）这个实现要求构建工具支持C++11的atomic std::mutex condition_veriable功能。这是C++11的基础特性，一般2011年以后的C++编译器都能支持。 例如，visual studio 2012以上。
 
 （2）这个类的实现中有两处使用了unique_lock而不是lock_guard，这是data_cond.wait所需要的，unique_lock是lock_guard的增强版。
 
@@ -11,197 +11,24 @@
 
 
 原文链接：https://blog.csdn.net/weixin_41855721/article/details/81703818
- 增加了一些功能函数，
- 补充了注释说明
+引用了他的思路，增加了一些功能函数， 补充了注释说明
 
- * */
-
-#include "thread_safe_queue.h"
-
-using namespace std;
+ termination_queue
+ 	// 在退出状态下，所有的功能函数不可用，返回false或者null。
+	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
 
-template<typename T>
-Thread_safe_queue<T>::Thread_safe_queue()
-{
-	m_termination = false;
-}
-template<typename T>
-Thread_safe_queue<T>::Thread_safe_queue(const Thread_safe_queue& other)
-{
-	std::lock_guard<std::mutex> lock_this(m_mutex);
-	std::lock_guard<std::mutex> lock_other(other.m_mutex);
-	m_data_queue = other.data_queue;
-	m_termination = other.m_termination;
-}
-template<typename T>
-Thread_safe_queue<T>::~Thread_safe_queue()
-{
-	//析构时，终止队列，让线程通过等待，方便线程推出。
-	termination_queue();
-}
+ pop系列函数
+	//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
 
-//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
-//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
-//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
 
-//等待并弹出数据，成功弹出则返回true
-// 队列为空则无限等待，termination终止队列，则返回false
-template<typename T>
-bool Thread_safe_queue<T>::wait_and_pop(T& value)
-{
-	if ( m_termination )
-	{
-		return false;
-	}
-	else
-	{
-		unique_lock<mutex> lk(m_mutex);
-		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
-		m_data_cond.wait(lk, [this]
-		{ return ((!m_data_queue.empty()) || m_termination); });
-		if (m_termination)
-		{
-			return false;
-		}
-		else
-		{
-			value = move(*m_data_queue.front());
-			m_data_queue.pop();
-			return true;
-		}
-	}
-}
-//尝试弹出数据，成功弹出则返回true
-//队列为空 或者 termination终止队列，返回false
-template<typename T>
-bool Thread_safe_queue<T>::try_pop(T& value)
-{
-	if ( m_termination )
-	{
-		return false;
-	}
-	else
-	{
-		lock_guard<mutex> lk(m_mutex);
-		if (m_data_queue.empty())
-		{
-			return false;
-		}
-		else
-		{
-			value = move(*m_data_queue.front());
-			m_data_queue.pop();
-			return true;
-		}
-	}
-}
-//等待并弹出数据，成功弹出则返回true
-// 队列为空则无限等待，termination终止队列，则返回false
-template<typename T>
-std::shared_ptr<T> Thread_safe_queue<T>::wait_and_pop()
-{
-	if ( m_termination )
-	{
-		return NULL;
-	}
-	else
-	{
-		unique_lock<mutex> lk(m_mutex);
-		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
-		m_data_cond.wait(lk, [this]
-		{ return ((!m_data_queue.empty()) || m_termination); });
-		if (m_termination)
-		{
-			return NULL;
-		}
-		else
-		{
-			shared_ptr<T> res = m_data_queue.front();
-			m_data_queue.pop();
-			return res;
-		}
-	}
-}
-//尝试弹出数据，成功弹出则返回true
-//队列为空 或者 termination终止队列，返回false
-template<typename T>
-std::shared_ptr<T> Thread_safe_queue<T>::try_pop()
-{
-	if ( m_termination )
-	{
-		return NULL;
-	}
-	else
-	{
-		lock_guard<mutex> lk(m_mutex);
-		if (m_data_queue.empty())
-		{
-			return NULL;
-		}
-		else
-		{
-			shared_ptr<T> res = m_data_queue.front();
-			m_data_queue.pop();
-			return res;
-		}
-	}
-}
-//插入一项，并唤醒一个线程，
-//如果成功插入，则返回true，  失败则返回false
-//注：只能唤醒一个线程，防止多线程误判empty()
-template<typename T>
-bool Thread_safe_queue<T>::push(T new_value)
-{
-	if (m_termination)
-	{
-		return false;
-	}
-	else
-	{
-		shared_ptr<T> data(make_shared<T>(move(new_value)));
-		lock_guard<mutex> lk(m_mutex);
-		m_data_queue.push(data);
-		m_data_cond.notify_one();
-		return true;
-	}
-}
-
-//判空
-template<typename T>
-bool Thread_safe_queue<T>::empty()
-{
-	lock_guard<mutex> lk(m_mutex);
-	return m_data_queue.empty();
-}
-//获取队列大小
-template<typename T>
-size_t Thread_safe_queue<T>::size()
-{
-	lock_guard<mutex> lk(m_mutex);
-	return m_data_queue.size();
-}
-//设置队列为退出状态。
-// 在退出状态下，所有的功能函数不可用，返回false或者null。
-// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
-template<typename T>
-void Thread_safe_queue<T>::termination_queue()
-{
-	lock_guard<mutex> lk(m_mutex);
-	m_termination = true;
-	m_data_cond.notify_all();
-}
-//获取退出状态
-template<typename T>
-bool Thread_safe_queue<T>::get_termination()
-{
-	return m_termination;
-}
-//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
-template<typename T>
-bool Thread_safe_queue<T>::is_pass()
-{
-	return (!m_data_queue.empty() || m_termination);
-}
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif
+ 然后在调用方include包含cpp文件，但是这样不好。
+ * */
 
+#include "thread_safe_queue.h"
 
 

tool/thread_safe_queue.h

@@ -23,6 +23,12 @@
 	//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
 	//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
 
+注注注注注意了：模板类不支持分离编译。 模板类的实现必须放在头文件
+ 为了方便阅读和编程规范，依然将声明和实现分开，就像是把cpp文件的代码复制到h文件的尾部。
+
+ 如果将实现放到cpp里面，那么就要为cpp文件加 ifndef define endif 防止重定义。
+ 然后在调用方include包含cpp文件，但是这样不好。
+
 
  * */
 
@@ -35,9 +41,8 @@
 #include <mutex>
 #include <condition_variable>
 
-using namespace std;
 
-template<typename T>
+template<class T>
 class Thread_safe_queue
 {
 public:
@@ -65,25 +70,28 @@ public:
 	//如果成功插入，则返回true，  失败则返回false
 	//注：只能唤醒一个线程，防止多线程误判empty()
 	bool push(T new_value);
+	//清除队列
+	bool clear();
 
 public:
 	//判空
 	bool empty();
 	//获取队列大小
 	size_t size();
-	//设置队列为退出状态。在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+	// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
 	// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
 	void termination_queue();
 	//获取退出状态
-	bool get_termination();
+	bool get_termination_flag();
 	//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
 	bool is_pass();
 
 protected:
-	mutex m_mutex;							//队列的锁
-	queue<shared_ptr<T>> m_data_queue;		//队列数据，使用智能指针shared_ptr
-	condition_variable m_data_cond;			//条件变量
-	atomic<bool> m_termination;				//终止标志位
+	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;		//终止标志位
 
 private:
 
@@ -95,4 +103,212 @@ private:
 
 
 
+
+
+template<class T>
+Thread_safe_queue<T>::Thread_safe_queue()
+{
+	m_termination_flag = false;
+}
+template<class T>
+Thread_safe_queue<T>::Thread_safe_queue(const Thread_safe_queue& other)
+{
+	std::unique_lock<std::mutex> lock_this(m_mutex);
+	std::unique_lock<std::mutex> lock_other(other.m_mutex);
+	m_data_queue = other.data_queue;
+	m_termination_flag = other.m_termination_flag;
+}
+template<class T>
+Thread_safe_queue<T>::~Thread_safe_queue()
+{
+	//析构时，终止队列，让线程通过等待，方便线程推出。
+	termination_queue();
+}
+
+//（1）没有调用termination时，每调用一次出队一个元素，直到队列为空本方法阻塞线程。
+//（2）在调用了termination后，本方法永不阻塞，如果原本已经处于阻塞状态，解除阻塞状态。
+//（3）返回true时，value值有效。返回false时，value值无效。调用了termination且队列为空时返回false.
+
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+bool Thread_safe_queue<T>::wait_and_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_queue.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return false;
+		}
+		else
+		{
+			value = move(*m_data_queue.front());
+			m_data_queue.pop();
+			return true;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+bool Thread_safe_queue<T>::try_pop(T& value)
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_queue.empty())
+		{
+			return false;
+		}
+		else
+		{
+			value = move(*m_data_queue.front());
+			m_data_queue.pop();
+			return true;
+		}
+	}
+}
+//等待并弹出数据，成功弹出则返回true
+// 队列为空则无限等待，termination终止队列，则返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_queue<T>::wait_and_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		//无限等待，一直阻塞，除非有新的数据加入或者终止队列
+		m_data_cond.wait(lk, [this]
+		{ return ((!m_data_queue.empty()) || m_termination_flag); });
+		if (m_termination_flag)
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_queue.front();
+			m_data_queue.pop();
+			return res;
+		}
+	}
+}
+//尝试弹出数据，成功弹出则返回true
+//队列为空 或者 termination终止队列，返回false
+template<class T>
+std::shared_ptr<T> Thread_safe_queue<T>::try_pop()
+{
+	if ( m_termination_flag )
+	{
+		return NULL;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		if (m_data_queue.empty())
+		{
+			return NULL;
+		}
+		else
+		{
+			std::shared_ptr<T> res = m_data_queue.front();
+			m_data_queue.pop();
+			return res;
+		}
+	}
+}
+//插入一项，并唤醒一个线程，
+//如果成功插入，则返回true，  失败则返回false
+//注：只能唤醒一个线程，防止多线程误判empty()
+template<class T>
+bool Thread_safe_queue<T>::push(T new_value)
+{
+	if (m_termination_flag)
+	{
+		return false;
+	}
+	else
+	{
+		std::shared_ptr<T> data(std::make_shared<T>(move(new_value)));
+		std::unique_lock<std::mutex> lk(m_mutex);
+		m_data_queue.push(data);
+		m_data_cond.notify_one();
+		return true;
+	}
+}
+//清除队列
+template<class T>
+bool Thread_safe_queue<T>::clear()
+{
+	if ( m_termination_flag )
+	{
+		return false;
+	}
+	else
+	{
+		std::unique_lock<std::mutex> lk(m_mutex);
+		while (m_data_queue.empty())
+		{
+			m_data_queue.pop();
+		}
+		return true;
+	}
+}
+
+//判空
+template<class T>
+bool Thread_safe_queue<T>::empty()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_queue.empty();
+}
+//获取队列大小
+template<class T>
+size_t Thread_safe_queue<T>::size()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	return m_data_queue.size();
+}
+//设置队列为退出状态。并唤醒所有的线程，使其通过wait
+// 在退出状态下，所有的功能函数不可用，必须直接返回false或者null。
+// wait_and_pop不会阻塞。让其直接通过，通过后直接return，不允许做其他的。
+template<class T>
+void Thread_safe_queue<T>::termination_queue()
+{
+	std::unique_lock<std::mutex> lk(m_mutex);
+	m_termination_flag = true;
+	m_data_cond.notify_all();
+}
+//获取退出状态
+template<class T>
+bool Thread_safe_queue<T>::get_termination_flag()
+{
+	return m_termination_flag;
+}
+//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
+template<class T>
+bool Thread_safe_queue<T>::is_pass()
+{
+	return (!m_data_queue.empty() || m_termination_flag);
+}
+
+
+
+
+
+
 #endif //LIDARMEASURE_THREAD_SAFE_QUEUE_H