完成mid70采集

CMakeLists.txt

@@ -4,8 +4,6 @@ project(shutter_verify)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++14)
 
-#add_definitions(-std=c++11 -msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
-
 FIND_PACKAGE(OpenCV REQUIRED)
 FIND_PACKAGE(PCL REQUIRED)
 FIND_PACKAGE(Protobuf REQUIRED)
@@ -25,26 +23,33 @@ include_directories(
 		verify
         laser
         plc
+		/usr/include
         /usr/local/include
         /usr/local/include/modbus
         /usr/local/include/snap7
+		${GLOG_INCLUDE_DIRS}
   ${PCL_INCLUDE_DIRS}
   ${OpenCV_INCLUDE_DIRS}
-        ${CERES_INCLUDE_DIRS}
+		${CERES_INCLUDE_DIRS}
+		/usr/local/include/csm
 )
 link_directories("/usr/local/lib")
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/error_code ERROR_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/tool TOOL_SRC )
 aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/laser LASER )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc PLC_SRC )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
-aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY )
-
-add_executable(shutter_verify  verifier.cpp ./main.cpp ${ERROR_SRC} ${LASER}
-		${PLC_SRC} ${TASK_MANAGER_SRC} ${TOOL_SRC} ${VERIFY})
-target_link_libraries(shutter_verify ${OpenCV_LIBS} ${CERES_LIBRARIES}
-        ${GLOG_LIBRARIES} ${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES}
-        /usr/local/lib/libgflags.a /usr/local/lib/liblivox_sdk_static.a
-		/usr/lib/x86_64-linux-gnu/libmodbus.so)
+#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/plc PLC_SRC )
+#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/task TASK_MANAGER_SRC )
+#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify VERIFY )
+#aux_source_directory(${CMAKE_CURRENT_LIST_DIR}/verify/match3d MATCH3D )
+
+
+add_executable(shutter_verify ./main.cpp ${ERROR_SRC} ${LASER}
+		${TOOL_SRC} )
+target_link_libraries(shutter_verify ${OpenCV_LIBS}
+		${PCL_LIBRARIES} ${PROTOBUF_LIBRARIES} ${CERES_LIBRARIES}
+		/usr/local/lib/libglog.a /usr/local/lib/libgflags.a
+		/usr/local/lib/liblivox_sdk_static.a
+		/usr/lib/x86_64-linux-gnu/libmodbus.so  ipopt)
+
 
 

laser/Laser.cpp

@@ -1,547 +0,0 @@
-
-
-#include "Laser.h"
-#include "laser_message.pb.h"
-
-Laser_base::Laser_base(int laser_id, Laser_proto::laser_parameter laser_param)
-:m_laser_id(laser_id)
-,m_laser_param(laser_param)
-,m_laser_scan_flag(false)
-,m_laser_statu(LASER_DISCONNECT)
-,m_points_count(0)
-,m_save_flag(false)
-{
-	mp_thread_receive = NULL;
-	mp_thread_transform = NULL;
-	mp_thread_publish = NULL;
-	init_laser_matrix();
-	mp_laser_task = NULL;
-}
-Laser_base::~Laser_base()
-{
-	
-	disconnect_laser();
-	close_save_path();
-}
-
-//雷达链接设备，为3个线程添加线程执行函数。
-Error_manager Laser_base::connect_laser()
-{
-	//检查雷达状态
-	if ( m_laser_statu != LASER_DISCONNECT )
-	{
-	    return Error_manager(Error_code::LASER_STATUS_ERROR, Error_level::MINOR_ERROR,
-	    					" Laser_base::connect_laser  m_laser_statu != LASER_DISCONNECT ");
-	}
-	else
-	{
-		//这里不建议用detach，而是应该在disconnect里面使用join
-		//创建接受缓存的线程，不允许重复创建
-		if (mp_thread_receive != NULL)
-		{
-			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
-								 " mp_thread_receive is alive ");
-		}
-		else
-		{
-			//接受缓存的线程，线程存活，暂停运行
-			m_condition_receive.reset(false, false, false);
-			mp_thread_receive = new std::thread(&Laser_base::thread_receive, this);
-		}
-		//转化数据的线程，不允许重复创建
-		if (mp_thread_transform != NULL)
-		{
-			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
-								 " mp_thread_transform is alive ");
-		}
-		else
-		{
-			//转化数据的线程，线程存活，暂停运行
-			m_condition_transform.reset(false, false, false);
-			mp_thread_transform = new std::thread(&Laser_base::thread_transform, this);
-		}
-		//发布信息的线程，不允许重复创建
-		if (mp_thread_publish != NULL)
-		{
-			return Error_manager(Error_code::LASER_CONNECT_FAILED, Error_level::MINOR_ERROR,
-								 " mp_thread_publish is alive ");
-		}
-		else
-		{
-			//发布信息的线程，线程存活，循环运行，
-			//注：mp_thread_publish 需要线程持续不断的往上位机发送信息，不受任务的影响。
-			m_condition_publish.reset(false, true, false);
-			mp_thread_publish = new std::thread(&Laser_base::thread_publish, this);
-		}
-
-		LOG(INFO)<<"laser connected success";
-		m_laser_statu = LASER_READY;
-
-	}
-	return Error_code::SUCCESS;
-}
-//雷达断开链接，释放3个线程
-Error_manager Laser_base::disconnect_laser()
-{
-	//终止队列，防止 wait_and_pop 阻塞线程。
-	m_queue_laser_data.termination_queue();
-	//杀死3个线程，强制退出
-	if (mp_thread_receive)
-	{
-		m_condition_receive.kill_all();
-	}
-	if (mp_thread_transform)
-	{
-		m_condition_transform.kill_all();
-	}
-	if (mp_thread_publish)
-	{
-		m_condition_publish.kill_all();
-	}
-	//回收3个线程的资源
-	if (mp_thread_receive)
-	{
-		mp_thread_receive->join();
-		delete mp_thread_receive;
-		mp_thread_receive = NULL;
-	}
-	if (mp_thread_transform)
-	{
-		mp_thread_transform->join();
-		delete mp_thread_transform;
-		mp_thread_transform = 0;
-	}
-	if (mp_thread_publish)
-	{
-		mp_thread_publish->join();
-		delete mp_thread_publish;
-		mp_thread_publish = NULL;
-	}
-	//清空队列
-	m_queue_laser_data.clear_and_delete();
-
-	if ( m_laser_statu != LASER_FAULT  )
-	{
-		m_laser_statu = LASER_DISCONNECT;
-	}
-	return Error_code::SUCCESS;
-}
-
-
-//对外的接口函数，负责接受并处理任务单，
-//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-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");
-	}
-}
-//检查雷达状态,是否正常运行
-Error_manager Laser_base::check_laser()
-{
-	if ( is_ready() )
-	{
-		return Error_code::SUCCESS;
-	}
-	return Error_manager(Error_code::LASER_CHECK_FAILED, Error_level::MINOR_ERROR,
-						" check_laser failed ");
-}
-//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-Error_manager Laser_base::start_scan()
-{
-	//LOG(INFO) << " Laser_base::start_scan "<< this;
-	if ( is_ready() )
-	{
-		//启动雷达扫描
-		m_laser_scan_flag=true;				//启动雷达扫描
-		m_laser_statu = LASER_BUSY;			//雷达状态 繁忙
-		m_condition_receive.notify_all(true);		//唤醒接受线程
-		m_condition_transform.notify_all(true);	//唤醒转化线程
-		//重置数据
-		m_points_count=0;
-		m_queue_laser_data.clear_and_delete();
-		m_last_data.clear();
-		return Error_code::SUCCESS;
-	}
-	else
-	{
-		return Error_manager(Error_code::LASER_START_FAILED, Error_level::MINOR_ERROR,
-							 "Laser_base::start_scan() is not ready ");
-	}
-}
-//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
-Error_manager Laser_base::stop_scan()
-{
-	//LOG(INFO) << " Laser_base::stop_scan "<< this;
-	//stop_scan 只是将 m_laser_scan_flag 改为 stop。
-	//然后多线程内部判断 m_laser_scan_flag，然后自动停止线程
-	m_laser_scan_flag=false;
-	return Error_code::SUCCESS;
-}
-
-//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
-Error_manager Laser_base::end_task()
-{
-	//LOG(INFO) << " Laser_base::end_task "<< this;
-	m_laser_scan_flag=false;
-	m_condition_receive.notify_all(false);
-	m_condition_transform.notify_all(false);
-	m_points_count=0;
-	m_queue_laser_data.clear_and_delete();
-	m_last_data.clear();
-
-	close_save_path();
-
-	//在结束任务单时，将雷达任务状态改为 TASK_OVER 已结束
-	if(mp_laser_task !=NULL)
-	{
-		//判断任务单的错误等级，
-		if ( mp_laser_task->get_task_error_manager().get_error_level() <= Error_level::MINOR_ERROR)
-		{
-			if ( m_laser_statu == LASER_BUSY )
-			{
-				//故障等级为 NORMAL NEGLIGIBLE_ERROR MINOR_ERROR，则恢复待机，可以接受下一次的任务单
-				m_laser_statu = LASER_READY;
-			}
-
-		}
-		else
-		{
-			//故障等级为 MAJOR_ERROR CRITICAL_ERROR，则雷达故障，不可以再接受任务单。
-			m_laser_statu = LASER_FAULT;
-		}
-		//强制改为TASK_OVER，不管它当前在做什么。
-		mp_laser_task->set_task_statu(TASK_OVER);
-	}
-	return Error_code::SUCCESS;
-}
-
-
-
-//设置保存文件的路径，并打开文件，
-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);
-		//std::cout << "huli m_points_log_tool path "<< pts_file << std::endl;
-
-		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;
-	}
-}
-//关闭保存文件，推出前一定要执行
-Error_manager Laser_base::close_save_path()
-{
-	m_save_flag = false;
-	m_points_log_tool.close();
-	m_binary_log_tool.close();
-	return Error_code::SUCCESS;
-}
-
-//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-bool Laser_base::is_ready()
-{
-	return ( get_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;
-}
-
-//线程执行函数，将二进制消息存入队列缓存，
-void Laser_base::thread_receive()
-{
-	LOG(INFO) << " thread_receive start "<< this;
-	//接受雷达消息，每次循环只接受一个 Binary_buf
-	while (m_condition_receive.is_alive())
-	{
-
-		m_condition_receive.wait();
-		if ( m_condition_receive.is_alive() )
-		{
-			//tp_binaty_buf的内存 需要手动分配，然后存入链表。
-			//m_queue_laser_data的内存由 Laser_base 基类管理。
-			Binary_buf* tp_binaty_buf = new Binary_buf();
-			//获取雷达的通信消息缓存
-			if (this->receive_buf_to_queue(*tp_binaty_buf))
-			{
-				//将缓存 存入队列。
-				//thread_receive存入前要手动分配内存，thread_transform取出后要记得释放。
-				m_queue_laser_data.push(tp_binaty_buf);
-			}
-			else
-			{
-				delete tp_binaty_buf;
-				tp_binaty_buf=NULL;
-
-				//接受线程无法接受到数据的同时，也收到了雷达停止扫描的信息，那么线程停止
-				//如果m_laser_scan_flag停止扫描，但是receive_buf_to_queue仍然能够接受到数据。那么线程继续运行。
-				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。
-				if ( !m_laser_scan_flag )
-				{
-					//停止线程，m_condition_receive.wait() 函数将会阻塞。
-					m_condition_receive.set_pass_ever(false);
-				}
-			}			
-		}
-	}
-	LOG(INFO) << " thread_receive end :"<<this;
-	return;
-}
-
-
-//线程执行函数，转化并处理三维点云。
-void Laser_base::thread_transform()
-{
-	LOG(INFO) << " thread_transform start "<< this;
-	//转化雷达数据，每次循环只转化一个 Binary_buf，然后得到 t_point3D_cloud.size() 个三维点，
-	while (m_condition_transform.is_alive())
-	{
-		m_condition_transform.wait();
-		if ( m_condition_transform.is_alive() )
-		{
-			//tp_binaty_buf的内存 从链表取出时，就自带内存了。需要手动释放。
-			//m_queue_laser_data的内存由 Laser_base 基类管理。
-			Binary_buf* tp_binaty_buf=NULL;
-			//第1步，从队列中取出缓存。
-			int huli_test = m_queue_laser_data.size();
-			//thread_receive存入前要手动分配内存，thread_transform取出后要记得释放。
-			bool t_pop_flag = m_queue_laser_data.try_pop(tp_binaty_buf);
-			/*if ( t_pop_flag )
-			{
-			    std::cout << "huli m_queue_laser_data.size() last = " << huli_test << std::endl;
-				std::cout << "huli m_queue_laser_data.size() now = " << m_queue_laser_data.size() << std::endl;
-
-			}*/
-
-			//第2步，处理数据，缓存转化缓存为三维点。
-			//如果获取到了 Binary_buf，或者上次有未完成的 m_last_data，那么就执行数据转化。
-			//注注注注注意了，扫描停止后，m_last_data一定要保证清空，不能有垃圾数据残留，这会阻塞线程的暂停
-			if (t_pop_flag || m_last_data.get_length() > 0)
-			{
-				//std::cout << "huli thread_transform " << m_queue_laser_data.size() << std::endl;
-				//std::cout << "huli t_pop_flag = "<< t_pop_flag << std::endl;
-				//缓存类型
-				Buf_type t_buf_type = BUF_UNKNOW;
-				//雷达扫描结果，三维点云（雷达自身坐标系）
-				std::vector<CPoint3D> t_point3D_cloud;
-				//第2.1步，缓存转化为三维点。
-				if (t_pop_flag)
-				{
-					if (tp_binaty_buf == NULL)
-					{
-						//try_pop返回true，但是tp_binaty_buf没有数据，直接跳过。这个一般不可能出现。
-						continue;
-					}
-					else
-					{
-						//保存雷达通信 二进制的源文件。
-						if(m_save_flag)
-						{
-							m_binary_log_tool.write(tp_binaty_buf->get_buf(), tp_binaty_buf->get_length());
-						}
-						//缓存转化为三维点。传入新的缓存，
-						t_buf_type= this->transform_buf_to_points(tp_binaty_buf, t_point3D_cloud);
-					}					
-				}
-				else if(m_last_data.get_length() > 0)
-				{
-					//缓存转化为三维点。没有新消息，就传null，这将处理 m_last_data 上一次遗留的缓存。
-					t_buf_type = this->transform_buf_to_points(NULL, t_point3D_cloud);
-				}
-
-				//第2.2步，判断t_buf_type，处理雷达数据。并存入任务单
-				if (t_buf_type == BUF_UNKNOW || t_buf_type == BUF_READY)
-				{
-					//跳过这次循环。
-					continue;
-				}
-				else if (t_buf_type == BUF_DATA || t_buf_type == BUF_START || t_buf_type == BUF_STOP)
-				{
-					//循环处理雷达数据
-					for (int i = 0; i < t_point3D_cloud.size(); ++i)
-					{
-						//三维点的坐标变换，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-						CPoint3D t_point = transform_by_matrix(t_point3D_cloud[i]);
-						//保存雷达扫描 三维点云的最终结果。
-						if(m_save_flag) {
-							char buf[64] = {0};
-							sprintf(buf, "%f %f %f\n", t_point.x, t_point.y, t_point.z);
-							m_points_log_tool.write(buf, strlen(buf));
-							//std::cout << "huli m_points_log_tool.write" << std::endl;
-						}
-
-						//此时 t_point 为雷达采集数据的结果，转换后的三维点云。
-						//将每个点存入任务单里面的点云容器。
-						if(mp_laser_task!=NULL)
-						{
-							Error_manager t_error= mp_laser_task->task_push_point(pcl::PointXYZ(t_point.x,t_point.y,t_point.z));
-							if ( t_error != Error_code::SUCCESS )
-							{
-								mp_laser_task->get_task_error_manager().compare_and_cover_error(t_error);
-								stop_scan();
-							}
-						}
-					}
-					//统计扫描点个数。
-					m_points_count += t_point3D_cloud.size();
-					//思科雷达停止扫描。
-					if (t_buf_type == BUF_STOP)
-					{
-						//注：这个只是思科的雷达停止处，不同的雷达在不同的地方调用 stop_scan
-						stop_scan();
-					}
-				}
-			}
-			else
-			{
-				//转化线程 不需要数据的同时，也收到了雷达停止扫描的信息并且接受线程已经停止，那么停止转化线程，
-				//注：必须先停止接受线程，再停止转化线程。转化线程停止之后即可结束任务。
-				//如果m_laser_scan_flag停止扫描，接受线程和转化线程还有未完成的数据。那么线程继续运行。
-				//m_laser_scan_flag停止的瞬间，可能还有残留的数据，需要继续执行。防止数据遗留。
-				if ( !m_laser_scan_flag && m_condition_receive.get_pass_ever() == false)
-				{
-					//停止线程，m_condition_transform.wait() 函数将会阻塞。
-					m_condition_transform.set_pass_ever(false);
-
-					//mp_thread_transform 停止时，雷达扫描任务彻底完成，此时结束任务
-					end_task();
-				}
-			}
-
-			//第3步，手动释放缓存
-			if ( tp_binaty_buf != NULL )
-			{
-				delete tp_binaty_buf;
-			}
-		}
-	}
-	LOG(INFO) << " thread_transform end "<< this;
-	return;
-}
-
-Error_manager Laser_base::publish_laser_to_message()
-{
-    return SUCCESS;
-}
-//线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
-void Laser_base::thread_publish(Laser_base *p_laser)
-{
-	LOG(INFO) << " thread_publish start ";
-	if(p_laser==NULL)
-	{
-		return;
-	}
-
-	while (p_laser->m_condition_publish.is_alive())
-	{
-		p_laser->m_condition_publish.wait();
-		if ( p_laser->m_condition_publish.is_alive() )
-		{
-			p_laser->publish_laser_to_message();
-			//每隔300ms，发送一次雷达信息状态。
-			std::this_thread::sleep_for(std::chrono::milliseconds(300));
-		}
-	}
-
-	LOG(INFO) << " thread_publish end ";
-	return;
-}
-
-
-
-//初始化变换矩阵，设置默认值
-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
-	{
-	    if(m_laser_id==5)
-	        std::cout<<"----------------------   horizon matrix";
-		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,198 +0,0 @@
-
-/*
- * laser雷达核心模块，主要是负责管理所有的雷达设备，按需采集三维点。
- * 由上层下发任务单，雷达模块按照任务指示，进行相应的雷达扫描。
- * 转换和拼接为标准参考系的三维坐标点云。
- * 然后通过任务单返回上层。
- */
-
-#ifndef __LASER__HH__
-#define __LASER__HH__
-#include "Point2D.h"
-#include "Point3D.h"
-#include "LogFiles.h"
-
-#include <glog/logging.h>
-
-#include "laser_parameter.pb.h"
-#include "laser_task_command.h"
-
-#include "../error_code/error_code.h"
-#include "../tool/binary_buf.h"
-#include "../tool/thread_safe_queue.h"
-#include "../tool/thread_condition.h"
-
-
-
-
-//雷达工作状态，基类三线程的状态
-//connect_laser 和  disconnect_laser  	中要切换状态 LASER_DISCONNECT <===> LASER_READY
-//start_scan 和 stop_scan 和 end_task	中要切换状态 LASER_READY <===> LASER_BUSY
-enum Laser_statu
-{
-	//默认值 LASER_DISCONNECT	=0
-	LASER_DISCONNECT	=0,	        //雷达断连
-	LASER_READY			=1,			//雷达正常待机，空闲
-	LASER_BUSY			=2,	        //雷达正在工作，正忙
-	LASER_FAULT			=3,         //雷达错误
-};
-
-//雷达变换矩阵的数组长度，默认为12个参数， size = 3 * ( 3 + 1 ) ，旋转加平移，没有缩放
-#define LASER_MATRIX_ARRAY_SIZE 12
-
-//雷达的基类，不能直接使用，必须子类继承
-class Laser_base
-{
-public:
-	Laser_base() = delete;
-	Laser_base(const Laser_base& other) = delete;
-	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
-	//input：id： 雷达设备的id，（唯一索引）
-	//input：laser_param：雷达的参数，
-	//注：利用protobuf创建 laser_parameter 类，然后从文件读取参数
-	Laser_base(int laser_id,Laser_proto::laser_parameter laser_param);
-	//析构函数
-	~Laser_base();
-
-	//雷达链接设备，为3个线程添加线程执行函数。
-	virtual Error_manager connect_laser();
-	//雷达断开链接，释放3个线程
-	virtual Error_manager disconnect_laser();
-	//对外的接口函数，负责接受并处理任务单，
-	//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-	//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-	virtual Error_manager execute_task(Task_Base* p_laser_task);
-	//检查雷达状态,是否正常运行
-	virtual Error_manager check_laser();
-	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-	virtual Error_manager start_scan();
-	//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
-	virtual Error_manager stop_scan();
-	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
-	virtual Error_manager end_task();
-
-public:
-	//设置保存文件的路径，并打开文件，
-	Error_manager set_open_save_path(std::string save_path,bool is_save=true);
-	//关闭保存文件，推出前一定要执行
-	Error_manager close_save_path();
-	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-	virtual bool is_ready();
-
-	//获取雷达id
-	int get_laser_id();
-
-
-protected:
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	// 纯虚函数，必须由子类重载，
-	virtual bool receive_buf_to_queue(Binary_buf& binary_buf) = 0;
-	//线程执行函数，将二进制消息存入队列缓存，
-	void thread_receive();
-
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)=0;
-	//线程执行函数，转化并处理三维点云。
-	void thread_transform();
-
-	//公开发布雷达信息的功能函数，
-	virtual Error_manager publish_laser_to_message();
-	//线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
-	static void thread_publish(Laser_base* p_laser);
-
-	//获取雷达状态
-	Laser_statu get_laser_statu();
-protected:
-	//初始化变换矩阵，设置默认值
-	Error_manager init_laser_matrix();
-	//设置变换矩阵，用作三维点的坐标变换，
-	Error_manager set_laser_matrix(double* p_matrix, int size);
-
-	//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-	virtual CPoint3D transform_by_matrix(CPoint3D point);
-
-
-protected:
-	//为了保证多线程的数据安全，修改共享数据必须加锁。  atomic 和 安全队列 可以不加锁进行读写
-	//建议：判断标志位使用 atomic， 容器要封装并使用 安全容器，
-	std::mutex          				m_laser_lock;            	//雷达数据锁
-
-	std::atomic<int>					m_laser_id;                 //雷达设备id
-	Laser_proto::laser_parameter  		m_laser_param;				//雷达的配置参数
-	//雷达变换矩阵，三维点的坐标变换的矩阵，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-	//必须在set_laser_matrix之后，才能使用。（connect时，从雷达的配置参数导入）
-	double								mp_laser_matrix[LASER_MATRIX_ARRAY_SIZE];	//雷达变换矩阵
-	//雷达扫描事件的标志位，受start和stop控制，然后其他线程判断m_scan_flag标准位，来进行启停。
-	std::atomic<bool>                	m_laser_scan_flag;          //雷达扫描的使能标志位
-	//雷达状态和任务状态同步，m_scan_flag停止之后，还要等任务执行完成,才会切回 ready。
-	std::atomic<Laser_statu>			m_laser_statu;             	//雷达工作状态，基类三线程的状态
-	//注：m_laser_statu是基类的三线程的状态，和livox sdk后台线程状态没有任何关系。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-
-	std::atomic<int>            		m_points_count;         	//雷达采集点的计数
-	Thread_safe_queue<Binary_buf*>		m_queue_laser_data;         //二进制缓存的队列容器
-	Binary_buf							m_last_data;			    //上一个二进制缓存，用作数据拼接
-
-
-	std::atomic<bool> 					m_save_flag;           		//雷达保存文件的使能标志位
-	std::string							m_save_path;            	//雷达保存文件的保存路径
-	CLogFile							m_binary_log_tool;          //二进制缓存的日志工具
-	CLogFile							m_points_log_tool;          //三维点云的日志工具
-
-
-	//线程指针的内存管理，由Connect和Disconnect进行分配和释放。
-	std::thread*						mp_thread_receive;    		//接受缓存的线程指针
-	Thread_condition					m_condition_receive;		//接受缓存的条件变量
-	std::thread*						mp_thread_transform;   		//转化数据的线程指针
-	Thread_condition					m_condition_transform;		//转化数据的条件变量
-	std::thread*        				mp_thread_publish;   		//发布信息的线程指针
-	Thread_condition					m_condition_publish;		//发布信息的条件变量
-
-	//任务单的指针，实际内存由应用层管理，
-	//接受任务后，指向新的任务单
-	Laser_task *  						mp_laser_task;        		//任务单的指针
-
-};
-
-
-//雷达的注册类，
-//雷达模块可能有多个不同的雷达，这些雷达需要制定不同的子类，从laser基类继承。
-//使用类的多态，从基类操作不同的子类。
-//雷达的注册器为单例模式，没有任何成员变量。
-//里面定义了 static std::map<std::string, CreateLaserFunc>* g_map  注册表，作为永久的内存。
-//然后提供了注册函数和访问函数，来操作这段静态变量（唯一，永久）
-class LaserRegistory
-{
-	typedef Laser_base*  (*CreateLaserFunc)(int id, Laser_proto::laser_parameter laser_param);
-public:
-	LaserRegistory(std::string name, CreateLaserFunc pFun) {
-		AddCreator(name, pFun);
-	}
-	static Laser_base* CreateLaser(std::string name, int id,Laser_proto::laser_parameter laser_param) {
-		if (GetFuncMap().count(name) == 0)
-			return 0;
-		return GetFuncMap()[name](id,laser_param);
-	}
-private:
-	static std::map<std::string, CreateLaserFunc>& GetFuncMap() {
-		static std::map<std::string, CreateLaserFunc>* g_map = new std::map<std::string, CreateLaserFunc>;
-
-		return *g_map;
-	}
-	void AddCreator(std::string name, CreateLaserFunc pFun) {
-		GetFuncMap()[name] = pFun;
-	}
-};
-
-
-#define RegisterLaser(NAME) \
-	static Laser_base* Create_##NAME##_Laser(int id, Laser_proto::laser_parameter param)	\
-	{																\
-		return new C##NAME##Laser(id,param);							\
-	}																\
-	LaserRegistory g_##NAME##_Laser(#NAME,Create_##NAME##_Laser);
-
-
-#endif

laser/Laser.puml

@@ -1,298 +0,0 @@
-@startuml
-skinparam classAttributeIconSize 0
-
-title  Laser_base  雷达的基类
-
-enum Laser_statu
-{
-//雷达工作状态，基类三线程的状态
-//connect_laser 和  disconnect_laser  	中要切换状态 LASER_DISCONNECT <===> LASER_READY
-//start_scan 和 stop_scan 和 end_task	中要切换状态 LASER_READY <===> LASER_BUSY
-//默认值 LASER_DISCONNECT	=0
-	LASER_DISCONNECT	=0,	        //雷达断连
-	LASER_READY			=1,			//雷达正常待机，空闲
-	LASER_BUSY			=2,	        //雷达正在工作，正忙
-	LASER_FAULT			=3,         //雷达错误
-}
-
-
-
-class Laser_base
-{
-//雷达的基类，不能直接使用，必须子类继承
-==public:==
-	Laser_base() = delete;
-	Laser_base(const Laser_base& other) = delete;
-..
-	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
-	//input：id： 雷达设备的id，（唯一索引）
-	//input：laser_param：雷达的参数，
-	//注：利用protobuf创建 laser_parameter 类，然后从文件读取参数
-	Laser_base(int laser_id,Laser_proto::laser_parameter laser_param);
-	//析构函数
-	~Laser_base();
-..
-	//雷达链接设备，为3个线程添加线程执行函数。
-	virtual Error_manager connect_laser();
-	//雷达断开链接，释放3个线程
-	virtual Error_manager disconnect_laser();
-	//对外的接口函数，负责接受并处理任务单，
-	//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-	//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-	virtual Error_manager execute_task(Task_Base* p_laser_task);
-	//检查雷达状态,是否正常运行
-	virtual Error_manager check_laser();
-	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-	virtual Error_manager start_scan();
-	//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
-	virtual Error_manager stop_scan();
-	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
-	virtual Error_manager end_task();
-==public:==
-	//设置保存文件的路径，并打开文件，
-	Error_manager set_open_save_path(std::string save_path,bool is_save=true);
-	//关闭保存文件，推出前一定要执行
-	Error_manager close_save_path();
-	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-	virtual bool is_ready();
-	//获取雷达id
-	int get_laser_id();
-==protected:==
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	// 纯虚函数，必须由子类重载，
-	virtual bool receive_buf_to_queue(Binary_buf& binary_buf) = 0;
-	//线程执行函数，将二进制消息存入队列缓存，
-	void thread_receive();
-..
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)=0;
-	//线程执行函数，转化并处理三维点云。
-	void thread_transform();
-..
-	//公开发布雷达信息的功能函数，
-	Error_manager publish_laser_to_message();
-	//线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
-	static void thread_publish(Laser_base* p_laser);
-..
-	//获取雷达状态
-	Laser_statu get_laser_statu();
-==protected:==
-	//初始化变换矩阵，设置默认值
-	Error_manager init_laser_matrix();
-	//设置变换矩阵，用作三维点的坐标变换，
-	Error_manager set_laser_matrix(double* p_matrix, int size);
-..
-	//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-	virtual CPoint3D transform_by_matrix(CPoint3D point);
-==protected:==
-	//为了保证多线程的数据安全，修改共享数据必须加锁。  atomic 和 安全队列 可以不加锁进行读写
-	//建议：判断标志位使用 atomic， 容器要封装并使用 安全容器，
-	std::mutex          				m_laser_lock;            	//雷达数据锁
-..
-	std::atomic<int>					m_laser_id;                 //雷达设备id
-	Laser_proto::laser_parameter  		m_laser_param;				//雷达的配置参数
-	//雷达变换矩阵，三维点的坐标变换的矩阵，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-	//必须在set_laser_matrix之后，才能使用。（connect时，从雷达的配置参数导入）
-	double								mp_laser_matrix[LASER_MATRIX_ARRAY_SIZE];	//雷达变换矩阵
-	//雷达扫描事件的标志位，受start和stop控制，然后其他线程判断m_scan_flag标准位，来进行启停。
-	std::atomic<bool>                	m_laser_scan_flag;          //雷达扫描的使能标志位
-	//雷达状态和任务状态同步，m_scan_flag停止之后，还要等任务执行完成,才会切回 ready。
-	std::atomic<Laser_statu>			m_laser_statu;             	//雷达工作状态，基类三线程的状态
-	//注：m_laser_statu是基类的三线程的状态，和livox sdk后台线程状态没有任何关系。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-..
-	std::atomic<int>            		m_points_count;         	//雷达采集点的计数
-	Thread_safe_queue<Binary_buf*>		m_queue_laser_data;         //二进制缓存的队列容器
-	Binary_buf							m_last_data;			    //上一个二进制缓存，用作数据拼接
-..
-	std::atomic<bool> 					m_save_flag;           		//雷达保存文件的使能标志位
-	std::string							m_save_path;            	//雷达保存文件的保存路径
-	CLogFile							m_binary_log_tool;          //二进制缓存的日志工具
-	CLogFile							m_points_log_tool;          //三维点云的日志工具
-..
-	//线程指针的内存管理，由Connect和Disconnect进行分配和释放。
-	std::thread*						mp_thread_receive;    		//接受缓存的线程指针
-	Thread_condition					m_condition_receive;		//接受缓存的条件变量
-	std::thread*						mp_thread_transform;   		//转化数据的线程指针
-	Thread_condition					m_condition_transform;		//转化数据的条件变量
-	std::thread*        				mp_thread_publish;   		//发布信息的线程指针
-	Thread_condition					m_condition_publish;		//发布信息的条件变量
-..
-	//任务单的指针，实际内存由应用层管理，
-	//接受任务后，指向新的任务单
-	Laser_task *  						mp_laser_task;        		//任务单的指针
-}
-
-
-class CPoint3D
-{
-//三维点
-	double x, y, z;
-}
-
-class CLogFile
-{
-//日志文件
-	void open(const char *_Filename, bool binary = false);
-	void write(const char *_Str, streamsize _Count);
-	void close();
-	fstream m_file;
-    std::mutex m_lock;
-}
-
-class laser_parameter
-{
-//雷达参数
-}
-
-
-class Laser_task
-{
-//雷达模块的任务指令，从Task_Base继承，
-//补充了雷达专属的数据输入和输出
-==public:==
-    //初始化任务单，必须初始化之后才可以使用，（必选参数）
-    Error_manager task_init(Task_statu task_statu,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock);
-..
-    //初始化任务单，必须初始化之后才可以使用，（可选参数）
-    Error_manager task_init(Task_statu task_statu,
-                            std::string & task_statu_information,
-                            unsigned int task_frame_maxnum,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock);
-..
-    //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
-    Error_manager task_push_point(pcl::PointXYZ point_xyz);
-==protected:==
-    //点云的采集帧数最大值，任务输入
-    unsigned int                    m_task_frame_maxnum;
-    //点云保存文件的路径，任务输入
-    std::string                     m_task_save_path;
-    //三维点云的数据保护锁，任务输入
-    std::mutex*                     mp_task_cloud_lock;
-    //采集结果，三维点云容器的智能指针，任务输出
-    //这里只是指针，实际内存由应用层管理，初始化时必须保证内存有效。
-    pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
-}
-
-
-class Task_Base
-{
-//任务单基类
-==public:==
-    //初始化任务单,初始任务单类型为 UNKONW_TASK
-    virtual Error_manager init();
-    //更新任务单
-    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
-    //获取任务类型
-    Task_type   get_task_type();
-==protected:==
-    Task_type                   m_task_type;                    //任务类型
-    Task_statu                  m_task_statu;                   //任务状态
-    std::string                 m_task_statu_information;       //任务状态说明
-	Error_manager               m_task_error_manager;//错误码，任务故障信息，任务输出
-}
-
-Laser_task <-- Task_Base : inherit
-
-class Error_manager
-{
-//错误码管理
-}
-
-
-class Binary_buf
-{
-//二进制缓存，
-	//比较前面部分的buf是否相等，使用 other.m_length 为标准
-	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);
-..
-	char* get_buf()const;
-	int	get_length()const;
---
-	char*		mp_buf;				//二进制缓存指针
-	int			m_length;			//二进制缓存长度
-}
-
-
-class Thread_condition
-{
-	bool wait();
-	//等待一定的时间（默认时间单位：毫秒ms），由 is_pass_wait 决定是否阻塞。
-	bool wait_for_millisecond(unsigned int millisecond);
-	//唤醒已经阻塞的线程，唤醒一个线程
-	void notify_one(bool pass_ever, bool pass_once = false);
-	//唤醒已经阻塞的线程，唤醒全部线程
-	void notify_all(bool pass_ever, bool pass_once = false);
-	//杀死所有的线程，强制退出线程函数，只是操作受当前Thread_condition影响的所有线程
-	void kill_all();
-	//判断是否或者，return !m_kill_flag
-	bool is_alive();
-==protected:==
-	static bool is_pass_wait(Thread_condition * other);
-..
-	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;	//线程的条件变量
-}
-
-
-class Thread_safe_queue <<    template<class T>    >>
-{
-	//等待并弹出数据，成功弹出则返回true
-	bool wait_and_pop(T& value);
-	//尝试弹出数据，成功弹出则返回true
-	bool try_pop(T& value);
-	//等待并弹出数据，成功弹出则返回true
-	std::shared_ptr<T> wait_and_pop();
-	//尝试弹出数据，成功弹出则返回true
-	std::shared_ptr<T> try_pop();
-	//插入一项，并唤醒一个线程，
-	bool push(T new_value);
-	//清除队列，只是将队列的实例抛出。T是实例内存，系统自动回收的。
-	bool clear();
-	//清除队列，抛出之后还要delete指针。T是动态内存，需要手动回收的。
-	bool clear_and_delete();
-==public:==
-	//判空
-	bool empty();
-	//获取队列大小
-	size_t size();
-	//设置队列为退出状态。并唤醒所有的线程，使其通过wait
-	void termination_queue();
-	//唤醒队列，恢复所有的功能函数。wait_and_pop会继续阻塞。
-	void wake_queue();
-	//获取退出状态
-	bool get_termination_flag();
-	//判断是否可以直接通过wait，  m_data_queue不为空或者m_termination终止时都可以通过等待。
-	bool is_pass();
-==protected:==
-	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;		//终止标志位
-}
-
-
-
-Laser_base <-- Laser_statu : include
-Laser_base <-- CPoint3D : include
-Laser_base <-- CLogFile : include
-Laser_base <-- laser_parameter : include
-Laser_base <-- Laser_task : include
-Laser_base <-- Error_manager : include
-Laser_base <-- Binary_buf : include
-Laser_base <-- Thread_condition : include
-Laser_base <-- Thread_safe_queue : include
-
-
-
-@enduml

laser/LivoxHorizon.cpp

@@ -1,78 +0,0 @@
-
-
-#include "LivoxHorizon.h"
-
-RegisterLaser(Horizon)
-
-
-
-CHorizonLaser::CHorizonLaser(int id, Laser_proto::laser_parameter laser_param)
-:CLivoxLaser(id, laser_param)
-{
-
-        //设备livox扫描最大帧数
-        m_frame_maxnum = laser_param.frame_num();
-        //判断参数类型，
-        if(laser_param.type()=="Horizon")
-        {
-            //填充雷达设备的广播码
-            g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
-            //初始化livox
-            InitLivox();
-        }
-
-
-}
-
-CHorizonLaser::~CHorizonLaser()
-{
-
-}
-//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-Error_manager CHorizonLaser::start_scan()
-{
-    //LOG(INFO) << " Horizon start :"<<this;
-    return CLivoxLaser::start_scan();
-}
-//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-// 纯虚函数，必须由子类重载，
-Buf_type CHorizonLaser::transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)
-{
-    static int count=0;
-
-    if ( p_binary_buf ==NULL )
-    {
-        return BUF_UNKNOW;
-    }
-    else
-    {
-        //livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
-        LivoxExtendRawPoint *tp_Livox_data = (LivoxExtendRawPoint *)p_binary_buf->get_buf();
-        //计算这一帧数据有多少个三维点。
-        int t_count = p_binary_buf->get_length() / (sizeof(LivoxExtendRawPoint));
-
-        if (t_count <= 0)
-        {
-            return BUF_UNKNOW;
-        }
-        else
-        {
-            //转变三维点格式，并存入vector。
-            for (int i = 0; i < t_count; ++i)
-            {
-                //LivoxExtendRawPoint 转为 CPoint3D
-                //int32_t 转 double。不要信号强度
-                LivoxExtendRawPoint t_livox_point = tp_Livox_data[i];
-                CPoint3D t_point3D;
-                t_point3D.x = t_livox_point.x/1000.0;
-                t_point3D.y = t_livox_point.y/1000.0;
-                t_point3D.z = t_livox_point.z/1000.0;
-                point3D_cloud.push_back(t_point3D);
-//				std::cout << "huli points:" << t_point3D.x << " : " << t_point3D.y << " : " << t_point3D.z<< std::endl;
-            }
-            return BUF_DATA;
-        }
-    }
-
-}
-

laser/LivoxHorizon.h

@@ -1,41 +0,0 @@
-#ifndef __LIVOX_HORIZON__H
-#define __LIVOX_HORIZON__H
-
-#include "Laser.h"
-#include "livox_def.h"
-#include "livox_sdk.h"
-#include <map>
-#include "LivoxLaser.h"
-
-
-
-
-//大疆livox雷达，从Laser_base继承。
-class CHorizonLaser :public CLivoxLaser
-{
-
-public:
-	CHorizonLaser() = delete;
-	CHorizonLaser(const CHorizonLaser& other) = delete;
-	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
-	//input：id： 雷达设备的id，（唯一索引）
-	//input：laser_param：雷达的参数，
-	//注：利用protobuf创建 laser_parameter 类，然后从文件读取参数
-	CHorizonLaser(int id, Laser_proto::laser_parameter laser_param);
-	~CHorizonLaser();
-    //雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-    virtual Error_manager start_scan();
-
-	//将二进制消息转化为三维点云的功能函数，并将其存入 任务单 , 每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud);
-};
-
-
-
-
-
-
-
-
-#endif

laser/LivoxHubLaser.cpp

@@ -1,10 +0,0 @@
-#include "LivoxHubLaser.h"
-
-CLivoxHubLaser::CLivoxHubLaser()
-{
-
-}
-CLivoxHubLaser::~CLivoxHubLaser()
-{
-
-}

laser/LivoxHubLaser.h

@@ -1,19 +0,0 @@
-#ifndef LIVOXHUBLASER_H
-#define LIVOXHUBLASER_H
-
-
-class CLivoxHubLaser
-{
-public:
-    CLivoxHubLaser();
-    ~CLivoxHubLaser();
-protected:
-    /*static void InitLivoxHub();
-    static void LidarDataCallbackHub(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser);
-    static void OnDeviceChangeHub(const DeviceInfo *info, DeviceEvent type);
-    static void OnDeviceBroadcastHub(const BroadcastDeviceInfo *info);
-    static void OnSampleCallbackHub(uint8_t status, uint8_t handle, uint8_t response, void *data);*/
-
-};
-
-#endif // LIVOXHUBLASER_H

laser/LivoxLaser.cpp

@@ -1,450 +0,0 @@
-
-#include "LivoxLaser.h"
-
-RegisterLaser(Livox)
-
-CLivoxLaser::DeviceItem CLivoxLaser::g_devices[kMaxLidarCount] = { 0 };
-
-std::map<uint8_t, std::string>	CLivoxLaser::g_handle_sn= std::map<uint8_t, std::string>();
-std::map<std::string, uint8_t>	CLivoxLaser::g_sn_handle = std::map<std::string, uint8_t>();
-std::map<std::string, CLivoxLaser*> CLivoxLaser::g_sn_laser= std::map<std::string, CLivoxLaser*>();
-CLivoxLaser*						CLivoxLaser::g_all_laser[kMaxLidarCount] = { 0 };
-unsigned int  CLivoxLaser::g_count[kMaxLidarCount] = { 0 };
-
-
-
-
-
-CLivoxLaser::CLivoxLaser(int id, Laser_proto::laser_parameter laser_param)
-:Laser_base(id, laser_param)
-{
-	//设备livox扫描最大帧数
-	m_frame_maxnum = laser_param.frame_num();
-	//判断参数类型，
-	if(laser_param.type()=="Livox")
-	{
-		//填充雷达设备的广播码
-		g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
-		//初始化livox
-		InitLivox();
-	}
-}
-
-CLivoxLaser::~CLivoxLaser()
-{
-
-}
-
-//雷达链接设备，为3个线程添加线程执行函数。
-Error_manager CLivoxLaser::connect_laser()
-{
-    Error_manager t_error;
-    //设置点云变换矩阵
-    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();
-    t_error = set_laser_matrix(matrix, 12);
-    if ( t_error != Error_code::SUCCESS )
-    {
-        return t_error;
-    }
-	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)
-{
-	//LOG(INFO) << " CLivoxLaser::execute_task start  "<< this;
-	Error_manager t_error;
-	Error_manager t_result;
-
-	//检查指针
-	if (p_laser_task == NULL) {
-		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-							 "Laser_base::porform_task failed, POINTER_IS_NULL");
-	}
-	//检查任务类型，
-	if (p_laser_task->get_task_type() != LASER_TASK)
-	{
-		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
-							 "laser task type error ！= LASER_TASK");
-	}
-
-	//接受任务，并将任务的状态改为TASK_SIGNED已签收
-	mp_laser_task = (Laser_task *) p_laser_task;
-	mp_laser_task->set_task_statu(TASK_SIGNED);
-
-	//检查消息内容是否正确，
-	//检查三维点云指针
-	if (mp_laser_task->get_task_point_cloud()== NULL)
-	{
-		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
-											Error_level::MINOR_ERROR,
-											"execute_task mp_task_point_cloud is null");
-		//将任务的状态改为 TASK_OVER 结束任务
-		mp_laser_task->set_task_statu(TASK_OVER);
-		//返回错误码
-		mp_laser_task->set_task_error_manager(t_result);
-		return t_result;
-	}
-	else
-	{
-		m_frame_maxnum=mp_laser_task->get_task_frame_maxnum();
-
-
-		//设置保存文件的路径
-		std::string save_path = mp_laser_task->get_save_path();
-		t_error = set_open_save_path(save_path,m_laser_param.is_save_txt());
-		if ( t_error != Error_code::SUCCESS )
-		{
-			//文件保存文件的路径的设置 允许失败。继续后面的动作
-			t_result.compare_and_cover_error(t_error);
-		}
-		//启动雷达扫描
-		t_error = start_scan();
-		if ( t_error != Error_code::SUCCESS )
-		{
-			t_result.compare_and_cover_error(t_error);
-
-			//将任务的状态改为 TASK_OVER 结束任务
-			mp_laser_task->set_task_statu(TASK_OVER);
-			//返回错误码
-			mp_laser_task->set_task_error_manager(t_result);
-			return t_result;
-		}
-		else
-		{
-			//将任务的状态改为 TASK_WORKING 处理中
-			mp_laser_task->set_task_statu(TASK_WORKING);
-		}
-	}
-	//返回错误码
-	if (t_result != Error_code::SUCCESS)
-	{
-		mp_laser_task->set_task_error_manager(t_result);
-	}
-	return t_result;
-}
-
-//检查雷达状态,是否正常运行
-Error_manager CLivoxLaser::check_laser()
-{
-	return Laser_base::check_laser();
-}
-//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-Error_manager CLivoxLaser::start_scan()
-{
-	//LOG(INFO) << " livox start :"<<this;
-	//清空livox子类的队列，
-	m_queue_livox_data.clear_and_delete();
-	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 Laser_base::end_task();
-}
-
-//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-bool CLivoxLaser::is_ready()
-{
-
-	//livox雷达设备的状态，livox sdk后台线程的状态
-	if ( m_laser_statu == LASER_READY &&
-		 g_devices[m_handle].device_state != kDeviceStateDisconnect  )
-	{
-		return true;
-	}
-	else
-	{
-	    return false;
-	}
-}
-
-//接受二进制消息的功能函数，每次只接受一个CBinaryData
-// 纯虚函数，必须由子类重载，
-bool CLivoxLaser::receive_buf_to_queue(Binary_buf& binary_buf)
-{
-	Binary_buf* tp_livox_buf;
-	if (m_queue_livox_data.try_pop(tp_livox_buf))
-	{
-		binary_buf = *tp_livox_buf;
-		delete tp_livox_buf;
-		//std::cout << "huli m_queue_livox_data.try_pop" << std::endl;
-		return true;
-	}
-	return false;
-}
-//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-// 纯虚函数，必须由子类重载，
-Buf_type CLivoxLaser::transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)
-{
-	if ( p_binary_buf ==NULL )
-	{
-		return BUF_UNKNOW;
-	}
-	else
-	{
-		//livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
-		LivoxRawPoint *tp_Livox_data = (LivoxRawPoint *)p_binary_buf->get_buf();
-		//计算这一帧数据有多少个三维点。
-		int t_count = p_binary_buf->get_length() / (sizeof(LivoxRawPoint));
-
-		if (t_count <= 0)
-		{
-			return BUF_UNKNOW;
-		}
-		else
-		{
-			//转变三维点格式，并存入vector。
-			for (int i = 0; i < t_count; ++i)
-			{
-				//LivoxRawPoint 转为 CPoint3D
-				//int32_t 转 double。不要信号强度
-				LivoxRawPoint t_livox_point = tp_Livox_data[i];
-				CPoint3D t_point3D;
-				t_point3D.x = t_livox_point.x;
-				t_point3D.y = t_livox_point.y;
-				t_point3D.z = t_livox_point.z;
-				point3D_cloud.push_back(t_point3D);
-//				std::cout << "huli points:" << t_point3D.x << " : " << t_point3D.y << " : " << t_point3D.z<< std::endl;
-			}
-			return BUF_DATA;
-		}
-	}
-
-}
-
-
-
-void CLivoxLaser::InitLivox()
-{
-	static bool g_init = false;
-	if (g_init == false)
-	{
-		if (!Init()) {
-			LOG(INFO) << "livox sdk init failed...";
-		}
-		else
-		{
-			LivoxSdkVersion _sdkversion;
-			GetLivoxSdkVersion(&_sdkversion);
-			char buf[255] = { 0 };
-			sprintf(buf, "Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
-			LOG(INFO) << buf;
-			SetBroadcastCallback(OnDeviceBroadcast);
-			SetDeviceStateUpdateCallback(OnDeviceChange);
-			g_init = true;
-		}
-	}
-}
-
-bool CLivoxLaser::IsScanComplete()
-{
-	//雷达的采集帧数判断，直接比较任务单里面的帧数最大值
-	if(mp_laser_task!=NULL)
-		return g_count[m_handle] >= mp_laser_task->get_task_frame_maxnum();
-	else
-		return false;
-}
-
-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(livox_status status, uint8_t handle, uint8_t response, void *data) {
-	CLivoxLaser* laser = (CLivoxLaser*)data;
-    LOG(INFO)<<"122333-------------------------------";
-	if (status == kStatusSuccess) {
-		if (response != 0) {
-			g_devices[handle].device_state = kDeviceStateConnect;
-		}
-	}
-	else if (status == kStatusTimeout) {
-		g_devices[handle].device_state = kDeviceStateConnect;
-	}
-}
-
-void CLivoxLaser::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
-{
-
-	if (info == NULL) {
-		return;
-	}
-
-	uint8_t handle = info->handle;
-	if (handle >= kMaxLidarCount) {
-		return;
-	}
-	/*std::cout<<"-----------------------------------------------------------------"<<std::endl;
-    std::cout<<" OnDeviceChange  handle :   "<<info->broadcast_code<<"  type : "<<type <<"  device_state : "
-                <<g_devices[handle].device_state<<std::endl;*/
-
-	if (type == kEventConnect) {
-		//QueryDeviceInformation(handle, OnDeviceInformation, NULL);
-		if (g_devices[handle].device_state == kDeviceStateDisconnect)
-		{
-			g_devices[handle].device_state = kDeviceStateConnect;
-			g_devices[handle].info = *info;
-		}
-	}
-	else if (type == kEventDisconnect) {
-		g_devices[handle].device_state = kDeviceStateDisconnect;
-	}
-	else if (type == kEventStateChange) {
-		g_devices[handle].info = *info;
-	}
-
-	if (g_devices[handle].device_state == kDeviceStateConnect)
-	{
-
-		if (g_devices[handle].info.state == kLidarStateNormal)
-		{
-			if (g_devices[handle].info.type == kDeviceTypeHub)
-			{
-				HubStartSampling(OnSampleCallback, NULL);
-			}
-			else
-			{
-				LidarStartSampling(handle, OnSampleCallback, NULL);
-			}
-			g_devices[handle].device_state = kDeviceStateSampling;
-		}
-	}
-}
-void CLivoxLaser::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
-{
-	if (info == NULL) {
-		return;
-	}
-
-	//printf("Receive Broadcast Code %s\n", info->broadcast_code);
-	LOG(INFO) << " broadcast  sn : " << info->broadcast_code;
-	bool result = false;
-	uint8_t handle = 0;
-	result = AddLidarToConnect(info->broadcast_code, &handle);
-
-	if (result == kStatusSuccess) {
-		/** Set the point cloud data for a specific Livox LiDAR. */
-		SetDataCallback(handle, LidarDataCallback, NULL);
-		g_devices[handle].handle = handle;
-		g_devices[handle].device_state = kDeviceStateDisconnect;
-
-		std::string sn = info->broadcast_code;
-		if (g_handle_sn.find(handle) != g_handle_sn.end())
-			g_handle_sn[handle] = sn;
-		else
-			g_handle_sn.insert(std::make_pair(handle,sn));
-
-		if (g_sn_handle.find(sn) != g_sn_handle.end())
-			g_sn_handle[sn] = handle;
-		else
-			g_sn_handle.insert(std::make_pair(sn,handle));
-
-	}
-
-}
-
-void CLivoxLaser::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *client_data)
-{
-//	std::cout << "huli LidarDataCallback" << std::endl;
-
-	CLivoxLaser* livox = g_all_laser[handle];
-	if (livox == NULL)
-	{
-		if (g_handle_sn.find(handle) != g_handle_sn.end())
-		{
-			std::string sn = g_handle_sn[handle];
-			if (g_sn_laser.find(sn) != g_sn_laser.end())
-			{
-				livox = g_sn_laser[sn];
-				g_all_laser[handle] = livox;
-				if (livox)
-					livox->UpdataHandle();
-			}
-		}
-	}
-//	std::cout << "huli LidarDataCallback " << std::endl;
-
-
-	if (data && livox)
-	{
-//		std::cout << "huli m_laser_scan_flag = " << livox->m_laser_scan_flag << std::endl;
-
-		//判断雷达扫描标志位
-		if (livox->m_laser_scan_flag)
-		{
-			if (livox->IsScanComplete())
-			{
-				//std::cout << "huli qwe point count = " << livox->g_count[livox->m_handle] << std::endl;
-				livox->stop_scan();
-				return;
-			}
-			/*else
-			{
-				//std::cout << "huli asd point count = " << livox->g_count[livox->m_handle] << std::endl;
-
-			}*/
-//			std::cout << "huli LidarDataCallback " << std::endl;
-
-			if (g_count[handle] >= livox->m_frame_maxnum)
-				return;
-			uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
-            if(data ->data_type == kCartesian)
-            {
-                LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
-                Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
-                livox->m_queue_livox_data.push(data_bin);
-            }
-            else if(data ->data_type == kExtendCartesian)
-            {
-                LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
-                Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxExtendRawPoint));
-                livox->m_queue_livox_data.push(data_bin);
-            }
-            else
-            {
-                return;
-            }
-
-			g_count[handle]++;
-
-		}
-        livox->m_laser_statu = LASER_READY;
-	}
-	/*else
-	{
-		//std::cout << "huli 2z error " << "data = "<< data  << std::endl;
-		//std::cout << "huli 2z error " << "livox = "<< livox  << std::endl;
-	}*/
-}

laser/LivoxLaser.h

@@ -1,96 +0,0 @@
-#ifndef __LIVOX_MID_40_LIDAR__H
-#define __LIVOX_MID_40_LIDAR__H
-#include "Laser.h"
-#include "livox_def.h"
-#include "livox_sdk.h"
-#include <map>
-
-//大疆livox雷达，从Laser_base继承。
-class CLivoxLaser :public Laser_base
-{
-protected:
-	//雷达设备状态，livox管理底层sdk，后台线程的工作状态
-	typedef enum
-	{
-		kDeviceStateDisconnect = 0,		//雷达设备状态	断开连接
-		kDeviceStateConnect = 1,		//雷达设备状态	连接正常
-		kDeviceStateSampling = 2,		//雷达设备状态	正在扫描
-	} DeviceState;
-
-	//雷达设备信息，
-	typedef struct
-	{
-		uint8_t handle;					//雷达控制句柄
-		DeviceState device_state;		//雷达设备状态
-		DeviceInfo info;				//雷达基本信息
-	} DeviceItem;
-
-
-public:
-	CLivoxLaser() = delete;
-	CLivoxLaser(const CLivoxLaser& other) = delete;
-	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
-	//input：id： 雷达设备的id，（唯一索引）
-	//input：laser_param：雷达的参数，
-	//注：利用protobuf创建 laser_parameter 类，然后从文件读取参数
-	CLivoxLaser(int id, Laser_proto::laser_parameter laser_param);
-	~CLivoxLaser();
-
-	//雷达链接设备，为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();
-
-	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-	virtual bool is_ready();
-
-protected:
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	// 纯虚函数，必须由子类重载，
-	virtual bool receive_buf_to_queue(Binary_buf& binary_buf);
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud);
-
-
-
-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);
-	static void OnSampleCallback(livox_status status, uint8_t handle, uint8_t response, void *data);
-
-protected:
-	uint8_t									m_handle;
-	unsigned int							m_frame_maxnum;
-	Thread_safe_queue<Binary_buf*>			m_queue_livox_data;
-
-	static DeviceItem						g_devices[kMaxLidarCount];
-	
-	static std::map<uint8_t,std::string>	g_handle_sn;
-	static std::map<std::string, uint8_t>	g_sn_handle;
-	static std::map<std::string, CLivoxLaser*> g_sn_laser;
-	static CLivoxLaser*						g_all_laser[kMaxLidarCount];
-	static unsigned int						g_count[kMaxLidarCount];
-
-
-};
-
-#endif
-

laser/LivoxLaser.puml

@@ -1,230 +0,0 @@
-@startuml
-skinparam classAttributeIconSize 0
-
-<<<<<<< HEAD
-title  CLivoxLaser 大疆livox雷达
-=======
-title  CLivoxLaser
->>>>>>> origin/hl
-
-
-
-
-class CLivoxLaser
-{
-//大疆livox雷达，从Laser_base继承。
-==protected:==
-	//雷达设备状态，livox管理底层sdk，后台线程的工作状态
-	typedef enum
-	{
-		kDeviceStateDisconnect = 0,		//雷达设备状态	断开连接
-		kDeviceStateConnect = 1,		//雷达设备状态	连接正常
-		kDeviceStateSampling = 2,		//雷达设备状态	正在扫描
-	} DeviceState;
-..
-	//雷达设备信息，
-	typedef struct
-	{
-		uint8_t handle;					//雷达控制句柄
-		DeviceState device_state;		//雷达设备状态
-		DeviceInfo info;				//雷达基本信息
-	} DeviceItem;
-==public:==
-	CLivoxLaser() = delete;
-	CLivoxLaser(const CLivoxLaser& other) = delete;
-	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
-	CLivoxLaser(int id, Laser_proto::laser_parameter laser_param);
-	~CLivoxLaser();
-..
-	//雷达链接设备，为3个线程添加线程执行函数。
-	virtual Error_manager connect_laser();
-	//雷达断开链接，释放3个线程
-	virtual Error_manager disconnect_laser();
-	//对外的接口函数，负责接受并处理任务单，
-	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();
-..
-	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-	virtual bool is_ready();
-==protected:==
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	virtual bool receive_buf_to_queue(Binary_buf& binary_buf);
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud);
-==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);
-	static void OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data);
-==protected:==
-	uint8_t									m_handle;
-	unsigned int							m_frame_maxnum;
-	Thread_safe_queue<Binary_buf*>			m_queue_livox_data;
-	static DeviceItem						g_devices[kMaxLidarCount];
-	static std::map<uint8_t,std::string>	g_handle_sn;
-	static std::map<std::string, uint8_t>	g_sn_handle;
-	static std::map<std::string, CLivoxLaser*> g_sn_laser;
-	static CLivoxLaser*						g_all_laser[kMaxLidarCount];
-	static unsigned int						g_count[kMaxLidarCount];
-}
-
-
-class Laser_base
-{
-//雷达的基类，不能直接使用，必须子类继承
-==public:==
-	Laser_base() = delete;
-	Laser_base(const Laser_base& other) = delete;
-..
-	//唯一的构造函数，按照设备名称和雷达参数来创建实例。
-	//input：id： 雷达设备的id，（唯一索引）
-	//input：laser_param：雷达的参数，
-	//注：利用protobuf创建 laser_parameter 类，然后从文件读取参数
-	Laser_base(int laser_id,Laser_proto::laser_parameter laser_param);
-	//析构函数
-	~Laser_base();
-..
-	//雷达链接设备，为3个线程添加线程执行函数。
-	virtual Error_manager connect_laser();
-	//雷达断开链接，释放3个线程
-	virtual Error_manager disconnect_laser();
-	//对外的接口函数，负责接受并处理任务单，
-	//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-	//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-	virtual Error_manager execute_task(Task_Base* p_laser_task);
-	//检查雷达状态,是否正常运行
-	virtual Error_manager check_laser();
-	//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-	virtual Error_manager start_scan();
-	//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
-	virtual Error_manager stop_scan();
-	//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
-	virtual Error_manager end_task();
-==public:==
-	//设置保存文件的路径，并打开文件，
-	Error_manager set_open_save_path(std::string save_path,bool is_save=true);
-	//关闭保存文件，推出前一定要执行
-	Error_manager close_save_path();
-	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-	virtual bool is_ready();
-	//获取雷达id
-	int get_laser_id();
-==protected:==
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	// 纯虚函数，必须由子类重载，
-	virtual bool receive_buf_to_queue(Binary_buf& binary_buf) = 0;
-	//线程执行函数，将二进制消息存入队列缓存，
-	void thread_receive();
-..
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)=0;
-	//线程执行函数，转化并处理三维点云。
-	void thread_transform();
-..
-	//公开发布雷达信息的功能函数，
-	Error_manager publish_laser_to_message();
-	//线程执行函数，公开发布雷达的相关信息，用作上位机的监视。
-	static void thread_publish(Laser_base* p_laser);
-..
-	//获取雷达状态
-	Laser_statu get_laser_statu();
-==protected:==
-	//初始化变换矩阵，设置默认值
-	Error_manager init_laser_matrix();
-	//设置变换矩阵，用作三维点的坐标变换，
-	Error_manager set_laser_matrix(double* p_matrix, int size);
-..
-	//三维点的坐标变换的功能函数，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-	virtual CPoint3D transform_by_matrix(CPoint3D point);
-==protected:==
-	//为了保证多线程的数据安全，修改共享数据必须加锁。  atomic 和 安全队列 可以不加锁进行读写
-	//建议：判断标志位使用 atomic， 容器要封装并使用 安全容器，
-	std::mutex          				m_laser_lock;            	//雷达数据锁
-..
-	std::atomic<int>					m_laser_id;                 //雷达设备id
-	Laser_proto::laser_parameter  		m_laser_param;				//雷达的配置参数
-	//雷达变换矩阵，三维点的坐标变换的矩阵，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
-	//必须在set_laser_matrix之后，才能使用。（connect时，从雷达的配置参数导入）
-	double								mp_laser_matrix[LASER_MATRIX_ARRAY_SIZE];	//雷达变换矩阵
-	//雷达扫描事件的标志位，受start和stop控制，然后其他线程判断m_scan_flag标准位，来进行启停。
-	std::atomic<bool>                	m_laser_scan_flag;          //雷达扫描的使能标志位
-	//雷达状态和任务状态同步，m_scan_flag停止之后，还要等任务执行完成,才会切回 ready。
-	std::atomic<Laser_statu>			m_laser_statu;             	//雷达工作状态，基类三线程的状态
-	//注：m_laser_statu是基类的三线程的状态，和livox sdk后台线程状态没有任何关系。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-..
-	std::atomic<int>            		m_points_count;         	//雷达采集点的计数
-	Thread_safe_queue<Binary_buf*>		m_queue_laser_data;         //二进制缓存的队列容器
-	Binary_buf							m_last_data;			    //上一个二进制缓存，用作数据拼接
-..
-	std::atomic<bool> 					m_save_flag;           		//雷达保存文件的使能标志位
-	std::string							m_save_path;            	//雷达保存文件的保存路径
-	CLogFile							m_binary_log_tool;          //二进制缓存的日志工具
-	CLogFile							m_points_log_tool;          //三维点云的日志工具
-..
-	//线程指针的内存管理，由Connect和Disconnect进行分配和释放。
-	std::thread*						mp_thread_receive;    		//接受缓存的线程指针
-	Thread_condition					m_condition_receive;		//接受缓存的条件变量
-	std::thread*						mp_thread_transform;   		//转化数据的线程指针
-	Thread_condition					m_condition_transform;		//转化数据的条件变量
-	std::thread*        				mp_thread_publish;   		//发布信息的线程指针
-	Thread_condition					m_condition_publish;		//发布信息的条件变量
-..
-	//任务单的指针，实际内存由应用层管理，
-	//接受任务后，指向新的任务单
-	Laser_task *  						mp_laser_task;        		//任务单的指针
-}
-
-class CLivoxMid100Laser
-{
-==public:==
-	CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param);
-	~CLivoxMid100Laser();
-..
-	//雷达链接设备，为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();
-..
-	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-	virtual bool is_ready();
-==protected:==
-	virtual bool IsScanComplete();
-	virtual void UpdataHandle();
-==protected:==
-	uint8_t		m_handle1;
-	uint8_t		m_handle2;
-	uint8_t		m_handle3;
-}
-
-Laser_base -> CLivoxLaser : inherit
-CLivoxLaser -> CLivoxMid100Laser : inherit
-
-
-@enduml

laser/LivoxMid100Laser.cpp

@@ -1,213 +0,0 @@
-
-#include "LivoxMid100Laser.h"
-#include <glog/logging.h>
-RegisterLaser(LivoxMid100)
-
-
-CLivoxMid100Laser::CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param)
-:CLivoxLaser(id, laser_param)
-{
-	//设备livox扫描最大帧数
-    m_frame_maxnum = laser_param.frame_num();
-	//判断参数类型，
-    if (laser_param.type() == "LivoxMid100")
-    {
-        std::string sn = laser_param.sn();
-        std::string sn1 = sn, sn2 = sn, sn3 = sn;
-        sn1 += "1";
-        sn2 += "2";
-        sn3 += "3";
-        g_sn_laser.insert(std::make_pair(sn1, this));
-        g_sn_laser.insert(std::make_pair(sn2, this));
-        g_sn_laser.insert(std::make_pair(sn3, this));
-		//初始化livox
-		InitLivox();
-    }
-}
-CLivoxMid100Laser::~CLivoxMid100Laser()
-{
-
-}
-
-//雷达链接设备，为3个线程添加线程执行函数。
-Error_manager CLivoxMid100Laser::connect_laser()
-{
-
-	return CLivoxLaser::connect_laser();
-}
-//雷达断开链接，释放3个线程
-Error_manager CLivoxMid100Laser::disconnect_laser()
-{
-	return CLivoxLaser::disconnect_laser();
-}
-
-
-
-//对外的接口函数，负责接受并处理任务单，
-//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
-//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
-Error_manager CLivoxMid100Laser::execute_task(Task_Base* p_laser_task)
-{
-	LOG(INFO) << " CLivoxMid100Laser::execute_task start  "<< this;
-	Error_manager t_error;
-	Error_manager t_result;
-
-	//检查指针
-	if (p_laser_task == NULL) {
-		return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-							 "Laser_base::porform_task failed, POINTER_IS_NULL");
-	}
-	//检查任务类型，
-	if (p_laser_task->get_task_type() != LASER_TASK)
-	{
-		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
-							 "laser task type error ！= LASER_TASK");
-	}
-    //检查当前状态
-    t_error=check_laser();
-	if(t_error!=SUCCESS)
-    {
-	    return t_error;
-    }
-	//接受任务，并将任务的状态改为TASK_SIGNED已签收
-	mp_laser_task = (Laser_task *) p_laser_task;
-	mp_laser_task->set_task_statu(TASK_SIGNED);
-
-	//检查消息内容是否正确，
-	//检查三维点云指针
-	if (mp_laser_task->get_task_point_cloud() == NULL)
-	{
-		t_result.error_manager_reset(Error_code::POINTER_IS_NULL,
-									 Error_level::MINOR_ERROR,
-									 "execute_task mp_task_point_cloud is null");
-		//将任务的状态改为 TASK_OVER 结束任务
-		mp_laser_task->set_task_statu(TASK_OVER);
-		//返回错误码
-		mp_laser_task->set_task_error_manager(t_result);
-		return t_result;
-	}
-	else
-	{
-		m_frame_maxnum=mp_laser_task->get_task_frame_maxnum();
-
-
-		//设置保存文件的路径
-		std::string save_path = mp_laser_task->get_save_path();
-
-		t_error = set_open_save_path(save_path,m_laser_param.is_save_txt());
-		if ( t_error != Error_code::SUCCESS )
-		{
-			//文件保存文件的路径的设置 允许失败。继续后面的动作
-			t_result.compare_and_cover_error(t_error);
-		}
-		//启动雷达扫描
-		t_error = start_scan();
-		if ( t_error != Error_code::SUCCESS )
-		{
-			t_result.compare_and_cover_error(t_error);
-
-			//将任务的状态改为 TASK_OVER 结束任务
-			mp_laser_task->set_task_statu(TASK_OVER);
-			//返回错误码
-			mp_laser_task->set_task_error_manager(t_result);
-			return t_result;
-		}
-		else
-		{
-			//将任务的状态改为 TASK_WORKING 处理中
-			mp_laser_task->set_task_statu(TASK_WORKING);
-		}
-	}
-	//返回错误码
-	if (t_result != Error_code::SUCCESS)
-	{
-		mp_laser_task->set_task_error_manager(t_result);
-	}
-	return t_result;
-}
-
-//检查雷达状态,是否正常运行
-Error_manager CLivoxMid100Laser::check_laser()
-{
-	return CLivoxLaser::check_laser();
-}
-
-//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
-Error_manager CLivoxMid100Laser::start_scan()
-{
-	LOG(INFO) << " livoxMid100 start :" << m_laser_param.sn();
-	//清空livox子类的队列，
-	m_queue_livox_data.clear_and_delete();
-	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 CLivoxLaser::end_task();
-}
-//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-bool CLivoxMid100Laser::is_ready()
-{
-	//3个livox雷达设备的状态，livox sdk后台线程的状态
-	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 && m_laser_statu == LASER_READY )
-	{
-		true;
-	}
-	else
-	{
-		false;
-	}
-}
-
-
-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

@@ -1,46 +0,0 @@
-#pragma once
-#include "LivoxLaser.h"
-#include "livox_def.h"
-#include "livox_sdk.h"
-#include <map>
-class CLivoxMid100Laser : public CLivoxLaser
-{
-
-public:
-	CLivoxMid100Laser(int id, Laser_proto::laser_parameter laser_param);
-	~CLivoxMid100Laser();
-
-	//雷达链接设备，为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();
-
-	//判断雷达状态是否为待机，如果已经准备好，则可以执行任务。
-	//子类重载 is_ready()，里面增加livox sdk后台线程状态的判断。
-	virtual bool is_ready();
-
-
-protected:
-	virtual bool IsScanComplete();
-	virtual void UpdataHandle();
-
-
-protected:
-	uint8_t		m_handle1;
-	uint8_t		m_handle2;
-	uint8_t		m_handle3;
-};
-
-

laser/LivoxMid70.cpp

@@ -0,0 +1,164 @@
+//
+// Created by zx on 2021/12/15.
+//
+
+#include "LivoxMid70.h"
+namespace livox
+{
+
+TimedLivoxExtendRawPoint::TimedLivoxExtendRawPoint()
+{
+
+    m_point_num=0;
+    m_frame= nullptr;
+    m_timeout=0;
+    m_time_point=std::chrono::steady_clock::now();
+}
+
+void TimedLivoxExtendRawPoint::Reset(LivoxExtendRawPoint* data ,int count,float timeout_time){
+    if(m_frame!=nullptr)
+    {
+        free(m_frame);
+    }
+    m_frame=(LivoxExtendRawPoint*)malloc(sizeof(LivoxExtendRawPoint)*count);
+    m_point_num=count;
+    memcpy(m_frame,data,count*sizeof(LivoxExtendRawPoint));
+    m_time_point=std::chrono::steady_clock::now();
+    m_timeout=timeout_time;
+}
+TimedLivoxExtendRawPoint::~TimedLivoxExtendRawPoint()
+{
+    if(m_frame!=nullptr)
+    {
+        free(m_frame);
+        m_frame=nullptr;
+        m_point_num=0;
+    }
+}
+
+void TimedLivoxExtendRawPoint::operator=(const TimedLivoxExtendRawPoint& frame)
+{
+    if(m_frame!=nullptr)
+    {
+        free(m_frame);
+    }
+    m_point_num=frame.m_point_num;
+    m_frame=(LivoxExtendRawPoint*)malloc(sizeof(LivoxExtendRawPoint)*frame.m_point_num);
+    memcpy(m_frame,frame.m_frame,m_point_num*sizeof(LivoxExtendRawPoint));
+    m_time_point=frame.m_time_point;
+    m_timeout=frame.m_timeout;
+}
+
+bool TimedLivoxExtendRawPoint::is_timeout(){
+    auto tic=std::chrono::steady_clock::now();
+    auto duration = std::chrono::duration_cast<std::chrono::microseconds>(tic - m_time_point);
+
+    double tm=double(duration.count()) * std::chrono::microseconds::period::num /
+            std::chrono::microseconds::period::den;
+
+    return tm>m_timeout;
+}
+
+
+LivoxMid70::LivoxMid70(){
+    m_idx=0;
+    m_fps=2.0;
+    m_transform=Eigen::Affine3f::Identity();
+}
+LivoxMid70::~LivoxMid70(){}
+
+void LivoxMid70::SetTransformParam(Eigen::Vector3f rpy,Eigen::Vector3f transpose)
+{
+    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(rpy(0),Eigen::Vector3f::UnitX()));
+    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(rpy(1),Eigen::Vector3f::UnitY()));
+    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(rpy(2),Eigen::Vector3f::UnitZ()));
+
+    Eigen::Matrix3f rotation_matrix;
+    rotation_matrix=yawAngle*pitchAngle*rollAngle;
+    m_transform.rotate(rotation_matrix);
+    m_transform.translation()<<transpose;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr LivoxMid70::GetCloud()
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+    /*printf("idx:%d\n",m_idx);
+    m_frames[m_idx].is_timeout();
+    int idc=m_idx-1;
+    idc=(idc>=0)?idc:MAX_FRAME+idc;
+    printf("idx-1:%d\n",idc);
+    m_frames[idc].is_timeout();*/
+    TimedLivoxExtendRawPoint     frames[MAX_FRAME];
+    m_mutex.lock();
+    for(int i=0;i<MAX_FRAME;++i)
+    {
+        frames[i]=m_frames[i];
+    }
+    m_mutex.unlock();
+
+    int id=m_idx-1;
+    for(int i=0;i<MAX_FRAME;++i)
+    {
+        int idc=id-i;
+        idc=(idc>=0)?idc:MAX_FRAME+idc;
+
+        if(frames[idc].is_timeout()==false)
+        {
+            for (int j = 0; j < frames[idc].m_point_num; ++j)
+            {
+                pcl::PointXYZ point;
+                point.x = frames[idc].m_frame[j].x/1000.0;
+                point.y = frames[idc].m_frame[j].y/1000.0;
+                point.z = frames[idc].m_frame[j].z/1000.0;
+
+                if(fabs(point.x)>8||fabs(point.y)>8||fabs(point.z)>8)
+                    continue;
+                cloud->push_back(point);
+            }
+
+        }
+        else
+        {
+            break;
+        }
+    }
+
+    //变换点云
+    if(cloud->size()>0)
+    {
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_t(new pcl::PointCloud<pcl::PointXYZ>);
+        pcl::transformPointCloud(*cloud,*cloud_t,m_transform);
+        return cloud_t;
+
+    }
+    return cloud;
+}
+
+
+void LivoxMid70::LidarDataCallback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num)
+{
+    LivoxEthPacket *eth_packet = data;
+
+    if (!data || !data_num || (handle >= kMaxLidarCount))
+    {
+        return;
+    }
+
+    if (eth_packet)
+    {
+        //uint64_t cur_timestamp = *((uint64_t *) (data->timestamp));
+
+        LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *) data->data;
+
+
+
+        //printf("  create point num:%d\n", data_num);
+        m_mutex.lock();
+        m_frames[m_idx].Reset(p_point_data, data_num,1.0/m_fps);
+        m_idx = (m_idx + 1) % MAX_FRAME;
+        m_mutex.unlock();
+
+    }
+}
+}

laser/LivoxMid70.h

@@ -0,0 +1,64 @@
+//
+// Created by zx on 2021/12/15.
+//
+
+#ifndef SHUTTER_VERIFY_LASER_LIVOXMID70_H_
+#define SHUTTER_VERIFY_LASER_LIVOXMID70_H_
+
+#include "lds_lidar.h"
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/transforms.h>
+#include <mutex>
+namespace livox
+{
+#define MAX_FRAME 1024
+class TimedLivoxExtendRawPoint
+{
+ public:
+    std::mutex  m_mutex;
+    LivoxExtendRawPoint*     m_frame;
+    int m_point_num;
+    std::chrono::steady_clock::time_point   m_time_point;
+    float m_timeout;
+
+    TimedLivoxExtendRawPoint();
+
+    void Reset(LivoxExtendRawPoint* data ,int count,float timeout_time=0.5);
+    void operator=(const TimedLivoxExtendRawPoint& frame);
+    ~TimedLivoxExtendRawPoint();
+
+    bool is_timeout();
+
+};
+
+class LivoxMid70 : public LdsLidar
+{
+ public:
+    LivoxMid70();
+    virtual ~LivoxMid70();
+
+    void SetFPS(float fps)
+    {
+        m_fps=fps;
+    }
+    pcl::PointCloud<pcl::PointXYZ>::Ptr GetCloud();
+    void SetTransformParam(Eigen::Vector3f rpy,Eigen::Vector3f transpose);
+
+ private:
+    virtual void LidarDataCallback(uint8_t handle, LivoxEthPacket *data,uint32_t data_num);
+
+
+ protected:
+    std::mutex                      m_mutex;
+    TimedLivoxExtendRawPoint     m_frames[MAX_FRAME];
+    int                     m_idx;
+
+    float                   m_fps;
+    Eigen::Affine3f         m_transform;
+};
+}
+
+
+#endif //SHUTTER_VERIFY_LASER_LIVOXMID70_H_

laser/LogFiles.cpp

@@ -1,145 +0,0 @@
-
-#include "LogFiles.h"
-#include <string.h>
-
-const string CLogFiles::m_strFileNameLoglaserBinary1 = "laser1.data";
-const string CLogFiles::m_strFileNameLoglaserValue1 = "laser1.txt";
-const string CLogFiles::m_strFileNameLogPoints1 = "points1.txt";
-
-const string CLogFiles::m_strFileNameLoglaserBinary2 = "laser2.data";
-const string CLogFiles::m_strFileNameLoglaserValue2 = "laser2.txt";
-const string CLogFiles::m_strFileNameLogPoints2 = "points2.txt";
-
-
-
-CLogFile::CLogFile()
-{
-
-}
-
-
-CLogFile::~CLogFile()
-{
-	if (m_file)
-	{
-        m_lock.lock();
-		m_file.close();
-        m_lock.unlock();
-	}
-}
-
-void CLogFile::open(const char *_Filename, bool binary /* = false */)
-{
-	if (!m_file.is_open())
-	{
-        m_lock.lock();
-		if (binary)
-			m_file.open(_Filename, ios_base::out | ios_base::binary);
-		else
-			m_file.open(_Filename, ios_base::out);
-        m_lock.unlock();
-	}
-}
-
-void CLogFile::write(const char *_Str,	streamsize _Count)
-{
-	if (m_file.is_open())
-	{
-        m_lock.lock();
-		m_file.write(_Str, _Count);
-        m_lock.unlock();
-	}
-}
-
-void CLogFile::write_double(double val)
-{
-	char buffer[512] = { 0 };
-	if (m_file.is_open())
-	{
-        m_lock.lock();
-        sprintf(buffer, "%f ", val);
-		m_file.write(buffer, strlen(buffer));
-        m_lock.unlock();
-	}
-}
-
-void CLogFile::write_int(int val)
-{
-	char buffer[512] = { 0 };
-	if (m_file.is_open())
-	{
-        m_lock.lock();
-        sprintf(buffer, "%d ", val);
-		m_file.write(buffer, strlen(buffer));
-        m_lock.unlock();
-	}
-}
-
-void CLogFile::close()
-{
-	if (m_file.is_open())
-	{
-        m_lock.lock();
-		m_file.close();
-        m_lock.unlock();
-	}
-}
-
-/****************************************************************************/
-
-CLogFiles::CLogFiles()
-	: m_bBinary(true)
-	, m_bValue(true)
-	, m_bPoints(true)
-{
-
-}
-
-CLogFiles::~CLogFiles()
-{
-	close_project();
-}
-
-void CLogFiles::close_project()
-{
-	m_logLaserBinary1.close();
-	m_logLaserValue1.close();
-	m_logPoints1.close();
-
-	m_logLaserBinary2.close();
-	m_logLaserValue2.close();
-	m_logPoints2.close();
-}
-
-void CLogFiles::new_project(const char * path)
-{
-	close_project();
-
-	if (m_bBinary) 
-	{
-		string filepath1 = string(path) + m_strFileNameLoglaserBinary1;
-		m_logLaserBinary1.open(filepath1.c_str(), true);
-
-		string filepath3 = string(path) + m_strFileNameLoglaserBinary2;
-		m_logLaserBinary2.open(filepath3.c_str(), true);
-	}
-
-	if (m_bValue)
-	{
-		string filepath1 = string(path) + m_strFileNameLoglaserValue1;
-		m_logLaserValue1.open(filepath1.c_str(), true);
-
-		string filepath3 = string(path) + m_strFileNameLoglaserValue2;
-		m_logLaserValue2.open(filepath3.c_str(), true);
-	}
-
-	if (m_bPoints)
-	{
-		string filepath2 = string(path) + m_strFileNameLogPoints1;
-		m_logPoints1.open(filepath2.c_str(), true);
-
-		string filepath4 = string(path) + m_strFileNameLogPoints2;
-		m_logPoints2.open(filepath4.c_str(), true);
-	}
-	
-}

laser/LogFiles.h

@@ -1,73 +0,0 @@
-#ifndef __LOG__FILE__HH
-#define __LOG__FILE__HH
-#include <string>
-#include <fstream>
-#include <mutex>
-
-using namespace std;
-
-class CLogFile
-{
-public:
-	CLogFile();
-	~CLogFile();
-
-	void open(const char *_Filename, bool binary = false);
-	void write(const char *_Str, streamsize _Count);
-	void close();
-	inline bool is_open() const { return m_file.is_open(); };
-
-	void write_double(double val);
-	void write_int(int val);
-
-public:
-	fstream m_file;
-
-public:
-    std::mutex m_lock;
-};
-
-
-class CLogFiles
-{
-public:
-	CLogFiles();
-	~CLogFiles();
-
-	void set_config(bool binary = true, bool value = true, bool points = true)
-	{
-		m_bBinary = binary;
-		m_bValue = value;
-		m_bPoints = points;
-	}
-
-	void new_project(const char * path);
-	void close_project();
-
-private:
-	bool m_bBinary;
-	bool m_bValue;
-	bool m_bPoints;
-
-
-public:		  
-	CLogFile m_logLaserBinary1;
-	CLogFile m_logLaserValue1;
-	CLogFile m_logPoints1;
-		
-	CLogFile m_logLaserBinary2;
-	CLogFile m_logLaserValue2;
-	CLogFile m_logPoints2;
-
-
-public:
-	static const string m_strFileNameLoglaserBinary1;
-	static const string m_strFileNameLoglaserValue1;
-	static const string m_strFileNameLogPoints1;
-
-	static const string m_strFileNameLoglaserBinary2;
-	static const string m_strFileNameLoglaserValue2;
-	static const string m_strFileNameLogPoints2;
-};
-
-#endif

laser/Point2D.cpp

@@ -1,24 +0,0 @@
-
-#include "Point2D.h"
-
-
-CPoint2D::CPoint2D(double x, double y)
-{
-	this->x = x;
-	this->y = y;
-}
-
-
-CPoint2D::~CPoint2D()
-{
-}
-
-double CPoint2D::GetDistance(CPoint2D &pt1, CPoint2D &pt2)
-{
-	return sqrt((pt1.x - pt2.x)*(pt1.x - pt2.x) + (pt1.y - pt2.y)*(pt1.y - pt2.y));
-}
-
-double CPoint2D::GetDistance(CPoint2D &pt)
-{
-	return sqrt((x - pt.x)*(x - pt.x) + (y - pt.y)*(y - pt.y));
-}

laser/Point2D.h

@@ -1,35 +0,0 @@
-#ifndef POINT_2D__HH__
-#define POINT_2D__HH__
-#include <math.h>
-
-#ifndef PI
-#define PI 3.141592653589793238462643383279502f   
-#endif
-
-#ifndef DEGREES
-#define DEGREES 0.01745329251994329576923690768489f                           
-#endif
-
-class CPoint2D
-{
-public:
-	double x, y;
-
-public:
-	CPoint2D(double x = 0, double y = 0);
-	virtual ~CPoint2D();
-	
-	inline double GetDistance(CPoint2D &pt);
-	static double GetDistance(CPoint2D &pt1, CPoint2D &pt2);
-};
-
-class CPointPolar2D
-{
-public:
-	double dist, theta;
-
-public:
-	CPointPolar2D(double d = 0, double t = 0) : dist(.0), theta(.0) { dist = d; theta = t; };
-	virtual ~CPointPolar2D() {};
-};
-#endif

laser/Point3D.cpp

@@ -1,34 +0,0 @@
-
-#include "Point3D.h"
-
-
-CPoint3D::CPoint3D()
-{
-	x = y = z = 0;
-}
-
-
-CPoint3D::~CPoint3D()
-{
-
-}
-
-CPoint3D::CPoint3D(double c1, double c2, double c3)
-{
-    x = c1;      y = c2;      z = c3;
-}
-
-
-
-CPoint3D &CPoint3D::operator=(const CPoint3D& pt) //ÖØÔصȺÅ
-{
-	x = pt.x;   z = pt.z;   y = pt.y;
-    return *this;
-}
-
-void CPoint3D::Translate(double cx, double cy, double cz) //ת»¯
-{
-    x += cx;
-    y += cy;
-    z += cz;
-}

laser/Point3D.h

@@ -1,19 +0,0 @@
-#ifndef __POINT_3D__HH
-#define __POINT_3D__HH
-class CPoint3D
-{
-public:
-	double x, y, z;
-
-	CPoint3D();
-	~CPoint3D();
-    CPoint3D(double c1, double c2, double c3);
-    CPoint3D& operator=(const CPoint3D& pt);
-	CPoint3D(const CPoint3D& pt)
-	{
-		*this = pt;
-	}
-    void Translate(double cx, double cy, double cz); //ת��
-};
-
-#endif

laser/Sick511FileLaser.cpp

@@ -1,323 +0,0 @@
-
-#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::transform_buf_to_points(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::receive_buf_to_queue(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 0;
-	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

@@ -1,55 +0,0 @@
-/*************************
-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:
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	// 纯虚函数，必须由子类重载，
-	virtual bool receive_buf_to_queue(Binary_buf& data);
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(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

@@ -1,343 +0,0 @@
-#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 ");
-
-	const 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::transform_buf_to_points(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::receive_buf_to_queue(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 0;
-	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(const 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

@@ -1,68 +0,0 @@
-/*************************
-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:
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	// 纯虚函数，必须由子类重载，
-	virtual bool receive_buf_to_queue(Binary_buf& data);
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points);
-
-
-private:
-	bool	Send(const 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

@@ -1,352 +0,0 @@
-
-#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 ");
-
-	const char* sendMsg = (char*)"$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()
-{
-	const 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::transform_buf_to_points(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::receive_buf_to_queue(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 0;
-	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(const 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

@@ -1,61 +0,0 @@
-/*************************
-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:
-	//接受二进制消息的功能函数，每次只接受一个CBinaryData
-	// 纯虚函数，必须由子类重载，
-	virtual bool receive_buf_to_queue(Binary_buf& data);
-	//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
-	// 纯虚函数，必须由子类重载，
-	virtual Buf_type transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points);
-	
-
-private:
-	bool Send(const 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
-};
-

laser/laser_message.pb.cc

@@ -1,432 +0,0 @@
-// Generated by the protocol buffer compiler.  DO NOT EDIT!
-// source: laser_message.proto
-
-#include "laser_message.pb.h"
-
-#include <algorithm>
-
-#include <google/protobuf/io/coded_stream.h>
-#include <google/protobuf/extension_set.h>
-#include <google/protobuf/wire_format_lite.h>
-#include <google/protobuf/descriptor.h>
-#include <google/protobuf/generated_message_reflection.h>
-#include <google/protobuf/reflection_ops.h>
-#include <google/protobuf/wire_format.h>
-// @@protoc_insertion_point(includes)
-#include <google/protobuf/port_def.inc>
-namespace laser_message {
-class laserMsgDefaultTypeInternal {
- public:
-  ::PROTOBUF_NAMESPACE_ID::internal::ExplicitlyConstructed<laserMsg> _instance;
-} _laserMsg_default_instance_;
-}  // namespace laser_message
-static void InitDefaultsscc_info_laserMsg_laser_5fmessage_2eproto() {
-  GOOGLE_PROTOBUF_VERIFY_VERSION;
-
-  {
-    void* ptr = &::laser_message::_laserMsg_default_instance_;
-    new (ptr) ::laser_message::laserMsg();
-    ::PROTOBUF_NAMESPACE_ID::internal::OnShutdownDestroyMessage(ptr);
-  }
-  ::laser_message::laserMsg::InitAsDefaultInstance();
-}
-
-::PROTOBUF_NAMESPACE_ID::internal::SCCInfo<0> scc_info_laserMsg_laser_5fmessage_2eproto =
-    {{ATOMIC_VAR_INIT(::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase::kUninitialized), 0, 0, InitDefaultsscc_info_laserMsg_laser_5fmessage_2eproto}, {}};
-
-static ::PROTOBUF_NAMESPACE_ID::Metadata file_level_metadata_laser_5fmessage_2eproto[1];
-static const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* file_level_enum_descriptors_laser_5fmessage_2eproto[1];
-static constexpr ::PROTOBUF_NAMESPACE_ID::ServiceDescriptor const** file_level_service_descriptors_laser_5fmessage_2eproto = nullptr;
-
-const ::PROTOBUF_NAMESPACE_ID::uint32 TableStruct_laser_5fmessage_2eproto::offsets[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
-  PROTOBUF_FIELD_OFFSET(::laser_message::laserMsg, _has_bits_),
-  PROTOBUF_FIELD_OFFSET(::laser_message::laserMsg, _internal_metadata_),
-  ~0u,  // no _extensions_
-  ~0u,  // no _oneof_case_
-  ~0u,  // no _weak_field_map_
-  PROTOBUF_FIELD_OFFSET(::laser_message::laserMsg, laser_status_),
-  PROTOBUF_FIELD_OFFSET(::laser_message::laserMsg, queue_data_count_),
-  PROTOBUF_FIELD_OFFSET(::laser_message::laserMsg, cloud_count_),
-  PROTOBUF_FIELD_OFFSET(::laser_message::laserMsg, id_),
-  0,
-  1,
-  2,
-  3,
-};
-static const ::PROTOBUF_NAMESPACE_ID::internal::MigrationSchema schemas[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) = {
-  { 0, 9, sizeof(::laser_message::laserMsg)},
-};
-
-static ::PROTOBUF_NAMESPACE_ID::Message const * const file_default_instances[] = {
-  reinterpret_cast<const ::PROTOBUF_NAMESPACE_ID::Message*>(&::laser_message::_laserMsg_default_instance_),
-};
-
-const char descriptor_table_protodef_laser_5fmessage_2eproto[] PROTOBUF_SECTION_VARIABLE(protodesc_cold) =
-  "\n\023laser_message.proto\022\rlaser_message\"w\n\010"
-  "laserMsg\0220\n\014laser_status\030\001 \001(\0162\032.laser_m"
-  "essage.laserStatus\022\030\n\020queue_data_count\030\002"
-  " \001(\005\022\023\n\013cloud_count\030\003 \001(\005\022\n\n\002id\030\004 \002(\005*]\n"
-  "\013laserStatus\022\023\n\017eLaserConnected\020\000\022\026\n\022eLa"
-  "serDisconnected\020\001\022\016\n\neLaserBusy\020\002\022\021\n\reLa"
-  "serUnknown\020\003"
-  ;
-static const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable*const descriptor_table_laser_5fmessage_2eproto_deps[1] = {
-};
-static ::PROTOBUF_NAMESPACE_ID::internal::SCCInfoBase*const descriptor_table_laser_5fmessage_2eproto_sccs[1] = {
-  &scc_info_laserMsg_laser_5fmessage_2eproto.base,
-};
-static ::PROTOBUF_NAMESPACE_ID::internal::once_flag descriptor_table_laser_5fmessage_2eproto_once;
-const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_laser_5fmessage_2eproto = {
-  false, false, descriptor_table_protodef_laser_5fmessage_2eproto, "laser_message.proto", 252,
-  &descriptor_table_laser_5fmessage_2eproto_once, descriptor_table_laser_5fmessage_2eproto_sccs, descriptor_table_laser_5fmessage_2eproto_deps, 1, 0,
-  schemas, file_default_instances, TableStruct_laser_5fmessage_2eproto::offsets,
-  file_level_metadata_laser_5fmessage_2eproto, 1, file_level_enum_descriptors_laser_5fmessage_2eproto, file_level_service_descriptors_laser_5fmessage_2eproto,
-};
-
-// Force running AddDescriptors() at dynamic initialization time.
-static bool dynamic_init_dummy_laser_5fmessage_2eproto = (static_cast<void>(::PROTOBUF_NAMESPACE_ID::internal::AddDescriptors(&descriptor_table_laser_5fmessage_2eproto)), true);
-namespace laser_message {
-const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* laserStatus_descriptor() {
-  ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&descriptor_table_laser_5fmessage_2eproto);
-  return file_level_enum_descriptors_laser_5fmessage_2eproto[0];
-}
-bool laserStatus_IsValid(int value) {
-  switch (value) {
-    case 0:
-    case 1:
-    case 2:
-    case 3:
-      return true;
-    default:
-      return false;
-  }
-}
-
-
-// ===================================================================
-
-void laserMsg::InitAsDefaultInstance() {
-}
-class laserMsg::_Internal {
- public:
-  using HasBits = decltype(std::declval<laserMsg>()._has_bits_);
-  static void set_has_laser_status(HasBits* has_bits) {
-    (*has_bits)[0] |= 1u;
-  }
-  static void set_has_queue_data_count(HasBits* has_bits) {
-    (*has_bits)[0] |= 2u;
-  }
-  static void set_has_cloud_count(HasBits* has_bits) {
-    (*has_bits)[0] |= 4u;
-  }
-  static void set_has_id(HasBits* has_bits) {
-    (*has_bits)[0] |= 8u;
-  }
-  static bool MissingRequiredFields(const HasBits& has_bits) {
-    return ((has_bits[0] & 0x00000008) ^ 0x00000008) != 0;
-  }
-};
-
-laserMsg::laserMsg(::PROTOBUF_NAMESPACE_ID::Arena* arena)
-  : ::PROTOBUF_NAMESPACE_ID::Message(arena) {
-  SharedCtor();
-  RegisterArenaDtor(arena);
-  // @@protoc_insertion_point(arena_constructor:laser_message.laserMsg)
-}
-laserMsg::laserMsg(const laserMsg& from)
-  : ::PROTOBUF_NAMESPACE_ID::Message(),
-      _has_bits_(from._has_bits_) {
-  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
-  ::memcpy(&laser_status_, &from.laser_status_,
-    static_cast<size_t>(reinterpret_cast<char*>(&id_) -
-    reinterpret_cast<char*>(&laser_status_)) + sizeof(id_));
-  // @@protoc_insertion_point(copy_constructor:laser_message.laserMsg)
-}
-
-void laserMsg::SharedCtor() {
-  ::memset(&laser_status_, 0, static_cast<size_t>(
-      reinterpret_cast<char*>(&id_) -
-      reinterpret_cast<char*>(&laser_status_)) + sizeof(id_));
-}
-
-laserMsg::~laserMsg() {
-  // @@protoc_insertion_point(destructor:laser_message.laserMsg)
-  SharedDtor();
-  _internal_metadata_.Delete<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
-}
-
-void laserMsg::SharedDtor() {
-  GOOGLE_DCHECK(GetArena() == nullptr);
-}
-
-void laserMsg::ArenaDtor(void* object) {
-  laserMsg* _this = reinterpret_cast< laserMsg* >(object);
-  (void)_this;
-}
-void laserMsg::RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena*) {
-}
-void laserMsg::SetCachedSize(int size) const {
-  _cached_size_.Set(size);
-}
-const laserMsg& laserMsg::default_instance() {
-  ::PROTOBUF_NAMESPACE_ID::internal::InitSCC(&::scc_info_laserMsg_laser_5fmessage_2eproto.base);
-  return *internal_default_instance();
-}
-
-
-void laserMsg::Clear() {
-// @@protoc_insertion_point(message_clear_start:laser_message.laserMsg)
-  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
-  // Prevent compiler warnings about cached_has_bits being unused
-  (void) cached_has_bits;
-
-  cached_has_bits = _has_bits_[0];
-  if (cached_has_bits & 0x0000000fu) {
-    ::memset(&laser_status_, 0, static_cast<size_t>(
-        reinterpret_cast<char*>(&id_) -
-        reinterpret_cast<char*>(&laser_status_)) + sizeof(id_));
-  }
-  _has_bits_.Clear();
-  _internal_metadata_.Clear<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
-}
-
-const char* laserMsg::_InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) {
-#define CHK_(x) if (PROTOBUF_PREDICT_FALSE(!(x))) goto failure
-  _Internal::HasBits has_bits{};
-  ::PROTOBUF_NAMESPACE_ID::Arena* arena = GetArena(); (void)arena;
-  while (!ctx->Done(&ptr)) {
-    ::PROTOBUF_NAMESPACE_ID::uint32 tag;
-    ptr = ::PROTOBUF_NAMESPACE_ID::internal::ReadTag(ptr, &tag);
-    CHK_(ptr);
-    switch (tag >> 3) {
-      // optional .laser_message.laserStatus laser_status = 1;
-      case 1:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 8)) {
-          ::PROTOBUF_NAMESPACE_ID::uint64 val = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-          if (PROTOBUF_PREDICT_TRUE(::laser_message::laserStatus_IsValid(val))) {
-            _internal_set_laser_status(static_cast<::laser_message::laserStatus>(val));
-          } else {
-            ::PROTOBUF_NAMESPACE_ID::internal::WriteVarint(1, val, mutable_unknown_fields());
-          }
-        } else goto handle_unusual;
-        continue;
-      // optional int32 queue_data_count = 2;
-      case 2:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 16)) {
-          _Internal::set_has_queue_data_count(&has_bits);
-          queue_data_count_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // optional int32 cloud_count = 3;
-      case 3:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 24)) {
-          _Internal::set_has_cloud_count(&has_bits);
-          cloud_count_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      // required int32 id = 4;
-      case 4:
-        if (PROTOBUF_PREDICT_TRUE(static_cast<::PROTOBUF_NAMESPACE_ID::uint8>(tag) == 32)) {
-          _Internal::set_has_id(&has_bits);
-          id_ = ::PROTOBUF_NAMESPACE_ID::internal::ReadVarint64(&ptr);
-          CHK_(ptr);
-        } else goto handle_unusual;
-        continue;
-      default: {
-      handle_unusual:
-        if ((tag & 7) == 4 || tag == 0) {
-          ctx->SetLastTag(tag);
-          goto success;
-        }
-        ptr = UnknownFieldParse(tag,
-            _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(),
-            ptr, ctx);
-        CHK_(ptr != nullptr);
-        continue;
-      }
-    }  // switch
-  }  // while
-success:
-  _has_bits_.Or(has_bits);
-  return ptr;
-failure:
-  ptr = nullptr;
-  goto success;
-#undef CHK_
-}
-
-::PROTOBUF_NAMESPACE_ID::uint8* laserMsg::_InternalSerialize(
-    ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const {
-  // @@protoc_insertion_point(serialize_to_array_start:laser_message.laserMsg)
-  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
-  (void) cached_has_bits;
-
-  cached_has_bits = _has_bits_[0];
-  // optional .laser_message.laserStatus laser_status = 1;
-  if (cached_has_bits & 0x00000001u) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteEnumToArray(
-      1, this->_internal_laser_status(), target);
-  }
-
-  // optional int32 queue_data_count = 2;
-  if (cached_has_bits & 0x00000002u) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(2, this->_internal_queue_data_count(), target);
-  }
-
-  // optional int32 cloud_count = 3;
-  if (cached_has_bits & 0x00000004u) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(3, this->_internal_cloud_count(), target);
-  }
-
-  // required int32 id = 4;
-  if (cached_has_bits & 0x00000008u) {
-    target = stream->EnsureSpace(target);
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::WriteInt32ToArray(4, this->_internal_id(), target);
-  }
-
-  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
-    target = ::PROTOBUF_NAMESPACE_ID::internal::WireFormat::InternalSerializeUnknownFieldsToArray(
-        _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance), target, stream);
-  }
-  // @@protoc_insertion_point(serialize_to_array_end:laser_message.laserMsg)
-  return target;
-}
-
-size_t laserMsg::ByteSizeLong() const {
-// @@protoc_insertion_point(message_byte_size_start:laser_message.laserMsg)
-  size_t total_size = 0;
-
-  // required int32 id = 4;
-  if (_internal_has_id()) {
-    total_size += 1 +
-      ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
-        this->_internal_id());
-  }
-  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
-  // Prevent compiler warnings about cached_has_bits being unused
-  (void) cached_has_bits;
-
-  cached_has_bits = _has_bits_[0];
-  if (cached_has_bits & 0x00000007u) {
-    // optional .laser_message.laserStatus laser_status = 1;
-    if (cached_has_bits & 0x00000001u) {
-      total_size += 1 +
-        ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::EnumSize(this->_internal_laser_status());
-    }
-
-    // optional int32 queue_data_count = 2;
-    if (cached_has_bits & 0x00000002u) {
-      total_size += 1 +
-        ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
-          this->_internal_queue_data_count());
-    }
-
-    // optional int32 cloud_count = 3;
-    if (cached_has_bits & 0x00000004u) {
-      total_size += 1 +
-        ::PROTOBUF_NAMESPACE_ID::internal::WireFormatLite::Int32Size(
-          this->_internal_cloud_count());
-    }
-
-  }
-  if (PROTOBUF_PREDICT_FALSE(_internal_metadata_.have_unknown_fields())) {
-    return ::PROTOBUF_NAMESPACE_ID::internal::ComputeUnknownFieldsSize(
-        _internal_metadata_, total_size, &_cached_size_);
-  }
-  int cached_size = ::PROTOBUF_NAMESPACE_ID::internal::ToCachedSize(total_size);
-  SetCachedSize(cached_size);
-  return total_size;
-}
-
-void laserMsg::MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
-// @@protoc_insertion_point(generalized_merge_from_start:laser_message.laserMsg)
-  GOOGLE_DCHECK_NE(&from, this);
-  const laserMsg* source =
-      ::PROTOBUF_NAMESPACE_ID::DynamicCastToGenerated<laserMsg>(
-          &from);
-  if (source == nullptr) {
-  // @@protoc_insertion_point(generalized_merge_from_cast_fail:laser_message.laserMsg)
-    ::PROTOBUF_NAMESPACE_ID::internal::ReflectionOps::Merge(from, this);
-  } else {
-  // @@protoc_insertion_point(generalized_merge_from_cast_success:laser_message.laserMsg)
-    MergeFrom(*source);
-  }
-}
-
-void laserMsg::MergeFrom(const laserMsg& from) {
-// @@protoc_insertion_point(class_specific_merge_from_start:laser_message.laserMsg)
-  GOOGLE_DCHECK_NE(&from, this);
-  _internal_metadata_.MergeFrom<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(from._internal_metadata_);
-  ::PROTOBUF_NAMESPACE_ID::uint32 cached_has_bits = 0;
-  (void) cached_has_bits;
-
-  cached_has_bits = from._has_bits_[0];
-  if (cached_has_bits & 0x0000000fu) {
-    if (cached_has_bits & 0x00000001u) {
-      laser_status_ = from.laser_status_;
-    }
-    if (cached_has_bits & 0x00000002u) {
-      queue_data_count_ = from.queue_data_count_;
-    }
-    if (cached_has_bits & 0x00000004u) {
-      cloud_count_ = from.cloud_count_;
-    }
-    if (cached_has_bits & 0x00000008u) {
-      id_ = from.id_;
-    }
-    _has_bits_[0] |= cached_has_bits;
-  }
-}
-
-void laserMsg::CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) {
-// @@protoc_insertion_point(generalized_copy_from_start:laser_message.laserMsg)
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-void laserMsg::CopyFrom(const laserMsg& from) {
-// @@protoc_insertion_point(class_specific_copy_from_start:laser_message.laserMsg)
-  if (&from == this) return;
-  Clear();
-  MergeFrom(from);
-}
-
-bool laserMsg::IsInitialized() const {
-  if (_Internal::MissingRequiredFields(_has_bits_)) return false;
-  return true;
-}
-
-void laserMsg::InternalSwap(laserMsg* other) {
-  using std::swap;
-  _internal_metadata_.Swap<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(&other->_internal_metadata_);
-  swap(_has_bits_[0], other->_has_bits_[0]);
-  ::PROTOBUF_NAMESPACE_ID::internal::memswap<
-      PROTOBUF_FIELD_OFFSET(laserMsg, id_)
-      + sizeof(laserMsg::id_)
-      - PROTOBUF_FIELD_OFFSET(laserMsg, laser_status_)>(
-          reinterpret_cast<char*>(&laser_status_),
-          reinterpret_cast<char*>(&other->laser_status_));
-}
-
-::PROTOBUF_NAMESPACE_ID::Metadata laserMsg::GetMetadata() const {
-  return GetMetadataStatic();
-}
-
-
-// @@protoc_insertion_point(namespace_scope)
-}  // namespace laser_message
-PROTOBUF_NAMESPACE_OPEN
-template<> PROTOBUF_NOINLINE ::laser_message::laserMsg* Arena::CreateMaybeMessage< ::laser_message::laserMsg >(Arena* arena) {
-  return Arena::CreateMessageInternal< ::laser_message::laserMsg >(arena);
-}
-PROTOBUF_NAMESPACE_CLOSE
-
-// @@protoc_insertion_point(global_scope)
-#include <google/protobuf/port_undef.inc>

laser/laser_message.pb.h

@@ -1,432 +0,0 @@
-// Generated by the protocol buffer compiler.  DO NOT EDIT!
-// source: laser_message.proto
-
-#ifndef GOOGLE_PROTOBUF_INCLUDED_laser_5fmessage_2eproto
-#define GOOGLE_PROTOBUF_INCLUDED_laser_5fmessage_2eproto
-
-#include <limits>
-#include <string>
-
-#include <google/protobuf/port_def.inc>
-#if PROTOBUF_VERSION < 3013000
-#error This file was generated by a newer version of protoc which is
-#error incompatible with your Protocol Buffer headers. Please update
-#error your headers.
-#endif
-#if 3013000 < PROTOBUF_MIN_PROTOC_VERSION
-#error This file was generated by an older version of protoc which is
-#error incompatible with your Protocol Buffer headers. Please
-#error regenerate this file with a newer version of protoc.
-#endif
-
-#include <google/protobuf/port_undef.inc>
-#include <google/protobuf/io/coded_stream.h>
-#include <google/protobuf/arena.h>
-#include <google/protobuf/arenastring.h>
-#include <google/protobuf/generated_message_table_driven.h>
-#include <google/protobuf/generated_message_util.h>
-#include <google/protobuf/inlined_string_field.h>
-#include <google/protobuf/metadata_lite.h>
-#include <google/protobuf/generated_message_reflection.h>
-#include <google/protobuf/message.h>
-#include <google/protobuf/repeated_field.h>  // IWYU pragma: export
-#include <google/protobuf/extension_set.h>  // IWYU pragma: export
-#include <google/protobuf/generated_enum_reflection.h>
-#include <google/protobuf/unknown_field_set.h>
-// @@protoc_insertion_point(includes)
-#include <google/protobuf/port_def.inc>
-#define PROTOBUF_INTERNAL_EXPORT_laser_5fmessage_2eproto
-PROTOBUF_NAMESPACE_OPEN
-namespace internal {
-class AnyMetadata;
-}  // namespace internal
-PROTOBUF_NAMESPACE_CLOSE
-
-// Internal implementation detail -- do not use these members.
-struct TableStruct_laser_5fmessage_2eproto {
-  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTableField entries[]
-    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
-  static const ::PROTOBUF_NAMESPACE_ID::internal::AuxiliaryParseTableField aux[]
-    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
-  static const ::PROTOBUF_NAMESPACE_ID::internal::ParseTable schema[1]
-    PROTOBUF_SECTION_VARIABLE(protodesc_cold);
-  static const ::PROTOBUF_NAMESPACE_ID::internal::FieldMetadata field_metadata[];
-  static const ::PROTOBUF_NAMESPACE_ID::internal::SerializationTable serialization_table[];
-  static const ::PROTOBUF_NAMESPACE_ID::uint32 offsets[];
-};
-extern const ::PROTOBUF_NAMESPACE_ID::internal::DescriptorTable descriptor_table_laser_5fmessage_2eproto;
-namespace laser_message {
-class laserMsg;
-class laserMsgDefaultTypeInternal;
-extern laserMsgDefaultTypeInternal _laserMsg_default_instance_;
-}  // namespace laser_message
-PROTOBUF_NAMESPACE_OPEN
-template<> ::laser_message::laserMsg* Arena::CreateMaybeMessage<::laser_message::laserMsg>(Arena*);
-PROTOBUF_NAMESPACE_CLOSE
-namespace laser_message {
-
-enum laserStatus : int {
-  eLaserConnected = 0,
-  eLaserDisconnected = 1,
-  eLaserBusy = 2,
-  eLaserUnknown = 3
-};
-bool laserStatus_IsValid(int value);
-constexpr laserStatus laserStatus_MIN = eLaserConnected;
-constexpr laserStatus laserStatus_MAX = eLaserUnknown;
-constexpr int laserStatus_ARRAYSIZE = laserStatus_MAX + 1;
-
-const ::PROTOBUF_NAMESPACE_ID::EnumDescriptor* laserStatus_descriptor();
-template<typename T>
-inline const std::string& laserStatus_Name(T enum_t_value) {
-  static_assert(::std::is_same<T, laserStatus>::value ||
-    ::std::is_integral<T>::value,
-    "Incorrect type passed to function laserStatus_Name.");
-  return ::PROTOBUF_NAMESPACE_ID::internal::NameOfEnum(
-    laserStatus_descriptor(), enum_t_value);
-}
-inline bool laserStatus_Parse(
-    ::PROTOBUF_NAMESPACE_ID::ConstStringParam name, laserStatus* value) {
-  return ::PROTOBUF_NAMESPACE_ID::internal::ParseNamedEnum<laserStatus>(
-    laserStatus_descriptor(), name, value);
-}
-// ===================================================================
-
-class laserMsg PROTOBUF_FINAL :
-    public ::PROTOBUF_NAMESPACE_ID::Message /* @@protoc_insertion_point(class_definition:laser_message.laserMsg) */ {
- public:
-  inline laserMsg() : laserMsg(nullptr) {}
-  virtual ~laserMsg();
-
-  laserMsg(const laserMsg& from);
-  laserMsg(laserMsg&& from) noexcept
-    : laserMsg() {
-    *this = ::std::move(from);
-  }
-
-  inline laserMsg& operator=(const laserMsg& from) {
-    CopyFrom(from);
-    return *this;
-  }
-  inline laserMsg& operator=(laserMsg&& from) noexcept {
-    if (GetArena() == from.GetArena()) {
-      if (this != &from) InternalSwap(&from);
-    } else {
-      CopyFrom(from);
-    }
-    return *this;
-  }
-
-  inline const ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet& unknown_fields() const {
-    return _internal_metadata_.unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>(::PROTOBUF_NAMESPACE_ID::UnknownFieldSet::default_instance);
-  }
-  inline ::PROTOBUF_NAMESPACE_ID::UnknownFieldSet* mutable_unknown_fields() {
-    return _internal_metadata_.mutable_unknown_fields<::PROTOBUF_NAMESPACE_ID::UnknownFieldSet>();
-  }
-
-  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* descriptor() {
-    return GetDescriptor();
-  }
-  static const ::PROTOBUF_NAMESPACE_ID::Descriptor* GetDescriptor() {
-    return GetMetadataStatic().descriptor;
-  }
-  static const ::PROTOBUF_NAMESPACE_ID::Reflection* GetReflection() {
-    return GetMetadataStatic().reflection;
-  }
-  static const laserMsg& default_instance();
-
-  static void InitAsDefaultInstance();  // FOR INTERNAL USE ONLY
-  static inline const laserMsg* internal_default_instance() {
-    return reinterpret_cast<const laserMsg*>(
-               &_laserMsg_default_instance_);
-  }
-  static constexpr int kIndexInFileMessages =
-    0;
-
-  friend void swap(laserMsg& a, laserMsg& b) {
-    a.Swap(&b);
-  }
-  inline void Swap(laserMsg* other) {
-    if (other == this) return;
-    if (GetArena() == other->GetArena()) {
-      InternalSwap(other);
-    } else {
-      ::PROTOBUF_NAMESPACE_ID::internal::GenericSwap(this, other);
-    }
-  }
-  void UnsafeArenaSwap(laserMsg* other) {
-    if (other == this) return;
-    GOOGLE_DCHECK(GetArena() == other->GetArena());
-    InternalSwap(other);
-  }
-
-  // implements Message ----------------------------------------------
-
-  inline laserMsg* New() const final {
-    return CreateMaybeMessage<laserMsg>(nullptr);
-  }
-
-  laserMsg* New(::PROTOBUF_NAMESPACE_ID::Arena* arena) const final {
-    return CreateMaybeMessage<laserMsg>(arena);
-  }
-  void CopyFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
-  void MergeFrom(const ::PROTOBUF_NAMESPACE_ID::Message& from) final;
-  void CopyFrom(const laserMsg& from);
-  void MergeFrom(const laserMsg& from);
-  PROTOBUF_ATTRIBUTE_REINITIALIZES void Clear() final;
-  bool IsInitialized() const final;
-
-  size_t ByteSizeLong() const final;
-  const char* _InternalParse(const char* ptr, ::PROTOBUF_NAMESPACE_ID::internal::ParseContext* ctx) final;
-  ::PROTOBUF_NAMESPACE_ID::uint8* _InternalSerialize(
-      ::PROTOBUF_NAMESPACE_ID::uint8* target, ::PROTOBUF_NAMESPACE_ID::io::EpsCopyOutputStream* stream) const final;
-  int GetCachedSize() const final { return _cached_size_.Get(); }
-
-  private:
-  inline void SharedCtor();
-  inline void SharedDtor();
-  void SetCachedSize(int size) const final;
-  void InternalSwap(laserMsg* other);
-  friend class ::PROTOBUF_NAMESPACE_ID::internal::AnyMetadata;
-  static ::PROTOBUF_NAMESPACE_ID::StringPiece FullMessageName() {
-    return "laser_message.laserMsg";
-  }
-  protected:
-  explicit laserMsg(::PROTOBUF_NAMESPACE_ID::Arena* arena);
-  private:
-  static void ArenaDtor(void* object);
-  inline void RegisterArenaDtor(::PROTOBUF_NAMESPACE_ID::Arena* arena);
-  public:
-
-  ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadata() const final;
-  private:
-  static ::PROTOBUF_NAMESPACE_ID::Metadata GetMetadataStatic() {
-    ::PROTOBUF_NAMESPACE_ID::internal::AssignDescriptors(&::descriptor_table_laser_5fmessage_2eproto);
-    return ::descriptor_table_laser_5fmessage_2eproto.file_level_metadata[kIndexInFileMessages];
-  }
-
-  public:
-
-  // nested types ----------------------------------------------------
-
-  // accessors -------------------------------------------------------
-
-  enum : int {
-    kLaserStatusFieldNumber = 1,
-    kQueueDataCountFieldNumber = 2,
-    kCloudCountFieldNumber = 3,
-    kIdFieldNumber = 4,
-  };
-  // optional .laser_message.laserStatus laser_status = 1;
-  bool has_laser_status() const;
-  private:
-  bool _internal_has_laser_status() const;
-  public:
-  void clear_laser_status();
-  ::laser_message::laserStatus laser_status() const;
-  void set_laser_status(::laser_message::laserStatus value);
-  private:
-  ::laser_message::laserStatus _internal_laser_status() const;
-  void _internal_set_laser_status(::laser_message::laserStatus value);
-  public:
-
-  // optional int32 queue_data_count = 2;
-  bool has_queue_data_count() const;
-  private:
-  bool _internal_has_queue_data_count() const;
-  public:
-  void clear_queue_data_count();
-  ::PROTOBUF_NAMESPACE_ID::int32 queue_data_count() const;
-  void set_queue_data_count(::PROTOBUF_NAMESPACE_ID::int32 value);
-  private:
-  ::PROTOBUF_NAMESPACE_ID::int32 _internal_queue_data_count() const;
-  void _internal_set_queue_data_count(::PROTOBUF_NAMESPACE_ID::int32 value);
-  public:
-
-  // optional int32 cloud_count = 3;
-  bool has_cloud_count() const;
-  private:
-  bool _internal_has_cloud_count() const;
-  public:
-  void clear_cloud_count();
-  ::PROTOBUF_NAMESPACE_ID::int32 cloud_count() const;
-  void set_cloud_count(::PROTOBUF_NAMESPACE_ID::int32 value);
-  private:
-  ::PROTOBUF_NAMESPACE_ID::int32 _internal_cloud_count() const;
-  void _internal_set_cloud_count(::PROTOBUF_NAMESPACE_ID::int32 value);
-  public:
-
-  // required int32 id = 4;
-  bool has_id() const;
-  private:
-  bool _internal_has_id() const;
-  public:
-  void clear_id();
-  ::PROTOBUF_NAMESPACE_ID::int32 id() const;
-  void set_id(::PROTOBUF_NAMESPACE_ID::int32 value);
-  private:
-  ::PROTOBUF_NAMESPACE_ID::int32 _internal_id() const;
-  void _internal_set_id(::PROTOBUF_NAMESPACE_ID::int32 value);
-  public:
-
-  // @@protoc_insertion_point(class_scope:laser_message.laserMsg)
- private:
-  class _Internal;
-
-  template <typename T> friend class ::PROTOBUF_NAMESPACE_ID::Arena::InternalHelper;
-  typedef void InternalArenaConstructable_;
-  typedef void DestructorSkippable_;
-  ::PROTOBUF_NAMESPACE_ID::internal::HasBits<1> _has_bits_;
-  mutable ::PROTOBUF_NAMESPACE_ID::internal::CachedSize _cached_size_;
-  int laser_status_;
-  ::PROTOBUF_NAMESPACE_ID::int32 queue_data_count_;
-  ::PROTOBUF_NAMESPACE_ID::int32 cloud_count_;
-  ::PROTOBUF_NAMESPACE_ID::int32 id_;
-  friend struct ::TableStruct_laser_5fmessage_2eproto;
-};
-// ===================================================================
-
-
-// ===================================================================
-
-#ifdef __GNUC__
-  #pragma GCC diagnostic push
-  #pragma GCC diagnostic ignored "-Wstrict-aliasing"
-#endif  // __GNUC__
-// laserMsg
-
-// optional .laser_message.laserStatus laser_status = 1;
-inline bool laserMsg::_internal_has_laser_status() const {
-  bool value = (_has_bits_[0] & 0x00000001u) != 0;
-  return value;
-}
-inline bool laserMsg::has_laser_status() const {
-  return _internal_has_laser_status();
-}
-inline void laserMsg::clear_laser_status() {
-  laser_status_ = 0;
-  _has_bits_[0] &= ~0x00000001u;
-}
-inline ::laser_message::laserStatus laserMsg::_internal_laser_status() const {
-  return static_cast< ::laser_message::laserStatus >(laser_status_);
-}
-inline ::laser_message::laserStatus laserMsg::laser_status() const {
-  // @@protoc_insertion_point(field_get:laser_message.laserMsg.laser_status)
-  return _internal_laser_status();
-}
-inline void laserMsg::_internal_set_laser_status(::laser_message::laserStatus value) {
-  assert(::laser_message::laserStatus_IsValid(value));
-  _has_bits_[0] |= 0x00000001u;
-  laser_status_ = value;
-}
-inline void laserMsg::set_laser_status(::laser_message::laserStatus value) {
-  _internal_set_laser_status(value);
-  // @@protoc_insertion_point(field_set:laser_message.laserMsg.laser_status)
-}
-
-// optional int32 queue_data_count = 2;
-inline bool laserMsg::_internal_has_queue_data_count() const {
-  bool value = (_has_bits_[0] & 0x00000002u) != 0;
-  return value;
-}
-inline bool laserMsg::has_queue_data_count() const {
-  return _internal_has_queue_data_count();
-}
-inline void laserMsg::clear_queue_data_count() {
-  queue_data_count_ = 0;
-  _has_bits_[0] &= ~0x00000002u;
-}
-inline ::PROTOBUF_NAMESPACE_ID::int32 laserMsg::_internal_queue_data_count() const {
-  return queue_data_count_;
-}
-inline ::PROTOBUF_NAMESPACE_ID::int32 laserMsg::queue_data_count() const {
-  // @@protoc_insertion_point(field_get:laser_message.laserMsg.queue_data_count)
-  return _internal_queue_data_count();
-}
-inline void laserMsg::_internal_set_queue_data_count(::PROTOBUF_NAMESPACE_ID::int32 value) {
-  _has_bits_[0] |= 0x00000002u;
-  queue_data_count_ = value;
-}
-inline void laserMsg::set_queue_data_count(::PROTOBUF_NAMESPACE_ID::int32 value) {
-  _internal_set_queue_data_count(value);
-  // @@protoc_insertion_point(field_set:laser_message.laserMsg.queue_data_count)
-}
-
-// optional int32 cloud_count = 3;
-inline bool laserMsg::_internal_has_cloud_count() const {
-  bool value = (_has_bits_[0] & 0x00000004u) != 0;
-  return value;
-}
-inline bool laserMsg::has_cloud_count() const {
-  return _internal_has_cloud_count();
-}
-inline void laserMsg::clear_cloud_count() {
-  cloud_count_ = 0;
-  _has_bits_[0] &= ~0x00000004u;
-}
-inline ::PROTOBUF_NAMESPACE_ID::int32 laserMsg::_internal_cloud_count() const {
-  return cloud_count_;
-}
-inline ::PROTOBUF_NAMESPACE_ID::int32 laserMsg::cloud_count() const {
-  // @@protoc_insertion_point(field_get:laser_message.laserMsg.cloud_count)
-  return _internal_cloud_count();
-}
-inline void laserMsg::_internal_set_cloud_count(::PROTOBUF_NAMESPACE_ID::int32 value) {
-  _has_bits_[0] |= 0x00000004u;
-  cloud_count_ = value;
-}
-inline void laserMsg::set_cloud_count(::PROTOBUF_NAMESPACE_ID::int32 value) {
-  _internal_set_cloud_count(value);
-  // @@protoc_insertion_point(field_set:laser_message.laserMsg.cloud_count)
-}
-
-// required int32 id = 4;
-inline bool laserMsg::_internal_has_id() const {
-  bool value = (_has_bits_[0] & 0x00000008u) != 0;
-  return value;
-}
-inline bool laserMsg::has_id() const {
-  return _internal_has_id();
-}
-inline void laserMsg::clear_id() {
-  id_ = 0;
-  _has_bits_[0] &= ~0x00000008u;
-}
-inline ::PROTOBUF_NAMESPACE_ID::int32 laserMsg::_internal_id() const {
-  return id_;
-}
-inline ::PROTOBUF_NAMESPACE_ID::int32 laserMsg::id() const {
-  // @@protoc_insertion_point(field_get:laser_message.laserMsg.id)
-  return _internal_id();
-}
-inline void laserMsg::_internal_set_id(::PROTOBUF_NAMESPACE_ID::int32 value) {
-  _has_bits_[0] |= 0x00000008u;
-  id_ = value;
-}
-inline void laserMsg::set_id(::PROTOBUF_NAMESPACE_ID::int32 value) {
-  _internal_set_id(value);
-  // @@protoc_insertion_point(field_set:laser_message.laserMsg.id)
-}
-
-#ifdef __GNUC__
-  #pragma GCC diagnostic pop
-#endif  // __GNUC__
-
-// @@protoc_insertion_point(namespace_scope)
-
-}  // namespace laser_message
-
-PROTOBUF_NAMESPACE_OPEN
-
-template <> struct is_proto_enum< ::laser_message::laserStatus> : ::std::true_type {};
-template <>
-inline const EnumDescriptor* GetEnumDescriptor< ::laser_message::laserStatus>() {
-  return ::laser_message::laserStatus_descriptor();
-}
-
-PROTOBUF_NAMESPACE_CLOSE
-
-// @@protoc_insertion_point(global_scope)
-
-#include <google/protobuf/port_undef.inc>
-#endif  // GOOGLE_PROTOBUF_INCLUDED_GOOGLE_PROTOBUF_INCLUDED_laser_5fmessage_2eproto

laser/laser_message.proto

@@ -1,17 +0,0 @@
-syntax = "proto2";
-package laser_message;
-
-enum laserStatus
-{
-    eLaserConnected=0;
-    eLaserDisconnected=1;
-    eLaserBusy=2;
-    eLaserUnknown=3;
-}
-message laserMsg
-{
-    optional laserStatus laser_status=1;
-    optional int32 queue_data_count=2;
-    optional int32 cloud_count=3;
-    required int32 id=4;
-}

laser/laser_parameter.proto

@@ -1,40 +0,0 @@
-syntax = "proto2";
-package Laser_proto;
-
-message laser_parameter {
-    optional string laser_ip = 1;
-    optional int64 laser_port = 2;
-    optional int64 laser_port_remote = 3;
-    optional double mat_r00 = 4 [default = 1.0];
-    optional double mat_r01 = 5 [default = 1.0];
-    optional double mat_r02 = 6 [default = 1.0];
-    optional double mat_r03 = 7 [default = 1.0];
-    optional double mat_r10 = 8 [default = 1.0];
-    optional double mat_r11 = 9 [default = 1.0];
-    optional double mat_r12 = 10 [default = 1.0];
-    optional double mat_r13 = 11 [default = 1.0];
-    optional double mat_r20 = 12 [default = 1.0];
-    optional double mat_r21 = 13 [default = 1.0];
-    optional double mat_r22 = 14 [default = 1.0];
-    optional double mat_r23 = 15 [default = 1.0];
-    optional double axis_x_theta = 16;
-    optional double axis_y_theta = 17;
-    optional double axis_z_theta = 18;
-    optional double translation_x = 19;
-    optional double translation_y = 20;
-    optional double translation_z = 21;
-
-    optional double install_angle = 22 [default = 0.0];
-    optional bool scan_direction = 23 [default = true];
-    optional string sn = 24;
-    optional int64 frame_num = 25 [default = 3000];
-    optional string type = 26 [default = ""];
-    optional bool is_save_banary=27 [default=false];
-    optional bool is_save_txt=28 [default=true];
-
-}
-
-message Laser_parameter_all
-{
-    repeated laser_parameter        laser_parameters=1;
-}

laser/laser_task_command.cpp

@@ -1,189 +0,0 @@
-
-//laser_task_command，是雷达任务指令的相关功能
-//功能：用作应用层向laser模块传输任务，的指令消息
-
-//用法：应用层直接调用laser的接口函数，并将Laser_task类作为参数传递
-//CLaser类则按照Laser_task的功能码和指定的参数，执行相应的功能
-//并将结果填充到Laser_task，返回给应用层
-
-#include "laser_task_command.h"
-//构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
-Laser_task::Laser_task()
-{
-    //构造函数锁定任务类型为LASER_TASK，后续不允许更改
-    m_task_type = LASER_TASK;
-    m_task_statu = TASK_CREATED;
-    //m_task_statu_information默认为空
-
-    m_task_frame_maxnum = 0;
-    mp_task_point_cloud = NULL;
-    //m_task_error_manager 默认为空
-    mp_task_cloud_lock=NULL;
-}
-//析构函数
-Laser_task::~Laser_task()
-{
-
-}
-
-//初始化任务单，必须初始化之后才可以使用，（必选参数）
-// input:task_statu 任务状态
-// output:p_task_point_cloud 三维点云容器的智能指针
-// input:p_task_cloud_lock 三维点云的数据保护锁
-//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-Error_manager Laser_task::task_init(Task_statu task_statu,
-                                    pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
-                                    std::mutex* p_task_cloud_lock)
-{
-    if(p_task_point_cloud == NULL)
-    {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "Laser_task::task_init p_task_point_cloud is null");
-    }
-    if(p_task_cloud_lock==NULL)
-    {
-        return Error_manager(POINTER_IS_NULL,MINOR_ERROR,"laser task input lock is null");
-    }
-
-    m_task_statu = task_statu;
-    m_task_statu_information.clear();
-
-    m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
-    mp_task_cloud_lock=p_task_cloud_lock;
-    mp_task_point_cloud = p_task_point_cloud;
-    m_task_error_manager.error_manager_clear_all();
-
-    return Error_code::SUCCESS;
-}
-
-//初始化任务单，必须初始化之后才可以使用，（可选参数）
-//    input:task_statu任务状态
-//    input:task_statu_information状态说明
-//    input:task_frame_maxnum点云的采集帧数最大值
-//    output:p_task_point_cloud三维点云容器的智能指针
-// input:p_task_cloud_lock 三维点云的数据保护锁
-//注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-Error_manager Laser_task::task_init(Task_statu task_statu,
-                                    std::string & task_statu_information,
-                                    unsigned int task_frame_maxnum,
-                                    pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
-                                    std::mutex* p_task_cloud_lock)
-{
-    if(p_task_point_cloud == NULL)
-    {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "Laser_task::task_init p_task_point_cloud is null");
-    }
-    if(p_task_cloud_lock==NULL)
-    {
-        return Error_manager(POINTER_IS_NULL,MINOR_ERROR,"laser task input lock is null");
-    }
-
-    m_task_statu = task_statu;
-    m_task_statu_information = task_statu_information;
-
-    if(task_frame_maxnum == 0)
-    {
-        m_task_frame_maxnum = TASK_FRAME_MAXNUM_DEFAULT;
-    }
-    else
-    {
-        m_task_frame_maxnum = task_frame_maxnum;
-    }
-    mp_task_cloud_lock=p_task_cloud_lock;
-    mp_task_point_cloud = p_task_point_cloud;
-    m_task_error_manager.error_manager_clear_all();
-
-    return Error_code::SUCCESS;
-}
-
-//push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
-Error_manager Laser_task::task_push_point(pcl::PointXYZ point_xyz)
-{
-    if(mp_task_cloud_lock==NULL)
-    {
-        return Error_manager(POINTER_IS_NULL,MINOR_ERROR,
-                             "push_point laser task input lock is null");
-    }
-    if(mp_task_point_cloud==NULL)
-    {
-        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
-                             "Laser_task::task_push_point p_task_point_cloud is null");
-    }
-    //加锁，并添加三维点。
-    mp_task_cloud_lock->lock();
-    mp_task_point_cloud->push_back(point_xyz);
-    mp_task_cloud_lock->unlock();
-    return SUCCESS;
-}
-
-
-
-
-//获取 点云的采集帧数最大值
-unsigned int Laser_task::get_task_frame_maxnum()
-{
-    return m_task_frame_maxnum;
-}
-//获取采集的点云保存路径
-std::string Laser_task::get_task_save_path()
-{
-    return m_task_save_path;
-}
-//获取 三维点云容器的智能指针
-pcl::PointCloud<pcl::PointXYZ>* Laser_task::get_task_point_cloud()
-{
-    return mp_task_point_cloud;
-}
-
-
-
-
-//设置 任务状态
-void Laser_task::set_task_statu(Task_statu task_statu)
-{
-    m_task_statu = task_statu;
-}
-//设置 任务状态说明
-void Laser_task::set_task_statu_information(std::string & task_statu_information)
-{
-    m_task_statu_information = task_statu_information;
-}
-//设置 点云的采集帧数最大值
-void Laser_task::set_task_frame_maxnum(unsigned int task_frame_maxnum)
-{
-    m_task_frame_maxnum = task_frame_maxnum;
-
-}
-//设置采集的点云保存路径
-void Laser_task::set_task_save_path(std::string task_save_path)
-{
-    m_task_save_path=task_save_path;
-}
-//设置 三维点云容器的智能指针
-void Laser_task::set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud)
-{
-    mp_task_point_cloud = p_task_point_cloud;
-}
-//设置 错误码
-void Laser_task::set_task_error_manager(Error_manager & error_manager)
-{
-    m_task_error_manager = error_manager;
-}
-
-//获取采集的点云保存路径
-std::string Laser_task::get_save_path()
-{
-    return m_save_path;
-}
-//设置采集的点云保存路径
-void Laser_task::set_save_path(std::string save_path)
-{
-    m_save_path=save_path;
-}
-
-
-
-
-
-

laser/laser_task_command.h

@@ -1,111 +0,0 @@
-
-//laser_task_command，是雷达任务指令的相关功能
-//功能：用作应用层向laser模块传输任务，的指令消息
-
-//用法：应用层直接调用laser的接口函数，并将Laser_task类作为参数传递
-//CLaser类则按照Laser_task的功能码和指定的参数，执行相应的功能
-//并将结果填充到Laser_task，返回给应用层
-
-
-#ifndef __LASER_TASK_COMMAND__HH__
-#define __LASER_TASK_COMMAND__HH__
-#include "Point2D.h"
-#include "Point3D.h"
-#include <pcl/point_types.h>
-#include <pcl/common/common.h>
-#include "../error_code/error_code.h"
-
-#include <vector>
-#include <mutex>
-
-#include "../task/task_command_manager.h"
-
-//任务点云的采集帧数最大值，默认值1000
-#define TASK_FRAME_MAXNUM_DEFAULT               1000
-
-//雷达模块的任务指令，从Task_Base继承，
-//补充了雷达专属的数据输入和输出
-class Laser_task:public Task_Base
-{
-public:
-    //构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
-    Laser_task();
-    //析构函数
-    virtual ~Laser_task();
-
-    //初始化任务单，必须初始化之后才可以使用，（必选参数）
-    // input:task_statu 任务状态
-    // output:p_task_point_cloud 三维点云容器的智能指针
-    // input:p_task_cloud_lock 三维点云的数据保护锁
-    //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-    Error_manager task_init(Task_statu task_statu,
-                            pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock);
-
-    //初始化任务单，必须初始化之后才可以使用，（可选参数）
-    //    input:task_statu任务状态
-    //    input:task_statu_information状态说明
-    //    input:task_frame_maxnum点云的采集帧数最大值
-    //    output:p_task_point_cloud 三维点云容器的智能指针
-    // input:p_task_cloud_lock 三维点云的数据保护锁
-    //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-    Error_manager task_init(Task_statu task_statu,
-                            std::string & task_statu_information,
-                            unsigned int task_frame_maxnum,
-                            pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock);
-
-    //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
-    Error_manager task_push_point(pcl::PointXYZ point_xyz);
-
-
-public:
-    //获取 点云的采集帧数最大值
-    unsigned int get_task_frame_maxnum();
-    //获取采集的点云保存路径
-    std::string get_task_save_path();
-    //获取 三维点云容器的智能指针
-    pcl::PointCloud<pcl::PointXYZ>* get_task_point_cloud();
-
-
-
-
-    //设置 任务状态
-    void set_task_statu(Task_statu task_statu);
-    //设置 任务状态说明
-    void set_task_statu_information(std::string & task_statu_information);
-    //设置 点云的采集帧数最大值
-    void set_task_frame_maxnum(unsigned int task_frame_maxnum);
-    //设置采集的点云保存路径
-    void set_task_save_path(std::string task_save_path);
-    //设置 三维点云容器的智能指针
-    void set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>* p_task_point_cloud);
-    //设置 错误码
-    void set_task_error_manager(Error_manager & error_manager);
-
-    //获取采集的点云保存路径
-    std::string get_save_path();
-    //设置采集的点云保存路径
-    void set_save_path(std::string save_path);
-
-protected:
-    //点云的采集帧数最大值，任务输入
-    unsigned int                    m_task_frame_maxnum;
-
-    std::string                     m_save_path;
-
-    //点云保存文件的路径，任务输入
-    std::string                     m_task_save_path;
-
-    //三维点云的数据保护锁，任务输入
-    std::mutex*                     mp_task_cloud_lock;
-    //采集结果，三维点云容器的智能指针，任务输出
-    //这里只是指针，实际内存由应用层管理，初始化时必须保证内存有效。
-    pcl::PointCloud<pcl::PointXYZ>*        mp_task_point_cloud;
-
-
-};
-
-
-#endif //__LASER_TASK_COMMAND__HH__
-

laser/laser_task_command.puml

@@ -1,114 +0,0 @@
-@startuml
-skinparam classAttributeIconSize 0
-
-<<<<<<< HEAD
-title  Laser_task 雷达模块的任务指令
-=======
-title  binary_buf是二进制缓存
->>>>>>> origin/hl
-
-note left of Laser_task
-//laser_task_command，是雷达任务指令的相关功能
-//功能：用作应用层向laser模块传输任务，的指令消息
-
-//用法：应用层直接调用laser的接口函数，并将Laser_task类作为参数传递
-//CLaser类则按照Laser_task的功能码和指定的参数，执行相应的功能
-//并将结果填充到Laser_task，返回给应用层
-end note
-
-
-class Laser_task
-{
-//雷达模块的任务指令，从Task_Base继承，
-//补充了雷达专属的数据输入和输出
-==public:==
-    //构造函数，构造函数锁定任务类型为LASER_TASK，后续不允许更改
-    Laser_task();
-    //析构函数
-    ~Laser_task();
-..
-    //初始化任务单，必须初始化之后才可以使用，（必选参数）
-    // input:task_statu 任务状态
-    // output:p_task_point_cloud 三维点云容器的智能指针
-    // input:p_task_cloud_lock 三维点云的数据保护锁
-    //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-    Error_manager task_init(Task_statu task_statu,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock);
-..
-    //初始化任务单，必须初始化之后才可以使用，（可选参数）
-    //    input:task_statu任务状态
-    //    input:task_statu_information状态说明
-    //    input:task_frame_maxnum点云的采集帧数最大值
-    //    output:p_task_point_cloud 三维点云容器的智能指针
-    // input:p_task_cloud_lock 三维点云的数据保护锁
-    //注：task_frame_maxnum默认为0，如果为0，则使用laser默认的点云的采集帧数最大值 TASK_FRAME_MAXNUM_DEFAULT
-    Error_manager task_init(Task_statu task_statu,
-                            std::string & task_statu_information,
-                            unsigned int task_frame_maxnum,
-                            pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud,
-                            std::mutex* p_task_cloud_lock);
-..
-    //push点云，把 point_xyz 添加到 p_task_point_cloud 里面。
-    Error_manager task_push_point(pcl::PointXYZ point_xyz);
-==public:==
-    //获取 点云的采集帧数最大值
-    unsigned int get_task_frame_maxnum();
-    //获取采集的点云保存路径
-    std::string get_task_save_path();
-    //获取 三维点云容器的智能指针
-    pcl::PointCloud<pcl::PointXYZ>::Ptr get_task_point_cloud();
-..
-    //设置 任务状态
-    void set_task_statu(Task_statu task_statu);
-    //设置 任务状态说明
-    void set_task_statu_information(std::string & task_statu_information);
-    //设置 点云的采集帧数最大值
-    void set_task_frame_maxnum(unsigned int task_frame_maxnum);
-    //设置采集的点云保存路径
-    void set_task_save_path(std::string task_save_path);
-    //设置 三维点云容器的智能指针
-    void set_task_point_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr p_task_point_cloud);
-    //设置 错误码
-    void set_task_error_manager(Error_manager & error_manager);
-==protected:==
-    //点云的采集帧数最大值，任务输入
-    unsigned int                    m_task_frame_maxnum;
-    //点云保存文件的路径，任务输入
-    std::string                     m_task_save_path;
-    //三维点云的数据保护锁，任务输入
-    std::mutex*                     mp_task_cloud_lock;
-    //采集结果，三维点云容器的智能指针，任务输出
-    //这里只是指针，实际内存由应用层管理，初始化时必须保证内存有效。
-    pcl::PointCloud<pcl::PointXYZ>::Ptr        mp_task_point_cloud;
-}
-
-
-class Task_Base
-{
-//任务单基类
-==public:==
-    //初始化任务单,初始任务单类型为 UNKONW_TASK
-    virtual Error_manager init();
-..
-    //更新任务单
-    Error_manager update_statu(Task_statu task_statu,std::string statu_information="");
-..
-    //获取任务类型
-    Task_type   get_task_type();
-..
-    //获取任务单状态
-    Task_statu  get_statu();
-..
-    //获取状态说明
-    std::string get_statu_information();
-==protected:==
-    Task_type                   m_task_type;                    //任务类型
-    Task_statu                  m_task_statu;                   //任务状态
-    std::string                 m_task_statu_information;       //任务状态说明
-	Error_manager               m_task_error_manager;//错误码，任务故障信息，任务输出
-}
-
-Laser_task <-- Task_Base : inherit
-
-@enduml

laser/lds_lidar.cpp

@@ -0,0 +1,613 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+#include "lds_lidar.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <thread>
+#include <memory>
+
+namespace livox
+{
+
+/** Const varible ------------------------------------------------------------------------------- */
+/** User add broadcast code here */
+
+LidarDevice lidars_[kMaxLidarCount];
+bool is_initialized_=false;
+bool is_created=false;
+
+thread_safe_map<std::string,LdsLidar*>  g_snPtr;
+
+static const char *local_broadcast_code_list[] = {
+        "000000000000001",
+};
+
+
+/** Lds lidar function ---------------------------------------------------------------------------*/
+LdsLidar::LdsLidar()
+{
+    if(is_created==false)
+    {
+        auto_connect_mode_ = true;
+        whitelist_count_ = 0;
+        is_initialized_ = false;
+
+        lidar_count_ = 0;
+        memset(broadcast_code_whitelist_, 0, sizeof(broadcast_code_whitelist_));
+        memset(lidars_, 0, sizeof(lidars_));
+        for (uint32_t i = 0; i < kMaxLidarCount; i++)
+        {
+            lidars_[i].handle = kMaxLidarCount;
+            /** Unallocated state */
+            lidars_[i].connect_state = kConnectStateOff;
+        }
+        is_created=true;
+    }
+}
+
+LdsLidar::~LdsLidar()
+{
+}
+
+
+int LdsLidar::AddBroadcastCode(std::string sn)
+{
+    if(g_snPtr.find(sn)==true)
+    {
+        printf(" %s has added \n",sn.c_str());
+        return -1;
+    }
+    g_snPtr[sn]=this;
+    return 0;
+}
+
+
+void LdsLidar::LidarDataCallback(uint8_t handle, LivoxEthPacket *data,uint32_t data_num)
+{
+    printf("data recieved ------------------------------------ \n");
+}
+
+int LdsLidar::InitLdsLidar()
+{
+
+    if (is_initialized_==false)
+    {
+        if (!Init())
+        {
+            Uninit();
+            printf("Livox-SDK init fail!\n");
+            return -1;
+        }
+        LivoxSdkVersion _sdkversion;
+        GetLivoxSdkVersion(&_sdkversion);
+        printf("Livox SDK version %d.%d.%d\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
+    }
+
+    SetBroadcastCallback(LdsLidar::OnDeviceBroadcast);
+    SetDeviceStateUpdateCallback(LdsLidar::OnDeviceChange);
+
+    /** Add commandline input broadcast code */
+    /*for (auto input_str : broadcast_code_strs)
+    {
+        LdsLidar::AddBroadcastCodeToWhitelist(input_str.c_str());
+    }*/
+
+    /** Add local broadcast code */
+    /*LdsLidar::AddLocalBroadcastCode();
+
+    if (whitelist_count_)
+    {
+        LdsLidar::DisableAutoConnectMode();
+        printf("Disable auto connect mode!\n");
+
+        printf("List all broadcast code in whiltelist:\n");
+        for (uint32_t i = 0; i < whitelist_count_; i++)
+        {
+            printf("%s\n", broadcast_code_whitelist_[i]);
+        }
+    }
+    else
+    {
+        LdsLidar::EnableAutoConnectMode();
+        printf("No broadcast code was added to whitelist, swith to automatic connection mode!\n");
+    }*/
+
+    /** Start livox sdk to receive lidar data */
+    if (!Start())
+    {
+        Uninit();
+        printf("Livox-SDK init fail!\n");
+        return -1;
+    }
+    printf(" start -----------\n");
+
+    /** Add here, only for callback use */
+
+    is_initialized_ = true;
+    printf("Livox-SDK init success!\n");
+
+    return 0;
+}
+
+
+int LdsLidar::DeInitLdsLidar(void)
+{
+
+    if (!is_initialized_)
+    {
+        printf("LiDAR data source is not exit");
+        return -1;
+    }
+
+    Uninit();
+    printf("Livox SDK Deinit completely!\n");
+
+    return 0;
+}
+
+
+/** Static function in LdsLidar for callback or event process ------------------------------------*/
+
+/** Receiving point cloud data from Livox LiDAR. */
+void LdsLidar::GetLidarDataCb(uint8_t handle, LivoxEthPacket *data,
+                              uint32_t data_num, void *client_data)
+{
+    using namespace std;
+
+    LdsLidar *lidar_this = static_cast<LdsLidar *>(client_data);
+    //printf("receive handle %d   ptr %p\n", handle,client_data);
+    lidar_this->LidarDataCallback(handle,data,data_num);
+    /*LivoxEthPacket *eth_packet = data;
+
+    if (!data || !data_num || (handle >= kMaxLidarCount))
+    {
+        return;
+    }
+
+    if (eth_packet)
+    {
+        lidar_this->data_recveive_count_[handle]++;
+        if (lidar_this->data_recveive_count_[handle] % 100 == 0)
+        {
+            printf("receive packet count %d %d\n", handle, lidar_this->data_recveive_count_[handle]);
+
+
+            uint64_t cur_timestamp = *((uint64_t *) (data->timestamp));
+            if (data->data_type == kCartesian)
+            {
+                printf("1\n");
+                LivoxRawPoint *p_point_data = (LivoxRawPoint *) data->data;
+            }
+            else if (data->data_type == kSpherical)
+            {
+                LivoxSpherPoint *p_point_data = (LivoxSpherPoint *) data->data;
+                printf("2\n");
+            }
+            else if (data->data_type == kExtendCartesian)
+            {
+                LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *) data->data;
+            }
+            else if (data->data_type == kExtendSpherical)
+            {
+                printf("4\n");
+                LivoxExtendSpherPoint *p_point_data = (LivoxExtendSpherPoint *) data->data;
+            }
+            else if (data->data_type == kDualExtendCartesian)
+            {
+                LivoxDualExtendRawPoint *p_point_data = (LivoxDualExtendRawPoint *) data->data;
+            }
+            else if (data->data_type == kDualExtendSpherical)
+            {
+                LivoxDualExtendSpherPoint *p_point_data = (LivoxDualExtendSpherPoint *) data->data;
+            }
+            else if (data->data_type == kImu)
+            {
+                LivoxImuPoint *p_point_data = (LivoxImuPoint *) data->data;
+            }
+            else if (data->data_type == kTripleExtendCartesian)
+            {
+                LivoxTripleExtendRawPoint *p_point_data = (LivoxTripleExtendRawPoint *) data->data;
+            }
+            else if (data->data_type == kTripleExtendSpherical)
+            {
+                LivoxTripleExtendSpherPoint *p_point_data = (LivoxTripleExtendSpherPoint *) data->data;
+            }
+        }
+    }*/
+}
+
+
+void LdsLidar::OnDeviceBroadcast(const BroadcastDeviceInfo *info)
+{
+    if (info == nullptr)
+    {
+        return;
+    }
+
+    if (info->dev_type == kDeviceTypeHub)
+    {
+        printf("In lidar mode, couldn't connect a hub : %s\n", info->broadcast_code);
+        return;
+    }
+
+    /*if(g_snPtr.find(std::string(info->broadcast_code))==false)
+    {
+        printf("sn:%s has not added map size:%d\n",info->broadcast_code,g_snPtr.size());
+        return;
+    }*/
+    livox_status result = kStatusFailure;
+    uint8_t handle = 0;
+    result = AddLidarToConnect(info->broadcast_code, &handle);
+    if (result == kStatusSuccess && handle < kMaxLidarCount)
+    {
+        LdsLidar* lidar=g_snPtr[info->broadcast_code];
+
+        SetDataCallback(handle, LdsLidar::GetLidarDataCb, (void*)lidar);
+
+        LidarDevice *p_lidar = &(lidars_[handle]);
+        p_lidar->handle = handle;
+        p_lidar->connect_state = kConnectStateOff;
+        p_lidar->config.enable_fan = true;
+        p_lidar->config.return_mode = kStrongestReturn;
+        p_lidar->config.coordinate = kCoordinateCartesian;
+        p_lidar->config.imu_rate = kImuFreq200Hz;
+    }
+    else
+    {
+        printf("Add lidar to connect is failed : %d %d \n", result, handle);
+    }
+}
+
+/** Callback function of changing of device state. */
+void LdsLidar::OnDeviceChange(const DeviceInfo *info, DeviceEvent type)
+{
+    if (info == nullptr)
+    {
+        return;
+    }
+
+    uint8_t handle = info->handle;
+    if (handle >= kMaxLidarCount)
+    {
+        return;
+    }
+    if(g_snPtr.find(info->broadcast_code)==false)
+        return;
+    LdsLidar* ptr=g_snPtr[info->broadcast_code];
+
+    LidarDevice *p_lidar = &(lidars_[handle]);
+    if (type == kEventConnect)
+    {
+        QueryDeviceInformation(handle, DeviceInformationCb, ptr);
+        if (p_lidar->connect_state == kConnectStateOff)
+        {
+            p_lidar->connect_state = kConnectStateOn;
+            p_lidar->info = *info;
+        }
+        printf("[WARNING] Lidar sn: [%s] Connect!!!\n", info->broadcast_code);
+    }
+    else if (type == kEventDisconnect)
+    {
+        p_lidar->connect_state = kConnectStateOff;
+        printf("[WARNING] Lidar sn: [%s] Disconnect!!!\n", info->broadcast_code);
+    }
+    else if (type == kEventStateChange)
+    {
+        p_lidar->info = *info;
+        printf("[WARNING] Lidar sn: [%s] StateChange!!!\n", info->broadcast_code);
+    }
+
+    if (p_lidar->connect_state == kConnectStateOn)
+    {
+        printf("Device Working State %d\n", p_lidar->info.state);
+        if (p_lidar->info.state == kLidarStateInit)
+        {
+            printf("Device State Change Progress %u\n", p_lidar->info.status.progress);
+        }
+        else
+        {
+            printf("Device State Error Code 0X%08x\n", p_lidar->info.status.status_code.error_code);
+        }
+        printf("Device feature %d\n", p_lidar->info.feature);
+        SetErrorMessageCallback(handle, LdsLidar::LidarErrorStatusCb);
+
+
+        if (p_lidar->info.state == kLidarStateNormal)
+        {
+            if (p_lidar->config.coordinate != 0)
+            {
+                SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, ptr);
+            }
+            else
+            {
+                SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, ptr);
+            }
+            p_lidar->config.set_bits |= kConfigCoordinate;
+
+            if (kDeviceTypeLidarMid40 != info->type)
+            {
+                LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode) (p_lidar->config.return_mode), \
+                                     LdsLidar::SetPointCloudReturnModeCb, ptr);
+                p_lidar->config.set_bits |= kConfigReturnMode;
+            }
+
+            if (kDeviceTypeLidarMid40 != info->type && kDeviceTypeLidarMid70 != info->type)
+            {
+                LidarSetImuPushFrequency(handle, (ImuFreq) (p_lidar->config.imu_rate), \
+                                 LdsLidar::SetImuRatePushFrequencyCb, ptr);
+                p_lidar->config.set_bits |= kConfigImuRate;
+            }
+
+            p_lidar->connect_state = kConnectStateConfig;
+        }
+    }
+}
+
+/** Query the firmware version of Livox LiDAR. */
+void LdsLidar::DeviceInformationCb(livox_status status, uint8_t handle, \
+                         DeviceInformationResponse *ack, void *client_data)
+{
+    if (status != kStatusSuccess)
+    {
+        printf("Device Query Informations Failed : %d\n", status);
+    }
+    if (ack)
+    {
+        printf("firm ver: %d.%d.%d.%d\n",
+               ack->firmware_version[0],
+               ack->firmware_version[1],
+               ack->firmware_version[2],
+               ack->firmware_version[3]);
+    }
+}
+
+/** Callback function of Lidar error message. */
+void LdsLidar::LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message)
+{
+    static uint32_t error_message_count = 0;
+    if (message != NULL)
+    {
+        ++error_message_count;
+        if (0 == (error_message_count % 100))
+        {
+            printf("handle: %u\n", handle);
+            printf("temp_status : %u\n", message->lidar_error_code.temp_status);
+            printf("volt_status : %u\n", message->lidar_error_code.volt_status);
+            printf("motor_status : %u\n", message->lidar_error_code.motor_status);
+            printf("dirty_warn : %u\n", message->lidar_error_code.dirty_warn);
+            printf("firmware_err : %u\n", message->lidar_error_code.firmware_err);
+            printf("pps_status : %u\n", message->lidar_error_code.device_status);
+            printf("fan_status : %u\n", message->lidar_error_code.fan_status);
+            printf("self_heating : %u\n", message->lidar_error_code.self_heating);
+            printf("ptp_status : %u\n", message->lidar_error_code.ptp_status);
+            printf("time_sync_status : %u\n", message->lidar_error_code.time_sync_status);
+            printf("system_status : %u\n", message->lidar_error_code.system_status);
+        }
+    }
+}
+
+void LdsLidar::ControlFanCb(livox_status status, uint8_t handle, \
+                           uint8_t response, void *client_data)
+{
+
+}
+
+void LdsLidar::SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
+                                         uint8_t response, void *client_data)
+{
+    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+
+    if (handle >= kMaxLidarCount)
+    {
+        return;
+    }
+    LidarDevice *p_lidar = &(lidars_[handle]);
+
+    if (status == kStatusSuccess)
+    {
+        p_lidar->config.set_bits &= ~((uint32_t) (kConfigReturnMode));
+        printf("Set return mode success!\n");
+
+        if (!p_lidar->config.set_bits)
+        {
+            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
+            p_lidar->connect_state = kConnectStateSampling;
+        }
+    }
+    else
+    {
+        LidarSetPointCloudReturnMode(handle, (PointCloudReturnMode) (p_lidar->config.return_mode), \
+                                 LdsLidar::SetPointCloudReturnModeCb, lds_lidar);
+        printf("Set return mode fail, try again!\n");
+    }
+}
+
+void LdsLidar::SetCoordinateCb(livox_status status, uint8_t handle, \
+                               uint8_t response, void *client_data)
+{
+    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+
+    if (handle >= kMaxLidarCount)
+    {
+        return;
+    }
+    LidarDevice *p_lidar = &(lidars_[handle]);
+
+    if (status == kStatusSuccess)
+    {
+        p_lidar->config.set_bits &= ~((uint32_t) (kConfigCoordinate));
+        printf("Set coordinate success!\n");
+
+        if (!p_lidar->config.set_bits)
+        {
+            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
+            p_lidar->connect_state = kConnectStateSampling;
+        }
+    }
+    else
+    {
+        if (p_lidar->config.coordinate != 0)
+        {
+            SetSphericalCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
+        }
+        else
+        {
+            SetCartesianCoordinate(handle, LdsLidar::SetCoordinateCb, lds_lidar);
+        }
+
+        printf("Set coordinate fail, try again!\n");
+    }
+}
+
+void LdsLidar::SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
+                                         uint8_t response, void *client_data)
+{
+    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+
+    if (handle >= kMaxLidarCount)
+    {
+        return;
+    }
+    LidarDevice *p_lidar = &(lidars_[handle]);
+
+    if (status == kStatusSuccess)
+    {
+        p_lidar->config.set_bits &= ~((uint32_t) (kConfigImuRate));
+        printf("Set imu rate success!\n");
+
+        if (!p_lidar->config.set_bits)
+        {
+            LidarStartSampling(handle, LdsLidar::StartSampleCb, lds_lidar);
+            p_lidar->connect_state = kConnectStateSampling;
+        }
+
+    }
+    else
+    {
+        LidarSetImuPushFrequency(handle, (ImuFreq) (p_lidar->config.imu_rate), \
+                             LdsLidar::SetImuRatePushFrequencyCb, lds_lidar);
+        printf("Set imu rate fail, try again!\n");
+    }
+}
+
+/** Callback function of starting sampling. */
+void LdsLidar::StartSampleCb(livox_status status, uint8_t handle, \
+                             uint8_t response, void *client_data)
+{
+    LdsLidar *lds_lidar = static_cast<LdsLidar *>(client_data);
+
+    if (handle >= kMaxLidarCount)
+    {
+        return;
+    }
+
+    LidarDevice *p_lidar = &(lidars_[handle]);
+    if (status == kStatusSuccess)
+    {
+        if (response != 0)
+        {
+            p_lidar->connect_state = kConnectStateOn;
+            printf("Lidar start sample fail : state[%d] handle[%d] res[%d]\n", \
+             status, handle, response);
+        }
+        else
+        {
+            printf("Lidar start sample success\n");
+        }
+    }
+    else if (status == kStatusTimeout)
+    {
+        p_lidar->connect_state = kConnectStateOn;
+        printf("Lidar start sample timeout : state[%d] handle[%d] res[%d]\n", \
+           status, handle, response);
+    }
+}
+
+/** Callback function of stopping sampling. */
+void LdsLidar::StopSampleCb(livox_status status, uint8_t handle, \
+                            uint8_t response, void *client_data)
+{
+}
+
+/** Add broadcast code to whitelist */
+int LdsLidar::AddBroadcastCodeToWhitelist(const char *bd_code)
+{
+    if (!bd_code || (strlen(bd_code) > kBroadcastCodeSize) || \
+      (whitelist_count_ >= kMaxLidarCount))
+    {
+        return -1;
+    }
+
+    if (LdsLidar::FindInWhitelist(bd_code))
+    {
+        printf("%s is alrealy exist!\n", bd_code);
+        return -1;
+    }
+
+    strcpy(broadcast_code_whitelist_[whitelist_count_], bd_code);
+    ++whitelist_count_;
+
+    return 0;
+}
+
+void LdsLidar::AddLocalBroadcastCode(void)
+{
+    for (size_t i = 0; i < sizeof(local_broadcast_code_list) / sizeof(intptr_t); ++i)
+    {
+        std::string invalid_bd = "000000000";
+        printf("Local broadcast code : %s\n", local_broadcast_code_list[i]);
+        if ((kBroadcastCodeSize == strlen(local_broadcast_code_list[i]) + 1) && \
+        (nullptr == strstr(local_broadcast_code_list[i], invalid_bd.c_str())))
+        {
+            LdsLidar::AddBroadcastCodeToWhitelist(local_broadcast_code_list[i]);
+        }
+        else
+        {
+            printf("Invalid local broadcast code : %s\n", local_broadcast_code_list[i]);
+        }
+    }
+}
+
+bool LdsLidar::FindInWhitelist(const char *bd_code)
+{
+    if (!bd_code)
+    {
+        return false;
+    }
+
+    for (uint32_t i = 0; i < whitelist_count_; i++)
+    {
+        if (strncmp(bd_code, broadcast_code_whitelist_[i], kBroadcastCodeSize) == 0)
+        {
+            return true;
+        }
+    }
+
+    return false;
+}
+
+}
+

laser/lds_lidar.h

@@ -0,0 +1,168 @@
+//
+// The MIT License (MIT)
+//
+// Copyright (c) 2019 Livox. All rights reserved.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+//
+
+/** Livox LiDAR data source, data from dependent lidar */
+
+#ifndef LDS_LIDAR_H_
+#define LDS_LIDAR_H_
+
+#include <stdio.h>
+#include <string.h>
+#include <memory>
+
+#include "livox_def.h"
+#include "livox_sdk.h"
+#include <chrono>
+#include <thread_safe_map.h>
+namespace livox
+{
+typedef enum
+{
+    kConnectStateOff = 0,
+    kConnectStateOn = 1,
+    kConnectStateConfig = 2,
+    kConnectStateSampling = 3,
+} LidarConnectState;
+
+typedef enum
+{
+    kConfigFan = 1,
+    kConfigReturnMode = 2,
+    kConfigCoordinate = 4,
+    kConfigImuRate = 8
+} LidarConfigCodeBit;
+
+typedef enum
+{
+    kCoordinateCartesian = 0,
+    kCoordinateSpherical
+} CoordinateType;
+
+typedef struct
+{
+    bool enable_fan;
+    uint32_t return_mode;
+    uint32_t coordinate;
+    uint32_t imu_rate;
+    volatile uint32_t set_bits;
+    volatile uint32_t get_bits;
+} UserConfig;
+
+typedef struct
+{
+    uint8_t handle;
+    LidarConnectState connect_state;
+    DeviceInfo info;
+    UserConfig config;
+} LidarDevice;
+
+
+
+
+/**
+ * LiDAR data source, data from dependent lidar.
+ */
+class LdsLidar
+{
+
+ public:
+    LdsLidar();
+
+    LdsLidar(const LdsLidar &) = delete;
+
+    virtual ~LdsLidar();
+    static int InitLdsLidar();
+
+    int AddBroadcastCode(std::string sn);
+
+    int DeInitLdsLidar(void);
+
+ protected:
+
+ private:
+    virtual void LidarDataCallback(uint8_t handle, LivoxEthPacket *data,uint32_t data_num);
+
+    LdsLidar &operator=(const LdsLidar &) = delete;
+
+    static void GetLidarDataCb(uint8_t handle, LivoxEthPacket *data, \
+                             uint32_t data_num, void *client_data);
+
+    static void OnDeviceBroadcast(const BroadcastDeviceInfo *info);
+
+    static void OnDeviceChange(const DeviceInfo *info, DeviceEvent type);
+
+    static void StartSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
+
+    static void StopSampleCb(livox_status status, uint8_t handle, uint8_t response, void *clent_data);
+
+    static void DeviceInformationCb(livox_status status, uint8_t handle, \
+                                  DeviceInformationResponse *ack, void *clent_data);
+
+    static void LidarErrorStatusCb(livox_status status, uint8_t handle, ErrorMessage *message);
+
+    static void ControlFanCb(livox_status status, uint8_t handle, \
+                           uint8_t response, void *clent_data);
+
+    static void SetPointCloudReturnModeCb(livox_status status, uint8_t handle, \
+                                        uint8_t response, void *clent_data);
+
+    static void SetCoordinateCb(livox_status status, uint8_t handle, \
+                              uint8_t response, void *clent_data);
+
+    static void SetImuRatePushFrequencyCb(livox_status status, uint8_t handle, \
+                                        uint8_t response, void *clent_data);
+
+    int AddBroadcastCodeToWhitelist(const char *broadcast_code);
+
+    void AddLocalBroadcastCode(void);
+
+    bool FindInWhitelist(const char *broadcast_code);
+
+    void EnableAutoConnectMode(void)
+    {
+        auto_connect_mode_ = true;
+    }
+
+    void DisableAutoConnectMode(void)
+    {
+        auto_connect_mode_ = false;
+    }
+
+    bool IsAutoConnectMode(void)
+    {
+        return auto_connect_mode_;
+    }
+
+    bool auto_connect_mode_;
+    uint32_t whitelist_count_;
+    char broadcast_code_whitelist_[kMaxLidarCount][kBroadcastCodeSize];
+
+    uint32_t lidar_count_;
+
+
+    uint32_t data_recveive_count_[kMaxLidarCount];
+};
+}
+
+#endif

main.cpp

@@ -4,14 +4,14 @@
 #include <fcntl.h>
 #include <iostream>
 #include "plc/plc_communicator.h"
-#include "laser/Laser.h"
+//#include "laser/Laser.h"
 #include "plc/plc_communicator.h"
 #include "tool/pathcreator.h"
 #include <glog/logging.h>
 #include "verifier.h"
 #include "proto_tool.h"
 #include <livox_sdk.h>
-
+#include "LivoxMid70.h"
 using google::protobuf::io::FileInputStream;
 using google::protobuf::io::FileOutputStream;
 using google::protobuf::io::ZeroCopyInputStream;
@@ -79,9 +79,18 @@ void init_glog()
 std::mutex   mut;
 pcl::visualization::PCLVisualizer viewer("Cloud");
 
+livox::LivoxMid70 read_lidar;
+livox::LivoxMid70 read_lidar1;
+
 int Working()
 {
-    Error_manager code;
+
+    while(1)
+    {
+        break;
+    }
+
+    /*Error_manager code;
 
     Eigen::Vector4f plane,plane1;
     plane[0]=0;
@@ -230,6 +239,8 @@ int Working()
 
 
         //处理点云--------------------------------------------------------------------------
+
+
         int color[3]={0,255,0};
         if(shutter.verify(scan_cloud.makeShared())!=SUCCESS)
         {
@@ -247,6 +258,8 @@ int Working()
         usleep(10*1000);
     }
 
+*/
+
 
 }
 
@@ -256,14 +269,68 @@ int main(int argc,char* argv[])
     init_glog();
     viewer.addCoordinateSystem(2.0,0,0,0,"car_center");
 
-    usleep(500*1000);
+
+    int ret = livox::LdsLidar::InitLdsLidar();
+
+    const float DEGREE=M_PI/180.0;
+    float transform1[]={-2.31,161.58,-64,2.512,0,0};
+    float transform2[]={176.45,28.05,71.85,0,0,0};
+
+
+    std::string sn1="3GGDJ8S00100921";
+    std::string sn2="3GGDJ8X00100541";
+    Eigen::Vector3f rpy1;
+    rpy1<<-2.31*DEGREE,161.58*DEGREE,-64*DEGREE;
+    Eigen::Vector3f rpy2;
+    rpy2<<176.45*DEGREE,28.05*DEGREE,71.85*DEGREE;
+
+    Eigen::Vector3f t1;
+    t1<<2.512,0,0;
+    Eigen::Vector3f t2;
+    t2<<0,0,0;
+
+
+    read_lidar.SetTransformParam(rpy1,t1);
+    read_lidar1.SetTransformParam(rpy2,t2);
+
+    if(read_lidar.AddBroadcastCode(sn1)!=0)
+        printf("%s  connect failed\n",sn1.c_str());
+    if(read_lidar1.AddBroadcastCode(sn2)!=0)
+    {
+        printf("%s  connect failed\n",sn2.c_str());
+
+    }
+    read_lidar.SetFPS(2.0);
+    read_lidar1.SetFPS(2.0);
+    usleep(3000*1000);
+
+
+
+
+    //lidar1.AddSNCode(sn1);
+    //lidar2.AddSNCode(sn2);
+
     std::thread* thread=new std::thread(Working);
 
     while(1)
     {
-        mut.lock();
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1=read_lidar.GetCloud();
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2=read_lidar1.GetCloud();
+        viewer.removeAllPointClouds();
+        int color[3]={0,255,0};
+        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud1, color[0],color[1],color[2]); // green
+        viewer.addPointCloud(cloud1, single_color, "cloud1");
+        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud1");
+
+
+        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud2, 255,0,0); // green
+        viewer.addPointCloud(cloud2, single_color2, "cloud2");
+        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud2");
+
+        printf("  lidar 1 count %d     lidar2 count %d  \n",cloud1->size(),cloud2->size());
+
         viewer.spinOnce();
-        mut.unlock();
         usleep(33*1000);
     }
 

sample_center_calib.cpp

@@ -0,0 +1,151 @@
+//
+// Created by zx on 2021/8/27.
+//
+#include "rotate_center_caliber.h"
+#include "point_tool.h"
+#include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/common/transforms.h>
+#include "tool/pathcreator.h"
+#include "proto_tool.h"
+#include <iostream>
+#include <glog/logging.h>
+
+//using google::protobuf::io::FileInputStream;
+//using google::protobuf::io::FileOutputStream;
+using google::protobuf::io::ZeroCopyInputStream;
+using google::protobuf::io::CodedInputStream;
+using google::protobuf::io::ZeroCopyOutputStream;
+using google::protobuf::io::CodedOutputStream;
+using google::protobuf::Message;
+
+GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
+{
+    time_t tt;
+    time( &tt );
+    tt = tt + 8*3600;  // transform the time zone
+    tm* t= gmtime( &tt );
+    char buf[255]={0};
+    sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
+            t->tm_year + 1900,
+            t->tm_mon + 1,
+            t->tm_mday,
+            t->tm_hour,
+            t->tm_min,
+            t->tm_sec);
+
+    FILE* tp_file=fopen(buf,"w");
+    fprintf(tp_file,data,strlen(data));
+    fclose(tp_file);
+
+}
+
+void init_glog()
+{
+    time_t tt = time(0);//时间cuo
+    struct tm* t = localtime(&tt);
+
+    char strYear[255]={0};
+    char strMonth[255]={0};
+    char strDay[255]={0};
+
+    sprintf(strYear,"%04d", t->tm_year+1900);
+    sprintf(strMonth,"%02d", t->tm_mon+1);
+    sprintf(strDay,"%02d", t->tm_mday);
+
+    char buf[255]={0};
+    getcwd(buf,255);
+    char strdir[255]={0};
+    sprintf(strdir,"%s/caliber_log/%s/%s/%s", buf,strYear,strMonth,strDay);
+    PathCreator creator;
+    creator.Mkdir(strdir);
+
+    char logPath[255] = { 0 };
+    sprintf(logPath, "%s/", strdir);
+    FLAGS_max_log_size = 100;
+    FLAGS_logbufsecs = 0;
+    google::InitGoogleLogging("shutter_verify");
+    google::SetStderrLogging(google::INFO);
+    google::SetLogDestination(0, logPath);
+    google::SetLogFilenameExtension("zxlog");
+    google::InstallFailureSignalHandler();
+    google::InstallFailureWriter(&shut_down_logging);
+    FLAGS_colorlogtostderr = true;        // Set log color
+    FLAGS_logbufsecs = 0;                // Set log output speed(s)
+    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
+    FLAGS_stop_logging_if_full_disk = true;
+}
+
+
+pcl::visualization::PCLVisualizer viewer;
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr rotation_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+        float theta,float ox,float oy)
+{
+
+    //先平移点云，后旋转,再平移
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_2.translation() << -ox, -oy, 0;
+    pcl::transformPointCloud(*cloud, *transformed_cloud, transform_2);
+
+    Eigen::Affine3f transform_r = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud1(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_r.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
+    pcl::transformPointCloud(*transformed_cloud, *transformed_cloud1, transform_r);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud2(new pcl::PointCloud<pcl::PointXYZ>());
+    Eigen::Affine3f transform_t = Eigen::Affine3f::Identity();
+    transform_t.translation() << ox, oy, 0;
+    pcl::transformPointCloud(*transformed_cloud1, *transformed_cloud2, transform_t);
+    return transformed_cloud2;
+}
+
+#include "tick.hpp"
+#include <thread>
+#include <mutex>
+std::mutex mut;
+
+void run()
+{
+    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud=ReadTxtCloud("./center_caliber_cloud_sample3.txt");
+    LOG(INFO)<<"  cloud size:"<<cloud->size();
+
+    mut.lock();
+    viewer.addPointCloud(cloud,"cloud_src");
+    mut.unlock();
+
+    rotate_center_caliber caliber;
+    caliber.set_first_frame(cloud);
+
+    float rotation_x=6.8;
+    float rotation_y=-3.5;
+
+    float theta=0;
+    for(theta=-7./180.*M_PI;theta<8./180.*M_PI;theta+=1./180.*M_PI)
+    {
+
+        pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud=rotation_cloud(cloud,theta,rotation_x,rotation_y);
+        Tick tick;
+        caliber.push_cloud(transform_cloud);
+        std::cout<<"time : "<<tick.tic_relatively()<<std::endl;
+        usleep(1000*500);
+    }
+}
+
+int main()
+{
+    init_glog();
+    viewer.addCoordinateSystem(1.0,0,0,0,"car_center");
+
+
+    std::thread* thread=new std::thread(run);
+
+    while(1)
+    {
+        mut.lock();
+        viewer.spinOnce();
+        mut.unlock();
+        usleep(50*1000);
+    }
+    return 0;
+}

tool/point_tool.cpp

@@ -81,9 +81,9 @@ bool string2point(std::string str,pcl::PointXYZ& point)
             return false;
         }
     }
-    point.x=data[0];
-    point.y=data[1];
-    point.z=data[2];
+    point.x=data[0]/1000.;
+    point.y=data[1]/1000.;
+    point.z=data[2]/1000.;
     return true;
 }
 
@@ -297,3 +297,10 @@ bool isRect(std::vector<cv::Point2f>& points)
 
 }
 
+
+Eigen::Matrix4f ndt_match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2)
+{
+
+}
+
+

tool/point_tool.h

@@ -28,4 +28,8 @@ std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segmentation(pcl::PointCloud<pc
 void rpy_to_rotation_matrix();
 void rotation_matrix_to_rpy();
 void reverse_test();
+
+
+//计算cloud2到cloud1的变换
+Eigen::Matrix4f ndt_match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2);
 #endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_POINT_TOOL_HPP_

tool/thread_safe_map.cpp

@@ -0,0 +1,5 @@
+//
+// Created by zx on 2020/7/3.
+//
+
+#include "thread_safe_map.h"

tool/thread_safe_map.h

@@ -0,0 +1,96 @@
+//
+// Created by zx on 2020/7/3.
+//
+
+#ifndef NNXX_TESTS_THREAD_SAFE_MAP_H
+#define NNXX_TESTS_THREAD_SAFE_MAP_H
+
+
+#include <map>
+#include <mutex>
+template<typename Key, typename Val>
+class thread_safe_map
+{
+public:
+    typedef typename std::map<Key, Val>::iterator this_iterator;
+    typedef typename std::map<Key, Val>::const_iterator this_const_iterator;
+    thread_safe_map()
+    {}
+
+    Val& operator [](const Key& key)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_[key];
+    }
+    int erase(const Key& key )
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.erase(key);
+    }
+
+    bool find_update(const Key& key,const Val& val)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        if(dataMap_.find(key)!=dataMap_.end()) {
+            dataMap_[key]=val;
+            return true;
+        }
+        return false;
+    }
+
+    bool find(const Key& key)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        if(dataMap_.find(key)!=dataMap_.end()) {
+            return true;
+        }
+        return false;
+    }
+
+    bool find(const Key& key,Val& val)
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        if(dataMap_.find(key)!=dataMap_.end()) {
+            val=dataMap_[key];
+            return true;
+        }
+        return false;
+    }
+
+    /*this_iterator find( const Key& key )
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.find(key);
+    }
+    this_const_iterator find( const Key& key ) const
+    {
+        std::lock_guard<std::mutex> lk(mtx_);
+        return dataMap_.find(key);
+    }*/
+
+    this_iterator begin()
+    {
+        return dataMap_.begin();
+    }
+
+    this_iterator end()
+    {
+        return dataMap_.end();
+    }
+
+    this_const_iterator end() const
+    {
+        return dataMap_.end();
+    }
+
+    unsigned int size()
+    {
+        return dataMap_.size();
+    }
+private:
+    std::map<Key, Val> dataMap_;
+    std::mutex mtx_;
+};
+
+
+#endif //NNXX_TESTS_THREAD_SAFE_MAP_H

tool/tick.hpp

@@ -0,0 +1,46 @@
+//
+// Created by zx on 2021/5/19.
+//
+
+#ifndef ROBOT_CONTROL_FEATURE_EXTRA_SRC_TICK_HPP_
+#define ROBOT_CONTROL_FEATURE_EXTRA_SRC_TICK_HPP_
+
+#include <chrono>
+
+class Tick
+{
+ public:
+    Tick()
+    {
+        m_start=std::chrono::steady_clock::now();
+        m_last_tick=m_start;
+    }
+    Tick(const Tick& tick)= default;
+    Tick& operator=(const Tick& tick)= default;
+    double tic()
+    {
+        auto tic=std::chrono::steady_clock::now();
+        m_last_tick=tic;
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(tic - m_start);
+        return double(duration.count()) * std::chrono::microseconds::period::num /
+                std::chrono::microseconds::period::den;
+    }
+    double tic_relatively()
+    {
+        auto tic=std::chrono::steady_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(tic - m_last_tick);
+        m_last_tick=tic;
+        return double(duration.count()) * std::chrono::microseconds::period::num /
+                std::chrono::microseconds::period::den;
+    }
+
+ private:
+    std::chrono::steady_clock::time_point     m_start;
+    std::chrono::steady_clock::time_point     m_last_tick;
+};
+
+
+
+
+
+#endif //ROBOT_CONTROL_FEATURE_EXTRA_SRC_TICK_HPP_

verify/lidar_caliber.cpp

@@ -0,0 +1,58 @@
+//
+// Created by zx on 2021/8/17.
+//
+
+#include "lidar_caliber.h"
+#include <pcl/registration/ndt.h>
+lidar_caliber::lidar_caliber()
+{
+
+}
+bool lidar_caliber::caliber(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1
+        ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,Eigen::Matrix<float,6,1>& transform1
+        ,Eigen::Matrix<float,6,1>& transfoem2)
+{
+    //transform1=match();
+
+}
+
+Eigen::Matrix4f lidar_caliber::match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center
+        ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Eigen::Matrix<float, 6,1> init_transform)
+{
+    //初始化正态分布变换（NDT）
+    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+    //设置依赖尺度NDT参数
+    //为终止条件设置最小转换差异
+    ndt.setTransformationEpsilon(0.01);
+    //为More-Thuente线搜索设置最大步长
+    ndt.setStepSize(0.1);
+    //设置NDT网格结构的分辨率（VoxelGridCovariance）
+    ndt.setResolution(1.0);
+    //设置匹配迭代的最大次数
+    ndt.setMaximumIterations(35);
+    // 设置要配准的点云
+    ndt.setInputCloud(cloud);
+    //设置点云配准目标
+    ndt.setInputTarget(cloud_center);
+    //设置使用机器人测距法得到的初始对准估计结果
+
+    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(init_transform(0), Eigen::Vector3f::UnitX()));
+    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(init_transform(1), Eigen::Vector3f::UnitY()));
+    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(init_transform(2), Eigen::Vector3f::UnitZ()));
+    Eigen::AngleAxisf rotation ;
+    rotation = yawAngle*pitchAngle*rollAngle;
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+
+    transform_2.translation() << init_transform(3),init_transform(4),init_transform(5);	// 三个数分别对应X轴、Y轴、Z轴方向上的平移
+    transform_2.rotate(rotation);	//同理，UnitX(),绕X轴；UnitY(
+
+    Eigen::Matrix4f init_guess = transform_2.matrix();
+    //计算需要的刚体变换以便将输入的点云匹配到目标点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    ndt.align(*output_cloud, init_guess);
+
+    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
+              << " score: " << ndt.getFitnessScore() << std::endl;
+    //使用创建的变换对未过滤的输入点云进行变换
+    return ndt.getFinalTransformation();
+}

verify/lidar_caliber.h

@@ -0,0 +1,24 @@
+//
+// Created by zx on 2021/8/17.
+//
+
+#ifndef SHUTTER_VERIFY_VERIFY_LIDAR_CALIBER_H_
+#define SHUTTER_VERIFY_VERIFY_LIDAR_CALIBER_H_
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+class lidar_caliber
+{
+ public:
+    lidar_caliber();
+    static bool caliber(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1
+                ,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2,Eigen::Matrix<float,6,1>& transform1
+                ,Eigen::Matrix<float,6,1>& transfoem2);
+
+    static Eigen::Matrix4f match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
+                ,Eigen::Matrix<float, 6,1> init_transform);
+};
+
+
+#endif //SHUTTER_VERIFY_VERIFY_LIDAR_CALIBER_H_

verify/match3d/common/math.h

@@ -0,0 +1,84 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_COMMON_MATH_H_
+#define CARTOGRAPHER_COMMON_MATH_H_
+
+#include <cmath>
+#include <vector>
+
+#include "Eigen/Core"
+#include "port.h"
+#include "ceres/ceres.h"
+
+namespace common {
+
+// Clamps 'value' to be in the range ['min', 'max'].
+template <typename T>
+T Clamp(const T value, const T min, const T max) {
+  if (value > max) {
+    return max;
+  }
+  if (value < min) {
+    return min;
+  }
+  return value;
+}
+
+// Calculates 'base'^'exponent'.
+template <typename T>
+constexpr T Power(T base, int exponent) {
+  return (exponent != 0) ? base * Power(base, exponent - 1) : T(1);
+}
+
+// Calculates a^2.
+template <typename T>
+constexpr T Pow2(T a) {
+  return Power(a, 2);
+}
+
+// Converts from degrees to radians.
+constexpr double DegToRad(double deg) { return M_PI * deg / 180.; }
+
+// Converts form radians to degrees.
+constexpr double RadToDeg(double rad) { return 180. * rad / M_PI; }
+
+// Bring the 'difference' between two angles into [-pi; pi].
+template <typename T>
+T NormalizeAngleDifference(T difference) {
+  const T kPi = T(M_PI);
+  while (difference > kPi) difference -= 2. * kPi;
+  while (difference < -kPi) difference += 2. * kPi;
+  return difference;
+}
+
+template <typename T>
+T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
+  return ceres::atan2(vector.y(), vector.x());
+}
+
+template <typename T>
+inline void QuaternionProduct(const double* const z, const T* const w,
+                              T* const zw) {
+  zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
+  zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
+  zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
+  zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
+}
+
+}  // namespace common
+
+#endif  // CARTOGRAPHER_COMMON_MATH_H_

verify/match3d/common/port.h

@@ -0,0 +1,68 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_COMMON_PORT_H_
+#define CARTOGRAPHER_COMMON_PORT_H_
+
+#include <boost/iostreams/device/back_inserter.hpp>
+#include <boost/iostreams/filter/gzip.hpp>
+#include <boost/iostreams/filtering_stream.hpp>
+#include <cinttypes>
+#include <cmath>
+#include <string>
+
+using int8 = int8_t;
+using int16 = int16_t;
+using int32 = int32_t;
+using int64 = int64_t;
+using uint8 = uint8_t;
+using uint16 = uint16_t;
+using uint32 = uint32_t;
+using uint64 = uint64_t;
+
+namespace common {
+
+inline int RoundToInt(const float x) { return std::lround(x); }
+
+inline int RoundToInt(const double x) { return std::lround(x); }
+
+inline int64 RoundToInt64(const float x) { return std::lround(x); }
+
+inline int64 RoundToInt64(const double x) { return std::lround(x); }
+
+inline void FastGzipString(const std::string& uncompressed,
+                           std::string* compressed) {
+  boost::iostreams::filtering_ostream out;
+  out.push(
+      boost::iostreams::gzip_compressor(boost::iostreams::zlib::best_speed));
+  out.push(boost::iostreams::back_inserter(*compressed));
+  boost::iostreams::write(out,
+                          reinterpret_cast<const char*>(uncompressed.data()),
+                          uncompressed.size());
+}
+
+inline void FastGunzipString(const std::string& compressed,
+                             std::string* decompressed) {
+  boost::iostreams::filtering_ostream out;
+  out.push(boost::iostreams::gzip_decompressor());
+  out.push(boost::iostreams::back_inserter(*decompressed));
+  boost::iostreams::write(out, reinterpret_cast<const char*>(compressed.data()),
+                          compressed.size());
+}
+
+}  // namespace common
+
+#endif  // CARTOGRAPHER_COMMON_PORT_H_

verify/match3d/detect_ceres3d.cpp

@@ -0,0 +1,236 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#include "detect_ceres3d.h"
+#include "interpolated_grid.hpp"
+#include <pcl/common/transforms.h>
+
+class CostFunctor3d
+{
+ private:
+    const InterpolatedProbabilityGrid m_interpolated_grid;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;     //左前点云
+
+ public:
+    CostFunctor3d(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                  const HybridGrid& interpolated_grid)
+                  :m_interpolated_grid(interpolated_grid),m_cloud(cloud)
+    {
+    }
+
+    template<typename T>
+    bool operator()(const T *const variable, T *residual) const
+    {
+        T cx = variable[0];
+        T cy = variable[1];
+        T theta = variable[2];
+
+        //整车旋转
+        Eigen::Rotation2D<T> rotation(theta);
+        Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+
+        //
+        int right_rear_num = m_cloud->size();
+        for (int i = 0; i < m_cloud->size(); ++i)
+        {
+            //先平移后旋转
+            const Eigen::Matrix<T, 2, 1> point((T(m_cloud->points[i].x)+cx),
+                                               (T(m_cloud->points[i].y)+cy));
+            //绕原点旋转
+            const Eigen::Matrix<T, 2, 1> rotate_point = rotation_matrix * point;
+
+            residual[i] = T(1.0) -
+                    m_interpolated_grid.GetInterpolatedValue(rotate_point[0],rotate_point[1],
+                                                                  T(m_cloud->points[i].z));
+        }
+        return true;
+    }
+
+};
+
+
+detect_ceres3d::detect_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr model_grid_cloud)
+{
+
+
+
+    float del_x=0.05;
+    float del_y=0.03;
+    float del_z=0.05;
+    Eigen::Matrix3f delta=Eigen::MatrixXf::Identity(3,3);
+    delta(0,0)=del_x*del_x;
+    delta(1,1)=del_y*del_y;
+    delta(2,2)=del_z*del_z;
+    Eigen::Matrix3f delta_inv=delta.inverse();
+
+
+    Eigen::Vector3f d(0,0,0.02);
+    Eigen::MatrixXf ce=-0.5*((d).transpose()*delta_inv*(d));
+    float cut_p=exp(ce(0,0));
+
+    float resolution=0.02;
+    mp_model_grid=new HybridGrid(resolution);
+
+
+    for(int i=0;i<model_grid_cloud->size();++i)
+    {
+        pcl::PointXYZ pt=model_grid_cloud->points[i];
+        Eigen::Vector3f u(pt.x,pt.y,pt.z);
+
+        for(float x=pt.x-0.1;x<pt.x+0.1;x+=mp_model_grid->resolution())
+        {
+            for(float y=pt.y-0.1;y<pt.y+0.1;y+=mp_model_grid->resolution())
+            {
+                for(float z=pt.z-0.1;z<pt.z+0.1;z+=mp_model_grid->resolution())
+                {
+                    Eigen::Vector3f p(x,y,z);
+                    Eigen::MatrixXf B=-0.5*((p-u).transpose()*delta_inv*(p-u));
+                    float prob=exp(B(0,0));
+                    if(prob>cut_p)
+                        prob=cut_p;
+                    Eigen::Array3i index=mp_model_grid->GetCellIndex(p);
+                    const Eigen::Array3i shifted_index = index+ (mp_model_grid->grid_size() >> 1);
+                    if ((shifted_index.cast<unsigned int>() >= mp_model_grid->grid_size()).any())
+                    {
+                    	std::cout<<index.transpose()<<","<<u.transpose()<<std::endl;
+                    }
+                    if(mp_model_grid->GetProbability(index)<prob)
+                        mp_model_grid->SetProbability(index,prob);
+
+                }
+            }
+        }
+
+    }
+
+    save_model("./");
+
+}
+
+#include "point_tool.h"
+void detect_ceres3d::save_model(std::string dir)
+{
+
+    InterpolatedProbabilityGrid inter_grid(*mp_model_grid);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr  cloud_grid(new pcl::PointCloud<pcl::PointXYZRGB>);
+    for(int x=0;x<mp_model_grid->grid_size();++x)
+    {
+        for(int y=0;y<mp_model_grid->grid_size();++y)
+        {
+            for(int z=0;z<mp_model_grid->grid_size();++z)
+            {
+                Eigen::Array3i index(x-mp_model_grid->grid_size()/2,
+                        y-mp_model_grid->grid_size()/2,z-mp_model_grid->grid_size()/2);
+                Eigen::Vector3f pt=mp_model_grid->GetCenterOfCell(index);
+
+                float prob=inter_grid.GetInterpolatedValue(double(pt[0]),double(pt[1]),double(pt[2]));
+                if(prob>0.01)
+                {
+                    Eigen::Vector3f pt=mp_model_grid->GetCenterOfCell(index);
+                    int rgb=int((prob-0.01)*255);
+                    pcl::PointXYZRGB point;
+                    point.x=pt[0];
+                    point.y=pt[1];
+                    point.z=pt[2];
+                    point.r=rgb;
+                    point.g=0;
+                    point.b=255-rgb;
+                    cloud_grid->push_back(point);
+                }
+            }
+        }
+    }
+
+
+    std::cout<<"  save model :"<<dir<<std::endl;
+    save_cloud_txt(cloud_grid,dir+"/model.txt");
+
+}
+
+detect_ceres3d::~detect_ceres3d()
+{
+    delete mp_model_grid;
+    mp_model_grid= nullptr;
+}
+
+
+
+bool detect_ceres3d::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+        Detect_result &detect_result, std::string &error_info)
+{
+
+
+    error_info = "";
+
+    double cx=detect_result.cx;
+    double cy=detect_result.cy;
+    double init_theta=detect_result.theta;
+
+
+    double variable[] = {cx, cy, init_theta};
+    /*printf("init solve x:%.3f  y: %.3f  wheel:%.3f  width:%.3f    theta : %.3f   front theta: %.3f\n",
+            variable[0], variable[1], variable[3], variable[4], variable[2], variable[5]);*/
+
+    // 第二部分：构建寻优问题
+    ceres::Problem problem;
+    CostFunctor3d *cost_func = new CostFunctor3d(cloud,*mp_model_grid);
+    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
+    ceres::CostFunction* cost_function =new
+            ceres::AutoDiffCostFunction<CostFunctor3d, ceres::DYNAMIC, 3>(
+            cost_func,cloud->size());
+    problem.AddResidualBlock(cost_function, NULL, variable); //向问题中添加误差项，本问题比较简单，添加一个就行。
+
+
+
+    //第三部分： 配置并运行求解器
+    ceres::Solver::Options options;
+    options.use_nonmonotonic_steps=false;
+    options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
+    //options.logging_type = ceres::LoggingType::SILENT;
+    options.max_num_iterations=500;
+    options.num_threads=2;
+    options.minimizer_progress_to_stdout = false;//输出到cout
+    ceres::Solver::Summary summary;//优化信息
+    ceres::Solve(options, &problem, &summary);//求解!!!
+
+    double loss=summary.final_cost/(cloud->size());
+
+    if(loss>0.05)
+    {
+        char buf[64]={0};
+        sprintf(buf," loss : %.5f  > 0.05",loss);
+        error_info=buf;
+        return false;
+    }
+
+    detect_result.cx=variable[0];
+    detect_result.cy=variable[1];
+    detect_result.theta=variable[2];
+
+    return true;
+
+}
+
+bool detect_ceres3d::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Detect_result &detect_result,
+                            pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud,std::string &error_info)
+{
+
+    if(detect(cloud,detect_result,error_info))
+    {
+
+        Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+        pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud0(new pcl::PointCloud<pcl::PointXYZ>());
+        transform_2.translation() << detect_result.cx, detect_result.cy, 0;
+        pcl::transformPointCloud(*cloud, *transform_cloud0, transform_2);
+
+        transform_2=Eigen::Affine3f::Identity();
+        transform_2.rotate(Eigen::AngleAxisf(detect_result.theta, Eigen::Vector3f::UnitZ()));
+        pcl::transformPointCloud(*transform_cloud0, *transformed_cloud, transform_2);
+
+
+        return true;
+    }
+    return false;
+
+}

verify/match3d/detect_ceres3d.h

@@ -0,0 +1,75 @@
+//
+// Created by zx on 2020/7/1.
+//
+
+#ifndef CERES_TEST_DETECT_WHEEL_CERES_H
+#define CERES_TEST_DETECT_WHEEL_CERES_H
+
+#include <ceres/ceres.h>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/centroid.h>
+#include <opencv2/opencv.hpp>
+#include <iostream>
+#include <string>
+#include <chrono>
+#include "boost/format.hpp"
+#include "hybrid_grid.hpp"
+
+class detect_ceres3d {
+public:
+    struct Loss_result
+    {
+        public: 
+        double lf_loss;
+        double rf_loss;
+        double lb_loss;
+        double rb_loss;
+        double total_avg_loss;
+        Loss_result& operator=(const Loss_result& loss_result)
+        {
+            this->lf_loss = loss_result.lf_loss;
+            this->rf_loss = loss_result.rf_loss;
+            this->lb_loss = loss_result.lb_loss;
+            this->rb_loss = loss_result.rb_loss;
+            this->total_avg_loss = loss_result.total_avg_loss;
+            return *this;
+        }
+    };
+    struct Detect_result
+    {
+     public:
+        double cx;
+        double cy;
+        double theta;
+        bool success;
+        Loss_result loss;
+        Detect_result& operator=(const Detect_result& detect_result)
+        {
+            this->cx = detect_result.cx;
+            this->cy = detect_result.cy;
+            this->theta = detect_result.theta;
+            this->success = detect_result.success;
+            this->loss=detect_result.loss;
+            return *this;
+        }
+    };
+    detect_ceres3d(pcl::PointCloud<pcl::PointXYZ>::Ptr model_grid_cloud);
+    ~detect_ceres3d();
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Detect_result &detect_result, std::string &error_info);
+    bool detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Detect_result &detect_result,
+                pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud,std::string &error_info);
+
+protected:
+    void save_model(std::string file);
+
+ private:
+    HybridGrid*                                  mp_model_grid;
+
+
+
+};
+
+
+#endif //CERES_TEST_DETECT_WHEEL_CERES_H

verify/match3d/hybrid_grid.hpp

@@ -0,0 +1,622 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_
+#define CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_
+
+#include <array>
+#include <cmath>
+#include <limits>
+#include <utility>
+#include <vector>
+
+#include "Eigen/Core"
+#include "common/math.h"
+#include "common/port.h"
+//#include "probability_values.h"
+#include "glog/logging.h"
+
+
+// Converts an 'index' with each dimension from 0 to 2^'bits' - 1 to a flat
+// z-major index.
+inline int ToFlatIndex(const Eigen::Array3i& index, const int bits) {
+  DCHECK((index >= 0).all() && (index < (1 << bits)).all()) << index;
+  return (((index.z() << bits) + index.y()) << bits) + index.x();
+}
+
+// Converts a flat z-major 'index' to a 3-dimensional index with each dimension
+// from 0 to 2^'bits' - 1.
+inline Eigen::Array3i To3DIndex(const int index, const int bits) {
+  DCHECK_LT(index, 1 << (3 * bits));
+  const int mask = (1 << bits) - 1;
+  return Eigen::Array3i(index & mask, (index >> bits) & mask,
+                        (index >> bits) >> bits);
+}
+
+// A function to compare value to the default value. (Allows specializations).
+template <typename TValueType>
+bool IsDefaultValue(const TValueType& v) {
+  return v == TValueType();
+}
+
+// Specialization to compare a std::vector to the default value.
+template <typename TElementType>
+bool IsDefaultValue(const std::vector<TElementType>& v) {
+  return v.empty();
+}
+
+// A flat grid of '2^kBits' x '2^kBits' x '2^kBits' voxels storing values of
+// type 'ValueType' in contiguous memory. Indices in each dimension are 0-based.
+
+
+// 包含自定义类型的数组
+template <typename TValueType, int kBits>
+class FlatGrid
+{
+ public:
+    using ValueType = TValueType;
+
+    // Creates a new flat grid with all values being default constructed.
+    FlatGrid()
+    {
+        for (ValueType &value : cells_)
+        {
+            value = ValueType();
+        }
+    }
+
+    FlatGrid(const FlatGrid &) = delete;
+
+    FlatGrid &operator=(const FlatGrid &) = delete;
+
+    // Returns the number of voxels per dimension.
+    static int grid_size()
+    {
+        return 1 << kBits;
+    }
+
+    // Returns the value stored at 'index', each dimension of 'index' being
+    // between 0 and grid_size() - 1.
+    ValueType value(const Eigen::Array3i &index) const
+    {
+        return cells_[ToFlatIndex(index, kBits)];
+    }
+
+    // Returns a pointer to a value to allow changing it.
+    ValueType *mutable_value(const Eigen::Array3i &index)
+    {
+        return &cells_[ToFlatIndex(index, kBits)];
+    }
+
+    // An iterator for iterating over all values not comparing equal to the
+    // default constructed value.
+    class Iterator
+    {
+     public:
+        Iterator() : current_(nullptr), end_(nullptr)
+        {
+        }
+
+        explicit Iterator(const FlatGrid &flat_grid)
+                : current_(flat_grid.cells_.data()),
+                  end_(flat_grid.cells_.data() + flat_grid.cells_.size())
+        {
+            while (!Done() && IsDefaultValue(*current_))
+            {
+                ++current_;
+            }
+        }
+
+        void Next()
+        {
+            DCHECK(!Done());
+            do
+            {
+                ++current_;
+            } while (!Done() && IsDefaultValue(*current_));
+        }
+
+        bool Done() const
+        {
+            return current_ == end_;
+        }
+
+        Eigen::Array3i GetCellIndex() const
+        {
+            DCHECK(!Done());
+            const int index = (1 << (3 * kBits)) - (end_ - current_);
+            return To3DIndex(index, kBits);
+        }
+
+        const ValueType &GetValue() const
+        {
+            DCHECK(!Done());
+            return *current_;
+        }
+
+     private:
+        const ValueType *current_;
+        const ValueType *end_;
+    };
+
+ private:
+    std::array<ValueType, 1 << (3 * kBits)> cells_;
+};
+
+// A grid consisting of '2^kBits' x '2^kBits' x '2^kBits' grids of type
+// 'WrappedGrid'. Wrapped grids are constructed on first access via
+// 'mutable_value()'.
+// grid 的 grid
+template <typename WrappedGrid, int kBits>
+class NestedGrid
+{
+ public:
+    using ValueType = typename WrappedGrid::ValueType;
+
+    // Returns the number of voxels per dimension.
+    static int grid_size()
+    {
+        return WrappedGrid::grid_size() << kBits;
+    }
+
+    // Returns the value stored at 'index', each dimension of 'index' being
+    // between 0 and grid_size() - 1.
+    ValueType value(const Eigen::Array3i &index) const
+    {
+        const Eigen::Array3i meta_index = GetMetaIndex(index);
+        const WrappedGrid *const meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, kBits)].get();
+        if (meta_cell == nullptr)
+        {
+            return ValueType();
+        }
+        const Eigen::Array3i inner_index =
+                index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->value(inner_index);
+    }
+
+    // Returns a pointer to the value at 'index' to allow changing it. If
+    // necessary a new wrapped grid is constructed to contain that value.
+    ValueType *mutable_value(const Eigen::Array3i &index)
+    {
+        const Eigen::Array3i meta_index = GetMetaIndex(index);
+        std::unique_ptr<WrappedGrid> &meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, kBits)];
+        if (meta_cell == nullptr)
+        {
+            meta_cell = std::make_unique<WrappedGrid>();
+        }
+        const Eigen::Array3i inner_index =
+                index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->mutable_value(inner_index);
+    }
+
+    // An iterator for iterating over all values not comparing equal to the
+    // default constructed value.
+    class Iterator
+    {
+     public:
+        Iterator() : current_(nullptr), end_(nullptr), nested_iterator_()
+        {
+        }
+
+        explicit Iterator(const NestedGrid &nested_grid)
+                : current_(nested_grid.meta_cells_.data()),
+                  end_(nested_grid.meta_cells_.data() + nested_grid.meta_cells_.size()),
+                  nested_iterator_()
+        {
+            AdvanceToValidNestedIterator();
+        }
+
+        void Next()
+        {
+            DCHECK(!Done());
+            nested_iterator_.Next();
+            if (!nested_iterator_.Done())
+            {
+                return;
+            }
+            ++current_;
+            AdvanceToValidNestedIterator();
+        }
+
+        bool Done() const
+        {
+            return current_ == end_;
+        }
+
+        Eigen::Array3i GetCellIndex() const
+        {
+            DCHECK(!Done());
+            const int index = (1 << (3 * kBits)) - (end_ - current_);
+            return To3DIndex(index, kBits) * WrappedGrid::grid_size() +
+                    nested_iterator_.GetCellIndex();
+        }
+
+        const ValueType &GetValue() const
+        {
+            DCHECK(!Done());
+            return nested_iterator_.GetValue();
+        }
+
+     private:
+        void AdvanceToValidNestedIterator()
+        {
+            for (; !Done(); ++current_)
+            {
+                if (*current_ != nullptr)
+                {
+                    nested_iterator_ = typename WrappedGrid::Iterator(**current_);
+                    if (!nested_iterator_.Done())
+                    {
+                        break;
+                    }
+                }
+            }
+        }
+
+        const std::unique_ptr<WrappedGrid> *current_;
+        const std::unique_ptr<WrappedGrid> *end_;
+        typename WrappedGrid::Iterator nested_iterator_;
+    };
+
+ private:
+    // Returns the Eigen::Array3i (meta) index of the meta cell containing
+    // 'index'.
+    Eigen::Array3i GetMetaIndex(const Eigen::Array3i &index) const
+    {
+        DCHECK((index >= 0).all()) << index;
+        const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
+        DCHECK((meta_index < (1 << kBits)).all()) << index;
+        return meta_index;
+    }
+
+    std::array<std::unique_ptr<WrappedGrid>, 1 << (3 * kBits)> meta_cells_;
+};
+
+// A grid consisting of 2x2x2 grids of type 'WrappedGrid' initially. Wrapped
+// grids are constructed on first access via 'mutable_value()'. If necessary,
+// the grid grows to twice the size in each dimension. The range of indices is
+// (almost) symmetric around the origin, i.e. negative indices are allowed.
+
+
+template <typename WrappedGrid>
+class DynamicGrid
+{
+ public:
+    using ValueType = typename WrappedGrid::ValueType;
+
+    DynamicGrid() : bits_(1), meta_cells_(8)
+    {
+    }
+
+    DynamicGrid(DynamicGrid &&) = default;
+
+    DynamicGrid &operator=(DynamicGrid &&) = default;
+
+    // Returns the current number of voxels per dimension.
+    int grid_size() const
+    {
+        return WrappedGrid::grid_size() << bits_;
+    }
+
+    // Returns the value stored at 'index'.
+    ValueType value(const Eigen::Array3i &index) const
+    {
+        const Eigen::Array3i shifted_index = index + (grid_size() >> 1);
+        // The cast to unsigned is for performance to check with 3 comparisons
+        // shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
+        if ((shifted_index.cast<unsigned int>() >= grid_size()).any())
+        {
+            return ValueType();
+        }
+        const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
+        const WrappedGrid *const meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, bits_)].get();
+        if (meta_cell == nullptr)
+        {
+            return ValueType();
+        }
+        const Eigen::Array3i inner_index =
+                shifted_index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->value(inner_index);
+    }
+
+    // Returns a pointer to the value at 'index' to allow changing it, dynamically
+    // growing the DynamicGrid and constructing new WrappedGrids as needed.
+    ValueType *mutable_value(const Eigen::Array3i &index)
+    {
+        const Eigen::Array3i shifted_index = index+ (grid_size() >> 1);
+        // The cast to unsigned is for performance to check with 3 comparisons
+         //shifted_index.[xyz] >= 0 and shifted_index.[xyz] < grid_size.
+        if ((shifted_index.cast<unsigned int>() >= grid_size()).any())
+        {
+            LOG(ERROR)<<shifted_index.transpose()<<" bound:"<<grid_size();
+            LOG(ERROR)<<" out of bound";
+            //Grow();
+            return 0;//mutable_value(index);
+        }
+        const Eigen::Array3i meta_index = GetMetaIndex(shifted_index);
+        std::unique_ptr<WrappedGrid> &meta_cell =
+                meta_cells_[ToFlatIndex(meta_index, bits_)];
+        if (meta_cell == nullptr)
+        {
+            meta_cell = std::make_unique<WrappedGrid>();
+        }
+        const Eigen::Array3i inner_index =
+                shifted_index - meta_index * WrappedGrid::grid_size();
+        return meta_cell->mutable_value(inner_index);
+    }
+
+    // An iterator for iterating over all values not comparing equal to the
+    // default constructed value.
+    class Iterator
+    {
+     public:
+        explicit Iterator(const DynamicGrid &dynamic_grid)
+                : bits_(dynamic_grid.bits_),
+                  current_(dynamic_grid.meta_cells_.data()),
+                  end_(dynamic_grid.meta_cells_.data() +
+                               dynamic_grid.meta_cells_.size()),
+                  nested_iterator_()
+        {
+            AdvanceToValidNestedIterator();
+        }
+
+        void Next()
+        {
+            DCHECK(!Done());
+            nested_iterator_.Next();
+            if (!nested_iterator_.Done())
+            {
+                return;
+            }
+            ++current_;
+            AdvanceToValidNestedIterator();
+        }
+
+        bool Done() const
+        {
+            return current_ == end_;
+        }
+
+        Eigen::Array3i GetCellIndex() const
+        {
+            DCHECK(!Done());
+            const int outer_index = (1 << (3 * bits_)) - (end_ - current_);
+            const Eigen::Array3i shifted_index =
+                    To3DIndex(outer_index, bits_) * WrappedGrid::grid_size() +
+                            nested_iterator_.GetCellIndex();
+            return shifted_index - ((1 << (bits_ - 1)) * WrappedGrid::grid_size());
+        }
+
+        const ValueType &GetValue() const
+        {
+            DCHECK(!Done());
+            return nested_iterator_.GetValue();
+        }
+
+        void AdvanceToEnd()
+        {
+            current_ = end_;
+        }
+
+        const std::pair<Eigen::Array3i, ValueType> operator*() const
+        {
+            return std::pair<Eigen::Array3i, ValueType>(GetCellIndex(), GetValue());
+        }
+
+        Iterator &operator++()
+        {
+            Next();
+            return *this;
+        }
+
+        bool operator!=(const Iterator &it) const
+        {
+            return it.current_ != current_;
+        }
+
+     private:
+        void AdvanceToValidNestedIterator()
+        {
+            for (; !Done(); ++current_)
+            {
+                if (*current_ != nullptr)
+                {
+                    nested_iterator_ = typename WrappedGrid::Iterator(**current_);
+                    if (!nested_iterator_.Done())
+                    {
+                        break;
+                    }
+                }
+            }
+        }
+
+        int bits_;
+        const std::unique_ptr<WrappedGrid> *current_;
+        const std::unique_ptr<WrappedGrid> *const end_;
+        typename WrappedGrid::Iterator nested_iterator_;
+    };
+
+ private:
+    // Returns the Eigen::Array3i (meta) index of the meta cell containing
+    // 'index'.
+    Eigen::Array3i GetMetaIndex(const Eigen::Array3i &index) const
+    {
+        DCHECK((index >= 0).all()) << index;
+        const Eigen::Array3i meta_index = index / WrappedGrid::grid_size();
+        DCHECK((meta_index < (1 << bits_)).all()) << index;
+        return meta_index;
+    }
+
+    // Grows this grid by a factor of 2 in each of the 3 dimensions.
+    void Grow()
+    {
+        const int new_bits = bits_ + 1;
+        CHECK_LE(new_bits, 8);
+        std::vector<std::unique_ptr<WrappedGrid>> new_meta_cells_(
+                8 * meta_cells_.size());
+        for (int z = 0; z != (1 << bits_); ++z)
+        {
+            for (int y = 0; y != (1 << bits_); ++y)
+            {
+                for (int x = 0; x != (1 << bits_); ++x)
+                {
+                    const Eigen::Array3i original_meta_index(x, y, z);
+                    const Eigen::Array3i new_meta_index =
+                            original_meta_index + (1 << (bits_ - 1));
+                    new_meta_cells_[ToFlatIndex(new_meta_index, new_bits)] =
+                            std::move(meta_cells_[ToFlatIndex(original_meta_index, bits_)]);
+                }
+            }
+        }
+        meta_cells_ = std::move(new_meta_cells_);
+        bits_ = new_bits;
+    }
+
+    int bits_;
+    std::vector<std::unique_ptr<WrappedGrid>> meta_cells_;
+};
+
+template <typename ValueType>
+using GridBase = DynamicGrid<NestedGrid<FlatGrid<ValueType, 3>, 3>>;
+
+// Represents a 3D grid as a wide, shallow tree.
+template <typename ValueType>
+class HybridGridBase : public GridBase<ValueType>
+{
+ public:
+    using Iterator = typename GridBase<ValueType>::Iterator;
+
+    // Creates a new tree-based probability grid with voxels having edge length
+    // 'resolution' around the origin which becomes the center of the cell at
+    // index (0, 0, 0).
+    explicit HybridGridBase(const float resolution) : resolution_(resolution)
+    {
+    }
+
+    float resolution() const
+    {
+        return resolution_;
+    }
+
+    // Returns the index of the cell containing the 'point'. Indices are integer
+    // vectors identifying cells, for this the coordinates are rounded to the next
+    // multiple of the resolution.
+    Eigen::Array3i GetCellIndex(const Eigen::Vector3f &point) const
+    {
+        Eigen::Array3f index = point.array() / resolution_;
+        return Eigen::Array3i(common::RoundToInt(index.x()),
+                              common::RoundToInt(index.y()),
+                              common::RoundToInt(index.z()));
+    }
+
+    // Returns one of the octants, (0, 0, 0), (1, 0, 0), ..., (1, 1, 1).
+    static Eigen::Array3i GetOctant(const int i)
+    {
+        DCHECK_GE(i, 0);
+        DCHECK_LT(i, 8);
+        return Eigen::Array3i(static_cast<bool>(i & 1), static_cast<bool>(i & 2),
+                              static_cast<bool>(i & 4));
+    }
+
+    // Returns the center of the cell at 'index'.
+    Eigen::Vector3f GetCenterOfCell(const Eigen::Array3i &index) const
+    {
+        return index.matrix().cast<float>() * resolution_;
+    }
+
+    // Iterator functions for range-based for loops.
+    Iterator begin() const
+    {
+        return Iterator(*this);
+    }
+
+    Iterator end() const
+    {
+        Iterator it(*this);
+        it.AdvanceToEnd();
+        return it;
+    }
+
+ private:
+    // Edge length of each voxel.
+    const float resolution_;
+};
+
+
+class HybridGrid : public HybridGridBase<float>
+{
+ public:
+    explicit HybridGrid(const float resolution)
+            : HybridGridBase<float>(resolution)
+    {
+    }
+
+
+    // Sets the probability of the cell at 'index' to the given 'probability'.
+    void SetProbability(const Eigen::Array3i &index, const float probability)
+    {
+        *mutable_value(index) = probability;
+    }
+
+
+    // Returns the probability of the cell with 'index'.
+    float GetProbability(const Eigen::Array3i &index) const
+    {
+        return value(index);
+    }
+
+    // Returns true if the probability at the specified 'index' is known.
+    bool IsKnown(const Eigen::Array3i &index) const
+    {
+        return value(index) != 0;
+    }
+
+ private:
+    // Markers at changed cells.
+    std::vector<ValueType *> update_indices_;
+};
+
+/*struct AverageIntensityData {
+  float sum = 0.f;
+  int count = 0;
+};
+
+class IntensityHybridGrid : public HybridGridBase<AverageIntensityData> {
+ public:
+  explicit IntensityHybridGrid(const float resolution)
+      : HybridGridBase<AverageIntensityData>(resolution) {}
+
+  void AddIntensity(const Eigen::Array3i& index, const float intensity) {
+    AverageIntensityData* const cell = mutable_value(index);
+    cell->count += 1;
+    cell->sum += intensity;
+  }
+
+  float GetIntensity(const Eigen::Array3i& index) const {
+    const AverageIntensityData& cell = value(index);
+    if (cell.count == 0) {
+      return 0.f;
+    } else {
+      return cell.sum / cell.count;
+    }
+  }
+};
+*/
+
+#endif  // CARTOGRAPHER_MAPPING_3D_HYBRID_GRID_H_

verify/match3d/interpolated_grid.hpp

@@ -0,0 +1,157 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_
+
+#include <cmath>
+
+#include "hybrid_grid.hpp"
+
+
+// Interpolates between HybridGrid voxels. We use the tricubic
+// interpolation which interpolates the values and has vanishing derivative at
+// these points.
+//
+// This class is templated to work with the autodiff that Ceres provides.
+// For this reason, it is also important that the interpolation scheme be
+// continuously differentiable.
+template <class HybridGridType>
+class InterpolatedGrid {
+ public:
+  explicit InterpolatedGrid(const HybridGridType& hybrid_grid)
+      : hybrid_grid_(hybrid_grid) {}
+
+  InterpolatedGrid(const InterpolatedGrid<HybridGridType>&) = delete;
+  InterpolatedGrid& operator=(const InterpolatedGrid<HybridGridType>&) = delete;
+
+  // Returns the interpolated value at (x, y, z) of the HybridGrid
+  // used to perform the interpolation.
+  //
+  // This is a piecewise, continuously differentiable function. We use the
+  // scalar part of Jet parameters to select our interval below. It is the
+  // tensor product volume of piecewise cubic polynomials that interpolate
+  // the values, and have vanishing derivative at the interval boundaries.
+  template <typename T>
+  T GetInterpolatedValue(const T& x, const T& y, const T& z) const {
+    double x1, y1, z1, x2, y2, z2;
+    ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2);
+
+    const Eigen::Array3i index1 =
+        hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1));
+    const double q111 = GetValue(hybrid_grid_, index1);
+    const double q112 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 0, 1));
+    const double q121 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 0));
+    const double q122 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(0, 1, 1));
+    const double q211 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 0));
+    const double q212 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 0, 1));
+    const double q221 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 0));
+    const double q222 =
+        GetValue(hybrid_grid_, index1 + Eigen::Array3i(1, 1, 1));
+
+    const T normalized_x = (x - x1) / (x2 - x1);
+    const T normalized_y = (y - y1) / (y2 - y1);
+    const T normalized_z = (z - z1) / (z2 - z1);
+
+    // Compute pow(..., 2) and pow(..., 3). Using pow() here is very expensive.
+    const T normalized_xx = normalized_x * normalized_x;
+    const T normalized_xxx = normalized_x * normalized_xx;
+    const T normalized_yy = normalized_y * normalized_y;
+    const T normalized_yyy = normalized_y * normalized_yy;
+    const T normalized_zz = normalized_z * normalized_z;
+    const T normalized_zzz = normalized_z * normalized_zz;
+
+    // We first interpolate in z, then y, then x. All 7 times this uses the same
+    // scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2).
+    // The first polynomial is 1 at t=0, 0 at t=1, the second polynomial is 0
+    // at t=0, 1 at t=1. Both polynomials have derivative zero at t=0 and t=1.
+    const T q11 = (q111 - q112) * normalized_zzz * 2. +
+                  (q112 - q111) * normalized_zz * 3. + q111;
+    const T q12 = (q121 - q122) * normalized_zzz * 2. +
+                  (q122 - q121) * normalized_zz * 3. + q121;
+    const T q21 = (q211 - q212) * normalized_zzz * 2. +
+                  (q212 - q211) * normalized_zz * 3. + q211;
+    const T q22 = (q221 - q222) * normalized_zzz * 2. +
+                  (q222 - q221) * normalized_zz * 3. + q221;
+    const T q1 = (q11 - q12) * normalized_yyy * 2. +
+                 (q12 - q11) * normalized_yy * 3. + q11;
+    const T q2 = (q21 - q22) * normalized_yyy * 2. +
+                 (q22 - q21) * normalized_yy * 3. + q21;
+    return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. +
+           q1;
+  }
+
+ private:
+  template <typename T>
+  void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z,
+                                      double* x1, double* y1, double* z1,
+                                      double* x2, double* y2,
+                                      double* z2) const {
+    const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z);
+    *x1 = lower.x();
+    *y1 = lower.y();
+    *z1 = lower.z();
+    *x2 = lower.x() + hybrid_grid_.resolution();
+    *y2 = lower.y() + hybrid_grid_.resolution();
+    *z2 = lower.z() + hybrid_grid_.resolution();
+  }
+
+  // Center of the next lower voxel, i.e., not necessarily the voxel containing
+  // (x, y, z). For each dimension, the largest voxel index so that the
+  // corresponding center is at most the given coordinate.
+  Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y,
+                                     const double z) const {
+    // Center of the cell containing (x, y, z).
+    Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell(
+        hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
+    // Move to the next lower voxel center.
+    if (center.x() > x) {
+      center.x() -= hybrid_grid_.resolution();
+    }
+    if (center.y() > y) {
+      center.y() -= hybrid_grid_.resolution();
+    }
+    if (center.z() > z) {
+      center.z() -= hybrid_grid_.resolution();
+    }
+    return center;
+  }
+
+  // Uses the scalar part of a Ceres Jet.
+  template <typename T>
+  Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y,
+                                     const T& jet_z) const {
+    return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
+  }
+
+  static float GetValue(const HybridGrid& probability_grid,
+                        const Eigen::Array3i& index) {
+    return probability_grid.GetProbability(index);
+  }
+
+  const HybridGridType& hybrid_grid_;
+};
+
+using InterpolatedProbabilityGrid = InterpolatedGrid<HybridGrid>;
+
+
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTERPOLATED_GRID_H_

verify/offset_solver.cpp

@@ -0,0 +1,197 @@
+//
+// Created by zx on 2021/8/26.
+//
+
+#include "offset_solver.h"
+
+//
+// Created by zx on 2021/7/1.
+//
+#include <cppad/ipopt/solve.hpp>
+#include <glog/logging.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/common/transforms.h>
+#include <pcl/filters/statistical_outlier_removal.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+
+using CppAD::AD;
+double CPPDA_PA=30.0;
+double CPPDA_PB=10.0;
+class affine_cppda_cost
+{
+ public:
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud;
+    double m_ox;
+    double m_oy;
+    affine_cppda_cost(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,double ox,double oy)
+            :m_cloud(cloud),m_ox(ox),m_oy(oy)
+    {}
+    typedef CPPAD_TESTVECTOR( AD<double> ) ADvector;
+    void operator()(ADvector& fg, const ADvector& x)
+    {
+        assert( fg.size() == 1 );   //误差
+        assert( x.size()  == 3);
+
+        AD<double> cx = x[1];
+        AD<double> cy = x[2];
+        AD<double> ca = x[0];
+
+        fg[0]=0;
+        for(int i=0;i<m_cloud->size();++i)
+        {
+            pcl::PointXYZ p=m_cloud->points[i];
+
+            double x=p.x-m_ox;
+            double y=p.y-m_oy;
+
+            AD<double> nx=CppAD::cos(ca)*x-CppAD::sin(ca)*y +cx;
+            AD<double> ny=CppAD::sin(ca)*x+CppAD::cos(ca)*y +cy;
+
+
+            fg[0]+=CppAD::pow(1.0/(1.0+CppAD::exp(CPPDA_PA*(nx+1.0)))
+                                      +1.0/(1.0+CppAD::exp(-CPPDA_PA*(nx-1.0))),2);
+
+            fg[0]+=CppAD::pow(1.0/(1.0+CppAD::exp(CPPDA_PB*(ny+2.4)))
+                                      +1.0/(1.0+CppAD::exp(-CPPDA_PB*(ny-2.4))),2);
+
+        }
+
+
+    }
+};
+
+offset_solver::offset_solver(double minx,double maxx,double mina,double maxa)
+    :m_minx(minx),m_maxx(maxx),m_mina(mina),m_maxa(maxa)
+{
+
+}
+
+Error_manager offset_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                    pcl::PointXYZ rotate_center,float& cx,float& cy,float& theta)
+{
+
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(*cloud, centroid);
+
+    LOG(INFO)<<"  cloud size:"<<cloud->size()<<" , center :"<<-centroid[0]<<"  ,"<<-centroid[1];
+    double variable[] = {theta,cx,cy};
+
+    bool ok = true;
+    size_t i;
+    typedef CPPAD_TESTVECTOR( double ) Dvector;
+
+    // number of independent variables (domain dimension for f and g)
+    size_t n_vars = 3;
+    // number of constraints (range dimension for g)
+    //size_t ng = 1;
+    // initial value of the independent variables
+    Dvector vars(n_vars);
+    for (int i = 0; i < n_vars; i++) {
+        vars[i] = 0.0;
+    }
+
+    Dvector vars_lowerbound(n_vars);
+    Dvector vars_upperbound(n_vars);
+    // Sets lower and upper limits for variables.
+    // Set all non-actuators upper and lowerlimits
+    // to the max negative and positive values.
+    vars_lowerbound[0]=m_mina;
+    vars_upperbound[0]=m_maxa;
+
+    vars_lowerbound[1] = m_minx;
+    vars_upperbound[1] = m_maxa;
+    vars_lowerbound[2] = -1.0e19;
+    vars_upperbound[2] = 1.0e19;
+
+
+    affine_cppda_cost affine_cost(cloud,rotate_center.x,rotate_center.y);
+    std::string options;
+    // Uncomment this if you'd like more print information
+    options += "Integer print_level  0\n";
+
+    options += "Sparse  true        forward\n";
+    options += "Sparse  true        reverse\n";
+    options += "Numeric max_cpu_time          0.5\n";
+
+    // place to return solution
+    CppAD::ipopt::solve_result<Dvector> solution;
+
+    // solve the problem
+    CppAD::ipopt::solve<Dvector, affine_cppda_cost>(
+            options, vars, vars_lowerbound, vars_upperbound, Dvector(),
+            Dvector(), affine_cost, solution);
+
+    // Check some of the solution values
+    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+    // Cost
+    if(ok)
+    {
+        auto cost = solution.obj_value;
+
+        theta = solution.x[0];
+        cx = solution.x[1];
+        cy = solution.x[2];
+
+        printf(" cppad solver  theta:%.3f , x;%.5f , y:%.5f\n",
+               theta * 180.0 / M_PI, cx, cy);
+
+        return SUCCESS;
+    }
+    return FAILED;
+}
+
+Error_manager offset_solver::solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                                   pcl::PointXYZ rotate_center,float& cx,float& cy,float& theta,
+                                   pcl::visualization::PCLVisualizer& viewer)
+{
+    Error_manager code=solve(cloud,rotate_center,cx,cy,theta);
+    if(code!=SUCCESS)
+    {
+        viewer.addText(code.get_error_description(),50,50,1,0,0,"offset_solve_error_code");
+        return code;
+    }
+
+    //先平移点云，后旋转,再平移
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_2.translation() << -rotate_center.x, -rotate_center.y, 0;
+    pcl::transformPointCloud(*cloud, *transformed_cloud, transform_2);
+
+    Eigen::Affine3f transform_r = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud1(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_r.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
+    pcl::transformPointCloud(*transformed_cloud, *transformed_cloud1, transform_r);
+
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud2(new pcl::PointCloud<pcl::PointXYZ>());
+    Eigen::Affine3f transform_t = Eigen::Affine3f::Identity();
+    transform_t.translation() << cx, 0, 0;
+    pcl::transformPointCloud(*transformed_cloud1, *transformed_cloud2, transform_t);
+
+    //创建坐标函数点云，用于显示
+    pcl::PointCloud<pcl::PointXYZ>::Ptr viewer_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    for (float x = -2; x < 2; x += 0.01)
+    {
+        float y = 4.0 * (1.0 / (1.0 + exp(CPPDA_PA * (x + 1.0)))
+                + 1.0 / (1.0 + exp(-CPPDA_PA * (x - 1.0))) - 1) + 1;
+        viewer_cloud->push_back(pcl::PointXYZ(x, y, 0));
+
+        float zx = 4.0 * (1.0 / (1.0 + exp(CPPDA_PB * (3 * x + 2.4)))
+                + 1.0 / (1.0 + exp(-CPPDA_PB * (3 * x - 2.4))) - 1) + 2.5;
+        viewer_cloud->push_back(pcl::PointXYZ(zx, 3 * x, 0));
+
+    }
+
+
+    viewer.removeAllPointClouds();
+    viewer.addPointCloud(transformed_cloud2, "cloud_");
+
+    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0); // green
+    viewer.addPointCloud(viewer_cloud, single_color, "cloud_axis");
+    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_axis");
+    viewer.spinOnce();
+
+    return code;
+}

verify/offset_solver.h

@@ -0,0 +1,35 @@
+//
+// Created by zx on 2021/8/26.
+//
+
+#ifndef SHUTTER_VERIFY_VERIFY_OFFSET_SOLVER_H_
+#define SHUTTER_VERIFY_VERIFY_OFFSET_SOLVER_H_
+#include "error_code.h"
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/visualization/pcl_visualizer.h>
+class offset_solver
+{
+ public:
+    offset_solver(double minx,double maxx,double mina,double maxa);
+
+
+
+    Error_manager solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                        pcl::PointXYZ rotate_center,float& cx,float& cy,float& theta);
+
+    Error_manager solve(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
+                        pcl::PointXYZ rotate_center,float& cx,float& cy,float& theta,
+                        pcl::visualization::PCLVisualizer& viewer);
+
+ protected:
+    double          m_minx;
+    double          m_maxx;
+    double          m_mina;
+    double          m_maxa;
+
+
+};
+
+
+#endif //SHUTTER_VERIFY_VERIFY_OFFSET_SOLVER_H_

verify/rotate_center_caliber.cpp

@@ -0,0 +1,274 @@
+//
+// Created by zx on 2021/8/27.
+//
+
+#include "rotate_center_caliber.h"
+#include <pcl/registration/ndt.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+extern pcl::visualization::PCLVisualizer viewer;
+extern std::mutex mut;
+
+
+rotate_center_caliber::rotate_center_caliber()
+{
+}
+rotate_center_caliber::~rotate_center_caliber()
+{
+    delete mp_matcher;
+    mp_matcher= nullptr;
+}
+
+void rotate_center_caliber::set_first_frame(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model)
+{
+    m_model_cloud=cloud_model;
+    Eigen::Vector4f centroid;
+    pcl::compute3DCentroid(*cloud_model, centroid);
+    m_center.x=centroid[0];
+    m_center.y=centroid[1];
+    m_center.z=centroid[2];
+
+    //平移点云
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_2.translation() << -centroid[0],-centroid[1],-centroid[2];
+    pcl::transformPointCloud(*cloud_model, *transformed_cloud, transform_2);
+
+    printf("model center: %.5f,%.5f,%.5f\n",centroid[0],centroid[1],centroid[2]);
+
+
+    mp_matcher=new detect_ceres3d(transformed_cloud);
+}
+
+bool rotate_center_caliber::push_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
+{
+
+    //平移点云
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+    pcl::PointCloud<pcl::PointXYZ>::Ptr offset_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    transform_2.translation() << -m_center.x,-m_center.y,-m_center.z;
+    pcl::transformPointCloud(*cloud, *offset_cloud, transform_2);
+
+    mut.lock();
+    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0); // green
+    viewer.removePointCloud("cloud_rotation");
+    viewer.addPointCloud(cloud, single_color,"cloud_rotation");
+    mut.unlock();
+
+
+    if(mp_matcher== nullptr)
+        return false;
+
+    static detect_ceres3d::Detect_result last_result;
+    std::string error;
+    pcl::PointCloud<pcl::PointXYZ>::Ptr transform_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+    detect_ceres3d::Detect_result result=last_result;
+    if(mp_matcher->detect(offset_cloud,result,transform_cloud,error))
+    {
+        if(fabs(result.theta-last_result.theta)>1.0/180.*M_PI)
+        {
+            printf(" detect success  r : %.3f   t:%.5f,%.5f\n", result.theta * 180. / M_PI, result.cx, result.cy);
+
+            Eigen::Affine3f transform_affine = Eigen::Affine3f::Identity();
+            pcl::PointCloud<pcl::PointXYZ>::Ptr new_cloud(new pcl::PointCloud<pcl::PointXYZ>());
+            transform_affine.translation() << m_center.x, m_center.y, m_center.z;
+            pcl::transformPointCloud(*transform_cloud, *new_cloud, transform_affine);
+
+            mut.lock();
+            pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 255, 0, 0); // green
+            viewer.removePointCloud("ndt_cloud");
+            viewer.addPointCloud(new_cloud, single_color, "ndt_cloud");
+            mut.unlock();
+
+            match_transform solve_data;
+            solve_data.theta = result.theta;
+            solve_data.dx = result.cx;
+            solve_data.dy = result.cy;
+
+            m_match_datas.push_back(solve_data);
+        }
+
+    }
+
+
+    /*Eigen::Matrix<float,6,1>  temp;
+    temp.setZero();
+    static Eigen::Matrix<float, 6,1> last_transform=temp;
+    printf("init r:%.3f,%.3f,%.3f, t :%.5f,%.5f,%.5f\n",
+            last_transform(2,0)*180.0/M_PI,last_transform(1,0)*180.0/M_PI,last_transform(0,0)*180.0/M_PI,
+           last_transform(3,0),last_transform(4,0),last_transform(5,0));
+    Eigen::Matrix4f matrix=match(m_model_cloud,cloud,last_transform);
+
+    //std::cout<<"  match :"<<matrix<<std::endl;
+
+    Eigen::Matrix3f roation=matrix.topLeftCorner(3,3);
+    Eigen::Vector3f euler_angles = roation.eulerAngles(0, 1, 2);
+
+    //std::cout<<euler_angles<<std::endl;
+    printf("r:%.3f,%.3f,%.3f  t:%.5f,%.5f\n",euler_angles[2]*180.0/M_PI,euler_angles[1]*180.0/M_PI,
+           euler_angles[0]*180.0/M_PI,matrix(0,3),matrix(1,3));
+
+    match_transform solve_data;
+    solve_data.theta=euler_angles[2];
+    solve_data.dx=matrix(0,3);
+    solve_data.dy=matrix(1,3);
+
+    m_match_datas.push_back(solve_data);
+
+    last_transform[0]=euler_angles[0];
+    last_transform[1]=euler_angles[1];
+    last_transform[2]=euler_angles[2];
+
+    last_transform[3]=matrix(0,3);
+    last_transform[4]=matrix(1,3);
+    last_transform[5]=matrix(2,3);
+*/
+
+    if(solve())
+    {
+        printf(" solve center = %.5f,%.5f\n",
+                m_rotation_center_x,m_rotation_center_y);
+
+        updata_points();
+    }
+
+}
+
+
+Eigen::Matrix4f rotate_center_caliber::match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,
+        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,Eigen::Matrix<float, 6,1> init_transform)
+{
+    //初始化正态分布变换（NDT）
+    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
+    //设置依赖尺度NDT参数
+    //为终止条件设置最小转换差异
+    ndt.setTransformationEpsilon(0.0001);
+    //为More-Thuente线搜索设置最大步长
+    ndt.setStepSize(0.1);
+    //设置NDT网格结构的分辨率（VoxelGridCovariance）
+    ndt.setResolution(0.3);
+    //设置匹配迭代的最大次数
+    ndt.setMaximumIterations(100);
+    // 设置要配准的点云
+    ndt.setInputCloud(cloud);
+    //设置点云配准目标
+    ndt.setInputTarget(cloud_center);
+    //设置使用机器人测距法得到的初始对准估计结果
+
+    Eigen::AngleAxisf rollAngle(Eigen::AngleAxisf(init_transform(0), Eigen::Vector3f::UnitX()));
+    Eigen::AngleAxisf pitchAngle(Eigen::AngleAxisf(init_transform(1), Eigen::Vector3f::UnitY()));
+    Eigen::AngleAxisf yawAngle(Eigen::AngleAxisf(init_transform(2), Eigen::Vector3f::UnitZ()));
+    Eigen::AngleAxisf rotation ;
+    rotation = yawAngle*pitchAngle*rollAngle;
+    Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
+
+    transform_2.translation() << init_transform(3),init_transform(4),init_transform(5);	// 三个数分别对应X轴、Y轴、Z轴方向上的平移
+    transform_2.rotate(rotation);	//同理，UnitX(),绕X轴；UnitY(
+
+    Eigen::Matrix4f init_guess = transform_2.matrix();
+    //计算需要的刚体变换以便将输入的点云匹配到目标点云
+    pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+    ndt.align(*output_cloud, init_guess);
+
+    std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged()
+              << " score: " << ndt.getFitnessScore() << std::endl;
+
+
+
+    //使用创建的变换对未过滤的输入点云进行变换
+    return ndt.getFinalTransformation();
+}
+
+bool rotate_center_caliber::solve()
+{
+    int size=m_match_datas.size();
+
+    if(size<1)
+    {
+        return false;
+    }
+
+    Eigen::MatrixXf A;
+    A.resize(2*size,2);
+    Eigen::MatrixXf B;
+    B.resize(2*size,1);
+
+    for(int i=0;i<size;i++)
+    {
+        match_transform data=m_match_datas[i];
+        float da=-data.theta;
+        float dx=-data.dx;
+        float dy=-data.dy;
+
+        float RA=1-cos(da);
+        float RB=sin(da);
+        float RC=-sin(da);
+        float RD=1-cos(da);
+
+        A(i*2+0,0)=RA;
+        A(i*2+0,1)=RB;
+        A(i*2+1,0)=RC;
+        A(i*2+1,1)=RD;
+
+        B(i*2+0,0)=dx;
+        B(i*2+1,0)=dy;
+
+    }
+    //std::cout<<"A:"<<A<<std::endl;
+    Eigen::MatrixXf result= (A.transpose()*A).inverse()*(A.transpose())*B;
+    //std::cout<<"result M:"<<result.transpose()<<std::endl;
+
+    m_rotation_center_x=result(0,0)+m_center.x;
+    m_rotation_center_y=result(1,0)+m_center.y;
+    return true;
+
+}
+void rotate_center_caliber::updata_points()
+{
+    if(m_match_datas.size()<1)
+        return ;
+    static int id = 0;
+    if(id==0)
+    {
+        viewer.addLine(pcl::PointXYZ(m_center.x,m_center.y,0),
+                pcl::PointXYZ(m_rotation_center_x,m_rotation_center_y,0),0,1,1,"org");
+    }
+    double max_d=-1,min_d=1e9;
+    for (int i = 0; i < m_match_datas.size(); ++i)
+    {
+
+        float x = -m_match_datas[i].dx + m_center.x;
+        float y = -m_match_datas[i].dy + m_center.y;
+
+        char buf[32] = {0};
+        sprintf(buf, "line_%d", i);
+        mut.lock();
+        if (i < id)
+        {
+            viewer.removeShape(buf);
+        }
+
+        viewer.addLine(pcl::PointXYZ(x, y, 0),
+                       pcl::PointXYZ(m_rotation_center_x, m_rotation_center_y, 0),
+                       0,
+                       0,
+                       1,
+                       buf);
+
+        mut.unlock();
+
+        double distance=sqrt(pow(m_rotation_center_x-x,2)+pow(m_rotation_center_y-y,2));
+        if(distance<min_d)
+            min_d=distance;
+        if(distance>max_d)
+            max_d=distance;
+
+    }
+    id = m_match_datas.size();
+    if((max_d-min_d)>0.01)
+    {
+        LOG(ERROR)<<" distances verify failed ";
+        return ;
+    }
+    LOG(INFO)<<" distances verify :"<<max_d-min_d;
+}

verify/rotate_center_caliber.h

@@ -0,0 +1,49 @@
+//
+// Created by zx on 2021/8/27.
+//
+
+#ifndef SHUTTER_VERIFY_VERIFY_ROTATE_CANTER_CALIBER_H_
+#define SHUTTER_VERIFY_VERIFY_ROTATE_CANTER_CALIBER_H_
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include "match3d/detect_ceres3d.h"
+#include <vector>
+class rotate_center_caliber
+{
+    typedef struct
+    {
+        float theta;
+        float dx;
+        float dy;
+    }match_transform;
+
+ public:
+    rotate_center_caliber();
+    ~rotate_center_caliber();
+    void set_first_frame(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model);
+
+    bool push_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
+
+
+    static Eigen::Matrix4f match(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_center,pcl::PointCloud<pcl::PointXYZ>::Ptr cloud
+            ,Eigen::Matrix<float, 6,1> init_transform);
+
+ protected:
+    bool solve();
+    void updata_points();
+
+ protected:
+    pcl::PointCloud<pcl::PointXYZ>::Ptr             m_model_cloud;
+    std::vector<match_transform>                    m_match_datas;
+
+    detect_ceres3d*                                 mp_matcher;
+
+    //待解算的参数， 原点中心坐标和旋转中心坐标
+    pcl::PointXYZ                                   m_center;
+    float       m_rotation_center_x;
+    float       m_rotation_center_y;
+};
+
+
+#endif //SHUTTER_VERIFY_VERIFY_ROTATE_CANTER_CALIBER_H_