puai_wj_2021/laser/LivoxLaser.cpp

#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_BASE_SCAN_TASK)
	{
		return Error_manager(Error_code::LIVOX_TASK_TYPE_ERROR, Error_level::MINOR_ERROR,
							 "laser task type error ！= LASER_BASE_SCAN_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;
	}*/
}