#include "LivoxHorizon.h"

RegisterLaser(Horizon)



CHorizonLaser::CHorizonLaser(int id, Laser_proto::laser_parameter laser_param)
:CLivoxLaser(id, laser_param)
{

        //设备livox扫描最大帧数
        m_frame_maxnum = laser_param.frame_num();
        //判断参数类型，
        if(laser_param.type()=="Horizon")
        {
            //填充雷达设备的广播码
            g_sn_laser.insert(std::make_pair(laser_param.sn(), this));
            //初始化livox
            InitLivox();
        }


}

CHorizonLaser::~CHorizonLaser()
{

}
//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
Error_manager CHorizonLaser::start_scan()
{
    //LOG(INFO) << " Horizon start :"<<this;
    return CLivoxLaser::start_scan();
}
//将二进制消息转化为三维点云的功能函数，每次只转化一个CBinaryData，
// 纯虚函数，必须由子类重载，
Buf_type CHorizonLaser::transform_buf_to_points(Binary_buf* p_binary_buf, std::vector<CPoint3D>& point3D_cloud)
{
    static int count=0;

    if ( p_binary_buf ==NULL )
    {
        return BUF_UNKNOW;
    }
    else
    {
        //livox的二进制数据格式就是三维点坐标。直接强行转化指针类型即可
        LivoxExtendRawPoint *tp_Livox_data = (LivoxExtendRawPoint *)p_binary_buf->get_buf();
        //计算这一帧数据有多少个三维点。
        int t_count = p_binary_buf->get_length() / (sizeof(LivoxExtendRawPoint));

        if (t_count <= 0)
        {
            return BUF_UNKNOW;
        }
        else
        {
            //转变三维点格式，并存入vector。
            for (int i = 0; i < t_count; ++i)
            {
                //LivoxExtendRawPoint 转为 CPoint3D
                //int32_t 转 double。不要信号强度
                LivoxExtendRawPoint t_livox_point = tp_Livox_data[i];
                CPoint3D t_point3D;
                t_point3D.x = t_livox_point.x/1000.0;
                t_point3D.y = t_livox_point.y/1000.0;
                t_point3D.z = t_livox_point.z/1000.0;
                point3D_cloud.push_back(t_point3D);
//				std::cout << "huli points:" << t_point3D.x << " : " << t_point3D.y << " : " << t_point3D.z<< std::endl;
            }
            return BUF_DATA;
        }
    }

}