LidarMeasure/laser/Laser.cpp

#include "Laser.h"

CBinaryData::CBinaryData()
	:m_buf(0)
	,m_length(0)
{
}
CBinaryData::CBinaryData(const char* buf, int len, DATA_type type)
    :m_buf(0),m_length(0)
{
	if (len > 0)
	{
		m_buf = (char*)malloc(len);
		memcpy(m_buf, buf, len);
		m_length = len;
	}
}
CBinaryData::CBinaryData(const CBinaryData& data)
    :m_buf(0),m_length(0)
{
	if (m_buf)
	{
		free(m_buf);
		m_buf = 0;
		m_length = 0;
	}
	if (data.Length() > 0)
	{
		m_buf = (char*)malloc(data.Length());
		memcpy(m_buf, data.Data(), data.Length());
		m_length = data.Length();
	}
}
CBinaryData::~CBinaryData()
{
    if (m_buf && m_length)
	{
		free(m_buf);
		m_length = 0;
	}
}
char* CBinaryData::Data()const {
	return m_buf;
}
int CBinaryData::Length()const {
	return m_length;
}
CBinaryData& CBinaryData::operator=(const CBinaryData& data)
{
	if (m_buf)
	{
		free(m_buf);
		m_buf = 0;
		m_length = 0;
	}
	if (data.Length() > 0)
	{
		m_buf = (char*)malloc(data.Length());
		memcpy(m_buf, data.Data(), data.Length());
		m_length = data.Length();
	}
	return *this;
}
bool CBinaryData::operator==(const char* str)
{
	int length = strlen(str);
	return  (strncmp((const char*)m_buf, str, length) == 0);
}
const char* CBinaryData::operator+(int n)
{
	if (n < m_length)
		return m_buf + n;
	else
		return NULL;
}
CBinaryData& CBinaryData::operator+(CBinaryData& data)
{
	if (data.Length() > 0)
	{
		int length = m_length + data.Length();
		char* buf = (char*)malloc(length);
		memcpy(buf, m_buf, m_length);
		memcpy(buf + m_length, data.Data(), data.Length());
		free(m_buf);
		m_buf = buf;
		m_length = length;
	}
	return *this;
}
char& CBinaryData::operator[](int n)
{
	if (n >= 0 && n < m_length)
		return m_buf[n];
}
//////////////

CLaser::CLaser(int id, Automatic::stLaserCalibParam laser_param)
	:m_ThreadRcv(0)
	,m_ThreadPro(0)
	,m_ThreadPub(0)
	,m_id(id)
    ,m_points_count(0)
	,m_statu(eLaser_disconnect)
	,m_dMatrix(0)
	, m_point_callback_fnc(0)
	, m_point_callback_pointer(0)
	,m_laser_param(laser_param)
	,m_bSave_file(false)
	,m_bscan_start(false)
{
    mp_laser_task = NULL;
}


CLaser::~CLaser()
{
	if (m_dMatrix)
	{
		free(m_dMatrix);
		m_dMatrix = 0;
	}
}

bool CLaser::Start()
{
    m_bscan_start=true;
	m_statu = eLaser_busy;
	m_bStart_capture.Notify(true);
    m_points_count=0;
	return true;
}

bool CLaser::Stop()
{
    m_bscan_start=false;
	m_bStart_capture.Notify(false);
	m_binary_log_tool.close();
	m_pts_log_tool.close();
	m_statu=eLaser_ready;

	//在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
	if(mp_laser_task !=NULL)
    {
	    if(mp_laser_task->get_statu() == TASK_WORKING)
        {
            mp_laser_task->set_task_statu(TASK_OVER);
        }
    }

	return true;
}

void CLaser::SetMetrix(double* data)
{
	if (m_dMatrix == 0)
	{
		m_dMatrix = (double*)malloc(12 * sizeof(double));
	}
	memcpy(m_dMatrix, data, 12 * sizeof(double));
}
void CLaser::SetPointCallBack(PointCallBack fnc, void* pointer)
{
	m_point_callback_fnc = fnc;
	m_point_callback_pointer = pointer;
}

bool CLaser::Connect()
{
	m_bThreadRcvRun.Notify(true);
	if(m_ThreadRcv==0)
		m_ThreadRcv = new std::thread(&CLaser::thread_recv, this);
	m_ThreadRcv->detach();

	m_bThreadProRun.Notify(true);
	if(m_ThreadPro==0)
		m_ThreadPro = new std::thread(&CLaser::thread_toXYZ, this);
	m_ThreadPro->detach();

    if(m_ThreadPub==0)
        m_ThreadPub = new std::thread(threadPublish, this);
    m_ThreadPub->detach();

	return true;
}
void CLaser::Disconnect()
{
	if (m_ThreadRcv)
	{
		delete m_ThreadRcv;
		m_ThreadRcv = 0;
	}

    if (m_ThreadPub)
    {
        delete m_ThreadPub;
        m_ThreadPub = 0;
    }

	if (m_ThreadPro)
	{
		delete m_ThreadPro;
		m_ThreadPro = NULL;
	}
	m_statu = eLaser_disconnect;
}


//对外的接口函数，负责接受并处理任务单，
//input：laser_task 雷达任务单，必须是子类，并且任务正确。（传引用）
Error_manager CLaser::porform_task(Laser_task* p_laser_task)
{
    Error_manager t_error_manager;
    //检查指针
    if(p_laser_task == NULL)
    {
        return Error_manager(Error_code::POINTER_IS_NULL, Error_level::MINOR_ERROR,
                             "CLaser::porform_task failed, POINTER_IS_NULL");
    }
    else
    {
        //接受任务，并将任务的状态改为TASK_SIGNED已签收
        mp_laser_task = p_laser_task;
        mp_laser_task->set_task_statu(TASK_SIGNED);
    }

    //检查消息内容是否正确，
    //检查点云指针
    if(p_laser_task->get_task_point3d_cloud_vector() == NULL)
    {
        t_error_manager.error_manager_reset(Error_code::POINTER_IS_NULL,
                                            Error_level::MINOR_ERROR,
                                            "CLaser::porform_task failed, POINTER_IS_NULL");
    }
        //检查任务类型
    else if(p_laser_task->get_task_type() != LASER_TASK)
    {
        t_error_manager.error_manager_reset(Error_code::LASER_TASK_PARAMETER_ERROR,
                                            Error_level::MINOR_ERROR,
                                            "CLaser::porform_task failed, task_type is error");
    }
    else
    {
        //将任务的状态改为 TASK_WORKING 处理中
        mp_laser_task->set_task_statu(TASK_WORKING);

        //启动雷达扫描
        Start();
    }

    //返回错误码
    if(t_error_manager != Error_code::SUCCESS)
    {
        mp_laser_task->set_task_error_manager(t_error_manager);
    }
    return t_error_manager;
}


void CLaser::SetSaveDir(std::string strDir,bool bSaveFile)
{
	m_bSave_file = bSaveFile;
	m_pts_log_tool.close();
	m_binary_log_tool.close();

	if (bSaveFile == false)
		return;

	m_pts_save_path = strDir;
	char pts_file[255] = { 0 };
	sprintf(pts_file, "%s/pts%d.txt", strDir.c_str(), m_id+1);
	m_pts_log_tool.open(pts_file);

	char bin_file[255] = { 0 };
	sprintf(bin_file, "%s/laser%d.data", strDir.c_str(), m_id+1);
	m_binary_log_tool.open(bin_file,true);
}

void CLaser::thread_recv()
{
	while (m_bThreadRcvRun.WaitFor(1))
	{
		if (m_bStart_capture.WaitFor(1) == false)
			continue;
		CBinaryData* data=new CBinaryData();
		if (this->RecvData(*data))
		{
			m_queue_laser_data.push(data);
		}
		else
		{
			delete data;
            data=0;
		}
		
	}
}

CPoint3D CLaser::transfor(CPoint3D point)
{
	if (m_dMatrix)
	{
		CPoint3D result;
		double x = point.x;
		double y = point.y;
		double z = point.z;
		result.x = x * m_dMatrix[0] + y*m_dMatrix[1] + z*m_dMatrix[2] + m_dMatrix[3];
		result.y = x * m_dMatrix[4] + y*m_dMatrix[5] + z*m_dMatrix[6] + m_dMatrix[7];
		result.z = x * m_dMatrix[8] + y*m_dMatrix[9] + z*m_dMatrix[10] + m_dMatrix[11];
		return result;
	}
	else
	{
		return point;
	}
}

void CLaser::thread_toXYZ()
{
	while (m_bThreadProRun.WaitFor(1))
	{
		if (m_bStart_capture.WaitFor(1) == false)
		{
			m_queue_laser_data.clear();
			continue;
		}
        CBinaryData* pData=NULL;
        bool bPop = m_queue_laser_data.try_pop(pData);
        DATA_type type = eUnknow;
        if (bPop || m_last_data.Length() != 0)
        {
            std::vector<CPoint3D> cloud;
            if (bPop)
            {
                if (pData == NULL)
                    continue;
                m_binary_log_tool.write(pData->Data(), pData->Length());
                type= this->Data2PointXYZ(pData, cloud);
            }
            else
            {
                type = this->Data2PointXYZ(NULL, cloud);
            }
            if (type == eUnknow)
            {
                delete pData;
                continue;
            }

            if (type == eData || type == eStart || type == eStop)
            {
                for (int i = 0; i < cloud.size(); ++i)
                {
                    CPoint3D point = transfor(cloud[i]);
                    if (m_point_callback_fnc)
                        m_point_callback_fnc(point, m_point_callback_pointer);
                    if(m_bSave_file) {
                        char buf[64] = {0};
                        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
                        m_pts_log_tool.write(buf, strlen(buf));
                    }

                    //此时cloud为雷达采集数据的结果，转换后的三维点云。
                    //将每个点存入任务单里面的点云容器。
                    mp_laser_task->get_task_point3d_cloud_vector()->push_back(point);
                }
                m_points_count+=cloud.size();
                if (type == eStop)
                {
                    if(m_bSave_file) {
                        m_pts_log_tool.close();
                        m_binary_log_tool.close();
                    }
                    m_statu = eLaser_ready;

                    //在停止雷达时，将雷达任务状态改为 TASK_OVER 已结束
                    if(mp_laser_task !=NULL)
                    {
                        if(mp_laser_task->get_statu() == TASK_WORKING)
                        {
                            mp_laser_task->set_task_statu(TASK_OVER);
                        }
                    }
                }

            }
            else if (type == eReady)
            {
                if(m_bSave_file) {
                    m_pts_log_tool.close();
                    m_binary_log_tool.close();
                }
                m_statu = eLaser_ready;
            }


            delete pData;
        }
		
	}
}


#include "unistd.h"
void CLaser::threadPublish(CLaser *laser)
{
    if(laser==0) return ;
    while(laser->m_bThreadRcvRun.WaitFor(1))
    {
        laser->PublishMsg();
        usleep(1000*300);
    }
}

#include "../common.h"
void CLaser::PublishMsg()
{
    globalmsg::msg msg;
    msg.set_msg_type(globalmsg::eLaser);

    globalmsg::laserStatus status;
    if(GetStatu()==eLaser_ready) status=globalmsg::eLaserConnected;
    else if(GetStatu()==eLaser_disconnect) status=globalmsg::eLaserDisconnected;
    else if(GetStatu()==eLaser_busy) status=globalmsg::eLaserBusy;
    else  status=globalmsg::eLaserUnknown;

    msg.mutable_laser_msg()->set_id(m_id);
    msg.mutable_laser_msg()->set_laser_status(status);
    msg.mutable_laser_msg()->set_queue_data_count(m_queue_laser_data.size());
    msg.mutable_laser_msg()->set_cloud_count(m_points_count);
    MeasureTopicPublisher::GetInstance()->Publish(msg.SerializeAsString());
}