20210510, 兼容Horizon新雷达

laser/LivoxHorizon.cpp

@@ -0,0 +1,100 @@
+
+
+#include "LivoxHorizon.h"
+#include "livox_driver.h"
+
+RegisterLaser(Horizon)
+
+
+
+CHorizonLaser::CHorizonLaser(int id, Laser_proto::laser_parameter laser_param)
+:CLivoxLaser(id, laser_param)
+{
+	Error_manager t_error;
+	m_frame_count = 0;
+	m_frame_maxnum = laser_param.frame_num();//初始化从外部参数导入,实际运行前从任务单导入.
+
+	//默认handle为-1,切记不能初始化为0.因为0表示0号雷达,是有效的.....
+	m_livox_device.handle = 255;
+
+	//判断参数类型，
+	if(laser_param.type()=="Horizon")
+	{
+		//填充雷达设备的广播码
+		t_error = Livox_driver::get_instance_references().Livox_insert_sn_laser(laser_param.sn(), this);
+		//此时不进行驱动层的初始化,在所有雷达创建完成之后,在laser_manager统一初始化.
+		if ( t_error != SUCCESS)
+		{
+			//如果sn码不规范,或者sn码重复都会引发错误,
+			//故障处理,
+			// 方案一:回退.取消创建雷达的操作.释放雷达内存.
+			//方案二:允许创建雷达,但是状态改为故障.禁用后续的功能.
+
+			//以后再写.
+			;
+		}
+	}
+}
+
+CHorizonLaser::~CHorizonLaser()
+{
+	//清空队列
+	m_queue_livox_data.clear_and_delete();
+}
+
+//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
+// 纯虚函数，必须由子类重载，
+Error_manager CHorizonLaser::transform_buf_to_points()
+{
+	Error_manager t_error;
+	Binary_buf* tp_binaty_buf=NULL;
+	//从队列中取出缓存。 tp_binaty_buf的内存 从链表取出时，就自带内存了。需要手动释放。
+	if(m_queue_laser_data.try_pop(tp_binaty_buf))
+	{
+		//livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
+		LivoxExtendRawPoint *tp_Livox_data = (LivoxExtendRawPoint *)tp_binaty_buf->get_buf();
+		//计算这一帧数据有多少个三维点。
+		int t_count = tp_binaty_buf->get_length() / (sizeof(LivoxExtendRawPoint));
+		//转变三维点格式，并存入vector。
+		for (int i = 0; i < t_count; ++i)
+		{
+			//三维点的坐标变换，从雷达自己的坐标系，转化到公共坐标系，（目前以plc为公共坐标系）
+			CPoint3D point_source;
+			point_source.x = tp_Livox_data[i].x;
+			point_source.y = tp_Livox_data[i].y;
+			point_source.z = tp_Livox_data[i].z;
+			CPoint3D point_destination = transform_by_matrix(point_source);
+
+			//保存雷达扫描 三维点云的最终结果。
+			if(m_save_flag)
+			{
+				char buf[64] = {0};
+				sprintf(buf, "%f %f %f\n", point_destination.x, point_destination.y, point_destination.z);
+				m_points_log_tool.write(buf, strlen(buf));
+			}
+
+			//LivoxExtendRawPoint 转为 pcl::PointXYZ
+			//int32_t 转 double。不要信号强度
+			pcl::PointXYZ  pointXYZ;
+			pointXYZ.x = point_destination.x/1000.0;
+			pointXYZ.y = point_destination.y/1000.0;
+			pointXYZ.z = point_destination.z/1000.0;
+			//注:单位毫米转为米, 最终数据单位统一为米.....
+			t_error =  mp_laser_task->task_push_point(&pointXYZ);
+			if ( t_error != Error_code::SUCCESS )
+			{
+				delete tp_binaty_buf;
+				return t_error;
+			}
+		}
+		//统计扫描点个数。
+		m_points_count += t_count;
+
+		delete tp_binaty_buf;
+	}
+	else
+	{
+		return NODATA;// m_queue_laser_data 没有数据并不意味着程序就出错了,
+	}
+	return Error_code::SUCCESS;
+}

laser/LivoxHorizon.h

@@ -0,0 +1,40 @@
+#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();
+
+
+	//将二进制消息转化为三维点云的功能函数，并将其存入 任务单 , 每次只转化一个CBinaryData，
+	// 纯虚函数，必须由子类重载，
+	virtual Error_manager transform_buf_to_points();
+};
+
+
+
+
+
+
+
+
+#endif

laser/laser_manager.cpp

@@ -76,6 +76,15 @@ Error_manager Laser_manager::laser_manager_init_from_protobuf(Laser_proto::Laser
 			}
 			break;
 		}
+		if (type.find("Horizon") != type.npos)
+		{
+			t_error = Livox_driver::get_instance_references().Livox_driver_init() ;
+			if ( t_error != Error_code::SUCCESS )
+			{
+				return t_error;
+			}
+			break;
+		}
 	}
 
 

laser/livox_driver.cpp

@@ -298,7 +298,7 @@ void Livox_driver::livox_device_change_callback(const DeviceInfo *p_info, Device
 //获取欧拉角(笛卡尔坐标)的回调函数。在 livox_device_change_callback 连接成功之后使用 LidarGetExtrinsicParameter 来设置回调函数。
 //雷达接受到查询指令后,会调用 livox_get_extrinsic_parameter_callback 来返回 LidarGetExtrinsicParameterResponse 的指针. 里面就有 欧拉角(笛卡尔坐标)
 //本函数由livox后台线程来执行. 每个雷达只需要调用一次.
-void Livox_driver::livox_get_extrinsic_parameter_callback(uint8_t status,
+void Livox_driver::livox_get_extrinsic_parameter_callback(livox_status status,
 														  uint8_t handle,
 														  LidarGetExtrinsicParameterResponse *response,
 														  void *client_data)
@@ -359,35 +359,83 @@ void Livox_driver::livox_data_callback(uint8_t handle, LivoxEthPacket *data, uin
 	std::map<uint8_t, CLivoxLaser *> &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
 	CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];
 
+
 	if (data && tp_livox_laser)
 	{
-		//判断是否采集完成
-		if (tp_livox_laser->is_scan_complete())
+//		if(data ->data_type == kCartesian) {
+//			LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
+//		}else if ( data ->data_type == kSpherical) {
+//			LivoxSpherPoint *p_point_data = (LivoxSpherPoint *)data->data;
+//		}else if ( data ->data_type == kExtendCartesian) {
+//			LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
+//		}else if ( data ->data_type == kExtendSpherical) {
+//			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;
+//		}
+
+
+		if(data ->data_type == kCartesian)
 		{
-			//按照指定的帧数来采集,到点就停止.
-			tp_livox_laser->stop_scan();
-			//注注注注注意了:stop_scan会调用 LidarStopSampling 通知livox后台线程停止扫描.
-			// 但是这个是有延迟的.会导致数据回调函数 livox_data_callback 仍然会调用3~5次.
-			// 因此这里会反复调用 stop_scan
-			return;
-		}
-		//data实际就是一个数据指针,里面包含data_num个三维点. data_num默认为100个
-		LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
+			//判断是否采集完成
+			if (tp_livox_laser->is_scan_complete())
+			{
+				//按照指定的帧数来采集,到点就停止.
+				tp_livox_laser->stop_scan();
+				//注注注注注意了:stop_scan会调用 LidarStopSampling 通知livox后台线程停止扫描.
+				// 但是这个是有延迟的.会导致数据回调函数 livox_data_callback 仍然会调用3~5次.
+				// 因此这里会反复调用 stop_scan
+				return;
+			}
+			//data实际就是一个数据指针,里面包含data_num个三维点. data_num默认为100个
+			LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
 
-		//把雷达数据填充到livox数据缓存,此时里面是没有解析的原始数据.  data_num默认为100个
-		Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
-		tp_livox_laser->push_livox_data(data_bin);
+			//把雷达数据填充到livox数据缓存,此时里面是没有解析的原始数据.  data_num默认为100个
+			Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
+			tp_livox_laser->push_livox_data(data_bin);
 
-		tp_livox_laser->add_livox_frame(1);
+			tp_livox_laser->add_livox_frame(1);
+		}
+		else if ( data ->data_type == kExtendCartesian)
+		{
+			//判断是否采集完成
+			if (tp_livox_laser->is_scan_complete())
+			{
+				//按照指定的帧数来采集,到点就停止.
+				tp_livox_laser->stop_scan();
+				//注注注注注意了:stop_scan会调用 LidarStopSampling 通知livox后台线程停止扫描.
+				// 但是这个是有延迟的.会导致数据回调函数 livox_data_callback 仍然会调用3~5次.
+				// 因此这里会反复调用 stop_scan
+				return;
+			}
+			//data实际就是一个数据指针,里面包含data_num个三维点. data_num默认为100个
+			LivoxExtendRawPoint *p_point_data = (LivoxExtendRawPoint *)data->data;
 
+			//把雷达数据填充到livox数据缓存,此时里面是没有解析的原始数据.  data_num默认为100个
+			Binary_buf* data_bin = new Binary_buf((char*)p_point_data, data_num * sizeof(LivoxExtendRawPoint));
+			tp_livox_laser->push_livox_data(data_bin);
 
+			tp_livox_laser->add_livox_frame(1);
+		}
+		else
+		{
+			return;
+		}
 	}
 }
 
 
 //雷达设备启动扫描的回调函数, 在 CLivoxLaser::start_scan() 需要启动扫描的时候使用 LidarStartSampling 来设置回调函数。
 //调用 LidarStartSampling 之后,livox后台进程就直接开始扫描了. 之后就会一直调用 livox_data_callback 来返回数据
-void Livox_driver::livox_start_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data)
+void Livox_driver::livox_start_sample_callback(livox_status status, uint8_t handle, uint8_t response, void *data)
 {
 	LOG(INFO) << " ---livox_start_sample_callback start--- " ;
 
@@ -426,7 +474,7 @@ void Livox_driver::livox_start_sample_callback(uint8_t status, uint8_t handle, u
 
 //雷达设备启动扫描的回调函数, 在 CLivoxLaser::stop_scan() 或者其他位置 需要停止的时候使用 LidarStopSampling 来设置回调函数。
 //调用 LidarStopSampling 之后,livox后台进程就直接停止扫描了. 也会停止调用 livox_data_callback
-void Livox_driver::livox_stop_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data)
+void Livox_driver::livox_stop_sample_callback(livox_status status, uint8_t handle, uint8_t response, void *data)
 {
 	LOG(INFO) << " ---livox_stop_sample_callback start--- " ;
 

laser/livox_driver.h

@@ -69,7 +69,7 @@ public://API functions
 	//获取欧拉角(笛卡尔坐标)的回调函数。在 livox_device_change_callback 连接成功之后使用 LidarGetExtrinsicParameter 来设置回调函数。
 	//雷达接受到查询指令后,会调用 livox_get_extrinsic_parameter_callback 来返回 LidarGetExtrinsicParameterResponse 的指针. 里面就有 欧拉角(笛卡尔坐标)
 	//本函数由livox后台线程来执行. 每个雷达只需要调用一次.
-	static void livox_get_extrinsic_parameter_callback(uint8_t status,
+	static void livox_get_extrinsic_parameter_callback(livox_status status,
 													   uint8_t handle,
 													   LidarGetExtrinsicParameterResponse *response,
 													   void *client_data);
@@ -82,11 +82,11 @@ public://API functions
 
 	//雷达设备启动扫描的回调函数, 在 CLivoxLaser::start_scan() 需要启动扫描的时候使用 LidarStartSampling 来设置回调函数。
 	//调用 LidarStartSampling 之后,livox后台进程就直接开始扫描了. 之后就会一直调用 livox_data_callback 来返回数据
-	static void livox_start_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data);
+	static void livox_start_sample_callback(livox_status status, uint8_t handle, uint8_t response, void *data);
 
 	//雷达设备启动扫描的回调函数, 在 CLivoxLaser::stop_scan() 或者其他位置 需要停止的时候使用 LidarStopSampling 来设置回调函数。
 	//调用 LidarStopSampling 之后,livox后台进程就直接停止扫描了. 也会停止调用 livox_data_callback
-	static void livox_stop_sample_callback(uint8_t status, uint8_t handle, uint8_t response, void *data);
+	static void livox_stop_sample_callback(livox_status status, uint8_t handle, uint8_t response, void *data);
 
 
 public://get or set member variable

main.cpp

@@ -70,7 +70,7 @@ int main(int argc,char* argv[])
 
 
 
-
+/*
 	Wanji_manager::get_instance_references().wanji_manager_init();
 	std::cout << " huli test :::: " << " wanji_manager_init = " << 1 << std::endl;
 
@@ -135,6 +135,7 @@ int main(int argc,char* argv[])
 	std::cout << " huli test :::: " << " uninit = " << 999999 << std::endl;
 	return 0;
 
+*/
 
 
 
@@ -147,10 +148,9 @@ int main(int argc,char* argv[])
 
 
 
+/*
 
 
-
-/*
 	int t_terminal_id = 0;
 	if ( argc == 2 )
 	{
@@ -343,8 +343,8 @@ int main(int argc,char* argv[])
 			std::vector<int> select_laser_id_vector;
 			select_laser_id_vector.push_back(0);
 			select_laser_id_vector.push_back(1);
-			select_laser_id_vector.push_back(2);
-			select_laser_id_vector.push_back(3);
+//			select_laser_id_vector.push_back(2);
+//			select_laser_id_vector.push_back(3);
 
 
 			time_t nowTime;
@@ -418,6 +418,7 @@ int main(int argc,char* argv[])
 			std::cout << t_error.to_string() << std::endl;
 			*/
 
+/*
 			Locate_manager_task * locate_manager_task = new Locate_manager_task;
 			locate_manager_task->task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(15000),
 										   true,t_save_path,&cloud_lock,&point_cloud_map, false	);
@@ -464,7 +465,7 @@ int main(int argc,char* argv[])
 			}
 			delete(locate_manager_task);
 			std::cout << "huli locate result:::::" << t_error.to_string() << std::endl;
-
+*/
 		}
 
 		cout << "huli: 601 :" << t_error.to_string() << endl;

setting/laser.prototxt

@@ -1,27 +1,7 @@
 #1号雷达
-#  0TFDFG700601881  0TFDFCE00502001 0TFDH5L00684HE1  0TFDH5R006RCJ91
-
-
-
-
-laser_parameters
-{
-	type:"Livox"
-	sn:"0TFDFG700601881"
-	frame_num:1000
-    mat_r00:1
-    mat_r01:0
-    mat_r02:0
-    mat_r03:0
-    mat_r10:0
-    mat_r11:1
-    mat_r12:0
-    mat_r13:0
-    mat_r20:0
-    mat_r21:0
-    mat_r22:1
-    mat_r23:0
-}
+#  Livox            0TFDFG700601881  0TFDFCE00502001 0TFDH5L00684HE1  0TFDH5R006RCJ91
+#  Horizon          1HDDH3200100691
+#
 
 
 laser_parameters
@@ -43,33 +23,10 @@ laser_parameters
     mat_r23:0
 }
 
-
-
-laser_parameters
-{
-	type:"Livox"
-	sn:"0TFDH5L00684HE1"
-	frame_num:1000
-    mat_r00:1
-    mat_r01:0
-    mat_r02:0
-    mat_r03:0
-    mat_r10:0
-    mat_r11:1
-    mat_r12:0
-    mat_r13:0
-    mat_r20:0
-    mat_r21:0
-    mat_r22:1
-    mat_r23:0
-}
-
-
-
 laser_parameters
 {
-	type:"Livox"
-	sn:"0TFDH5R006RCJ91"
+	type:"Horizon"
+	sn:"1HDDH3200100691"
 	frame_num:1000
     mat_r00:1
     mat_r01:0