LidarMeasure/laser/LivoxLaser.cpp

#include "LivoxLaser.h"
#include "../common.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 };

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];
	}

}


bool CLivoxLaser::IsScanComplete()
{
	return g_count[m_handle] >= m_frame_maxnum;
}

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;
		}
	}
}

CLivoxLaser::CLivoxLaser(int id, Automatic::stLaserCalibParam laser_param)
	:CLaser(id, laser_param)
{
	
	m_frame_maxnum = laser_param.frame_num();
	if(laser_param.type()=="Livox")
		g_sn_laser.insert(std::make_pair(laser_param.sn(), this));

	InitLivox();
}




CLivoxLaser::~CLivoxLaser()
{
}

void CLivoxLaser::OnSampleCallback(uint8_t status, uint8_t handle, uint8_t response, void *data) {
	CLivoxLaser* laser = (CLivoxLaser*)data;

	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)
{
	///?????????????? ?????????handle
	CLivoxLaser* livox = g_all_laser[handle];
	if (livox == 0)
	{
		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();
			}
		}
	}

	
	if (data && livox)
	{
		if (livox->m_bStart_capture.WaitFor(1))
		{
			if (livox->IsScanComplete())
			{
				livox->Stop();
				return;
			}
			if (g_count[handle] >= livox->m_frame_maxnum)
				return;
			uint64_t cur_timestamp = *((uint64_t *)(data->timestamp));
			LivoxRawPoint *p_point_data = (LivoxRawPoint *)data->data;
			
			CBinaryData* data_bin = new CBinaryData((char*)p_point_data, data_num * sizeof(LivoxRawPoint));
			livox->m_queue_livox_data.push(data_bin);
			g_count[handle]++;
			
		}
        else if(livox->m_statu!=eLaser_ready)
		{
			//?????????????????
			livox->m_statu = eLaser_ready;
            usleep(10*1000);
		}
	}
	
}

eLaserStatu CLivoxLaser::GetStatu()
{
    if(g_devices[m_handle].device_state == kDeviceStateConnect||
            g_devices[m_handle].device_state == kDeviceStateSampling)
    {
        m_statu=eLaser_ready;
    }
    if(g_devices[m_handle].device_state == kDeviceStateDisconnect)
    {
        m_statu=eLaser_disconnect;
    }
    return m_statu;
}

bool CLivoxLaser::Connect()
{
	return CLaser::Connect();
}
void CLivoxLaser::Disconnect() 
{
	CLaser::Disconnect();
}
bool CLivoxLaser::Start()
{
	LOG(INFO) << " livox start :"<<this; 
	//???????????
	m_queue_livox_data.clear();
	g_count[m_handle] = 0;
	return CLaser::Start();
}
bool CLivoxLaser::Stop()
{
	return CLaser::Stop();
}

bool CLivoxLaser::RecvData(CBinaryData& data)
{
	CBinaryData* livox_data;
	if (m_queue_livox_data.try_pop(livox_data))
	{
		data = *livox_data;
		delete livox_data;
		return true;
	}
	return false;
}
DATA_type CLivoxLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
{
	LivoxRawPoint *p_point_data = (LivoxRawPoint *)pData->Data();
	int count = pData->Length() / (sizeof(LivoxRawPoint));
	
	if (count <= 0)
		return eUnknow;
	for (int i = 0; i < count; ++i)
	{
		LivoxRawPoint point = p_point_data[i];
		CPoint3D pt;
		pt.x = point.x;
		pt.y = point.y;
		pt.z = point.z;
		points.push_back(pt);
	}
	return eData;
}