Terminal_process_CT/laser/livox_driver.cpp

/*
 * livox_driver 大疆雷达的底层驱动
 *
 * 本模块作为单例模式,全局使用.
 * 负责通过调用livox sdk,与livox的后台进行进行对接. 直接控制大疆的雷达设备.
 *

 */

#include "livox_driver.h"

Livox_driver::Livox_driver()
{
	m_init_flag = false;
}

Livox_driver::~Livox_driver()
{
//	因为退出顺序的原因,析构不再自动调用 Livox_driver_uninit ,
//	而是在推出前手动调用Livox_driver_uninit
	Livox_driver_uninit();
}

//livox雷达驱动初始化.
Error_manager Livox_driver::Livox_driver_init(bool aoto_create_livox_flag)
{
	LOG(INFO) << " ---Livox_driver_init run --- "<< this;
	if (m_init_flag == false)
	{
		//Init是livox_sdk.h里面的初始化函数, 此时会创建livox后台线程
		if (!Init()) {
			Uninit();
			LOG(ERROR) << "livox sdk init failed...";
			return Error_manager(LIVOX_SKD_INIT_FAILED,MINOR_ERROR,"Livox laser init failed...");
		}
		else
		{
			//显示 livox sdk 版本
			LivoxSdkVersion _sdkversion;
			GetLivoxSdkVersion(&_sdkversion);
			char buf[255] = { 0 };
			sprintf(buf, "livox sdk init success. \n Livox SDK version %d.%d.%d .\n", _sdkversion.major, _sdkversion.minor, _sdkversion.patch);
			LOG(INFO) << buf;

			m_aoto_create_livox_flag = aoto_create_livox_flag;

			//SetBroadcastCallback 设置广播的回调函数,此时会向局域网发送广播消息.
			// 收到广播的所有雷达设备都会调用一次livox_device_broadcast_callback,
			//livox_device_broadcast_callback 函数由livox后台线程来执行.
			SetBroadcastCallback(livox_device_broadcast_callback);

			//SetDeviceStateUpdateCallback 设置状态更新的回调函数.
			//雷达设备建立连接或者断开连接,都会调用 livox_device_change_callback
			//livox_device_change_callback 函数由livox后台线程来执行.
			SetDeviceStateUpdateCallback(livox_device_change_callback);

			//启动设备底层的扫描线程，该线程在单独在后台运行。(该线程由livox_sdk来维护)
			//只有在start之后.各种回调函数才能生效.
			if (!Start())
			{
				m_init_flag = false;
				Uninit();
				LOG(ERROR) << "livox sdk start failed...";
				return Error_manager(LIVOX_SKD_INIT_FAILED,MINOR_ERROR,"Livox laser Start failed...");
			}
			else
			{
				LOG(ERROR) << "livox sdk start success...";
				m_init_flag = true;

			}

		}
	}


	std::cout << " huli test :::: " << " m_sn_laser_map.size() = " << m_sn_laser_map.size() << std::endl;
	std::cout << " huli test :::: " << " m_handle_laser_map.size() = " << m_handle_laser_map.size() << std::endl;

	return Error_code::SUCCESS;
}

//livox雷达驱动反初始化.
Error_manager Livox_driver::Livox_driver_uninit()
{
	if ( m_init_flag )
	{
		LOG(INFO) << " ---Livox_driver::Livox_driver_uninit() run --- "<< this;

		//回收livox_sdk的相关资源.销毁livox_sdk的后台线程
		Uninit();
		m_init_flag = false;
	}

	return Error_code::SUCCESS;
}

//插入广播码和雷达实例的映射表
Error_manager Livox_driver::Livox_insert_sn_laser(std::string sn, CLivoxLaser* p_livox_laser)
{
	//雷达的sn码一般为15个字符加一个\0
	if ( sn.size() != kBroadcastCodeSize-1 )
	{
		return Error_manager(Error_code::LIVOX_DRIVER_SN_ERROR, Error_level::MINOR_ERROR,
							 " Livox_insert_sn_laser error , sn size != 15 ");
	}
	//填充雷达设备广播码和雷达实例的映射表.如果重复,直接报错,决不能重复创建雷达.
	if ( m_sn_laser_map.find(sn) == m_sn_laser_map.end() )
	{
		//没找到就新增一个sn--laser映射
		m_sn_laser_map[sn] = p_livox_laser;
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::LIVOX_DRIVER_SN_REPEAT, Error_level::MINOR_ERROR,
							 " Livox_insert_sn_laser error , sn repeat  ");
	}
	return Error_code::SUCCESS;
}

//判断 Livox_driver 模块是否正常, 只有正常之后,才能启动或者停止扫描.
bool Livox_driver::is_ready()
{
	return  m_init_flag;
}


//判断是否自动创建雷达,默认为false
bool Livox_driver::is_aoto_create_livox()
{
	return m_aoto_create_livox_flag;
}

//雷达驱动层开始取样.
Error_manager Livox_driver::Livox_driver_start_sample(uint8_t handle)
{
	if ( is_ready() )
	{
		LidarStartSampling(handle, livox_start_sample_callback, NULL);
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::LIVOX_DRIVER_NOT_READY, Error_level::MINOR_ERROR,
							 " Livox_driver is_ready error ");
	}
	return Error_code::SUCCESS;
}
//雷达驱动层停止取样.
Error_manager Livox_driver::Livox_driver_stop_sample(uint8_t handle)
{
	if ( is_ready() )
	{
		LidarStopSampling(handle, livox_stop_sample_callback, NULL);
		return Error_code::SUCCESS;
	}
	else
	{
		return Error_manager(Error_code::LIVOX_DRIVER_NOT_READY, Error_level::MINOR_ERROR,
							 " Livox_driver is_ready error ");
	}
	return Error_code::SUCCESS;
}








//设备广播的回调函数,在 Livox_driver_init 时使用 SetBroadcastCallback() 进行配置.
//局域网内的所有livox雷达在收到广播之后,都会调用本函数.
//本函数由livox后台线程来执行. 每个雷达调用一次.
void Livox_driver::livox_device_broadcast_callback(const BroadcastDeviceInfo *p_info)
{
	LOG(INFO) << " ---livox_device_broadcast_callback start--- " ;
	if (p_info == NULL) {
		return;
	}

	//检查设备类型,后续最好确定具体的型号.
	if ( p_info->dev_type != kDeviceTypeHub )
	{

		//自动创建雷达实例,就是不从外部参数创建,在驱动层链接设备的时候,按照能够连接上的设备自动创建雷达实例.默认为false
		if ( Livox_driver::get_instance_references().is_aoto_create_livox() )
		{
			//创建雷达啊实例.
			//为 laser_manager 的 m_laser_vector 分配内存.
			//以后再写.
			;
		}
		else//默认从外部导入参数来创建雷达. 之前在CLivoxLaser初始化是,就已经填充了 m_sn_laser_map
		{
			//按照输入参数来创建雷达.
			//校验sn码,将雷达与设备链接起来,注意了,要先校验,再连接
			std::string t_sn = p_info->broadcast_code;
			std::map<std::string, CLivoxLaser*> & t_sn_laser_map = Livox_driver::get_instance_references().m_sn_laser_map;
			if ( t_sn_laser_map.find(t_sn) != t_sn_laser_map.end() )
			{
				//找到sn了,就连接laser和设备.
				CLivoxLaser* tp_livox_laser = t_sn_laser_map[t_sn];
				uint8_t t_handle = 0;
				//连接雷达设备,连接成功之后,后台进行会分配一个新的handle,默认0到31
				bool result = AddLidarToConnect(p_info->broadcast_code, &t_handle);

				if (result == kStatusSuccess)
				{
					//SetDataCallback 函数会为每个雷达设置数据回调函数,
					//注意了,livox的后台线程不会立刻调用livox_data_callback,
					//只有在雷达使用 LidarStartSampling 启动扫描后,当有数据返回时,才会调用livox_data_callback 传回雷达数据.
					SetDataCallback(t_handle, livox_data_callback, NULL);

					//此时只是单方面连接,状态仍然是 K_DEVICE_STATE_DISCONNECT
					//需要在 livox_device_change_callback 雷达设备响应连接之后,并确认连接的结果,才会修改雷达连接状态.
					tp_livox_laser->set_handle(t_handle);
					tp_livox_laser->set_device_state( CLivoxLaser::K_DEVICE_STATE_DISCONNECT);
					//填充雷达句柄和雷达实例的映射表. handle是livox后台线程分配的唯一码.一般不会出错.
					Livox_driver::get_instance_references().m_handle_laser_map[t_handle] = tp_livox_laser;
				}
				else
				{
					//连接失败.    (一般雷达设备有应答,就会连接成功.)
					tp_livox_laser->set_handle(-1);
					tp_livox_laser->set_device_state( CLivoxLaser::K_DEVICE_STATE_FAULT);
				}
			}
			else
			{

			}
		}
	}
}

//雷达设备状态改变的回调函数,在 Livox_driver_init 时使用 SetDeviceStateUpdateCallback() 来进行配置
//雷达设备建立连接或者断开连接,都会调用 livox_device_change_callback 来通知我们雷达状态的改变.
//本函数由livox后台线程来执行. 每个雷达可能调用多次.
void Livox_driver::livox_device_change_callback(const DeviceInfo *p_info, DeviceEvent type)
{
	LOG(INFO) << " ---livox_device_change_callback start--- " ;

	if (p_info == NULL) {
		return;
	}

	uint8_t t_handle = p_info->handle;

	//检查handle的有效性
	std::map<uint8_t, CLivoxLaser*> & t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
	if ( t_handle_laser_map.find(t_handle) == t_handle_laser_map.end() )
	{
		return;
	}
	else
	{
		CLivoxLaser* tp_livox_laser = t_handle_laser_map[t_handle];
		//正常情况下,每个雷达连接都会调用两次,分别为 kEventConnect 和 kEventStateChange
		switch ( type )
		{
			case kEventConnect:
				//连接成功,    则从断连切换到连接
				if ( tp_livox_laser->get_device_state() == CLivoxLaser::K_DEVICE_STATE_DISCONNECT )
				{
					tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
					tp_livox_laser->set_device_info(*p_info);
					//LidarGetExtrinsicParameter 函数会为每个雷达设置 获取欧拉角的回调函数,
					//获取欧拉角, 并转化为变换矩阵 mp_laser_matrix
					LidarGetExtrinsicParameter(t_handle, livox_get_extrinsic_parameter_callback,NULL);
				}
				break;
			case kEventDisconnect:
				//断开连接,		如果不是故障,则切换到断连
				if ( tp_livox_laser->get_device_state() != CLivoxLaser::K_DEVICE_STATE_FAULT )
				{
					tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_DISCONNECT);
					tp_livox_laser->set_device_info(*p_info);
				}
				break;
			case kEventStateChange:
				//单纯的刷新雷达设备信息
				tp_livox_laser->set_device_info(*p_info);
				break;
			default:

				break;
		}
	}

	//注意了:此时并不启动雷达扫描.
	//只有当接受到扫描任务之后,由  CLivoxLaser::start_scan() 来调用 LidarStartSampling
}

//获取欧拉角(笛卡尔坐标)的回调函数。在 livox_device_change_callback 连接成功之后使用 LidarGetExtrinsicParameter 来设置回调函数。
//雷达接受到查询指令后,会调用 livox_get_extrinsic_parameter_callback 来返回 LidarGetExtrinsicParameterResponse 的指针. 里面就有 欧拉角(笛卡尔坐标)
//本函数由livox后台线程来执行. 每个雷达只需要调用一次.
void Livox_driver::livox_get_extrinsic_parameter_callback(livox_status status,
														  uint8_t handle,
														  LidarGetExtrinsicParameterResponse *response,
														  void *client_data)
{
	LOG(INFO) << " ---livox_get_extrinsic_parameter_callback start--- " ;

	if ( status == kStatusSuccess )
	{
		//	校验handle的有效性
		std::map<uint8_t, CLivoxLaser*> & t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
		if ( t_handle_laser_map.find(handle) == t_handle_laser_map.end() )
		{
			return;
		}
		else
		{
			CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];

			double t_pitch = response->pitch;
			double t_roll = response->roll;
			double t_yaw = response->yaw;
			double t_x = response->x;
			double t_y = response->y;
			double t_z = response->z;

			//欧拉角转化为3*4矩阵
			t_roll = t_roll * M_PI / 180.0;
			t_pitch = t_pitch * M_PI / 180.0;
			t_yaw = t_yaw * M_PI / 180.0;
			double rotate[12] = {
			std::cos(t_pitch) * std::cos(t_yaw),
			std::sin(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) - std::cos(t_roll) * std::sin(t_yaw),
			std::cos(t_roll) * std::sin(t_pitch) * std::cos(t_yaw) + std::sin(t_roll) * std::sin(t_yaw),
			t_x,
			std::cos(t_pitch) * std::sin(t_yaw),
			std::sin(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) + std::cos(t_roll) * std::cos(t_yaw),
			std::cos(t_roll) * std::sin(t_pitch) * std::sin(t_yaw) - std::sin(t_roll) * std::cos(t_yaw),
			t_y,
			-std::sin(t_pitch),
			std::sin(t_roll) * std::cos(t_pitch),
			std::cos(t_roll) * std::cos(t_pitch),
			t_z
			};
			//设置变换矩阵
			tp_livox_laser->set_laser_matrix(rotate, 12);
		}
	}

}

//雷达设备数据返回 的回调函数. 在 livox_device_broadcast_callback 连接成功之后使用 SetDataCallback  来设置回调函数。
//雷达在调用 LidarStartSampling 开始扫描之后,就一直调用 livox_data_callback 来返回数据.
//雷达在调用 LidarStopSampling 停止扫描之后,就停止扫描.本函数也会停止调用.
//本函数由livox后台线程来执行. 每帧数据都会调用一次. data里面有多个三维点.
void Livox_driver::livox_data_callback(uint8_t handle, LivoxEthPacket *data, uint32_t data_num, void *laser)
{
	//livox_data_callback函数会高频率的调用,为了提高2效率,这里就不验证handle的有效性了,这就需要保证前面的正确操作
	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(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)
		{
			//判断是否采集完成
			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);

			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(livox_status status, uint8_t handle, uint8_t response, void *data)
{
	LOG(INFO) << " ---livox_start_sample_callback start--- " ;

	//	校验handle的有效性
	std::map<uint8_t, CLivoxLaser *> &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
	if (t_handle_laser_map.find(handle) == t_handle_laser_map.end())
	{
		return;
	}
	else
	{
		CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];

		if (status == kStatusSuccess)
		{
			if (response != 0)
			{
				tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
			}
			else
			{
				//返回成功并且应答没有报错,这里将雷达设备状态改为扫描中.
				tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_SAMPLING);
			}
		}
		else if (status == kStatusTimeout)
		{
			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
		}
		else
		{
			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_FAULT);
		}
	}
}

//雷达设备启动扫描的回调函数, 在 CLivoxLaser::stop_scan() 或者其他位置 需要停止的时候使用 LidarStopSampling 来设置回调函数。
//调用 LidarStopSampling 之后,livox后台进程就直接停止扫描了. 也会停止调用 livox_data_callback
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--- " ;

//	校验handle的有效性
	std::map<uint8_t, CLivoxLaser *> &t_handle_laser_map = Livox_driver::get_instance_references().m_handle_laser_map;
	if (t_handle_laser_map.find(handle) == t_handle_laser_map.end())
	{
		return;
	}
	else
	{
		CLivoxLaser *tp_livox_laser = t_handle_laser_map[handle];

		if (status == kStatusSuccess || status == kStatusTimeout)
		{
			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_CONNECT);
		}
		else
		{
			tp_livox_laser->set_device_state(CLivoxLaser::K_DEVICE_STATE_FAULT);
		}
	}
}