shutter_verify/safety_excutor.cpp

//
// Created by zx on 2021/12/16.
//

#include "safety_excutor.h"


safety_excutor::safety_excutor()
{
    mp_lidars=nullptr;
}
safety_excutor::~safety_excutor()
{
    if(mp_lidars!= nullptr)
    {
        delete[] mp_lidars;
        mp_lidars= nullptr;
    }
}


bool safety_excutor::init(const shutter::shutter_param& parameter)
{
    if(parameter.lidars_size()==0)
        return false;
    mp_lidars=new livox::LivoxMid70[parameter.lidars_size()];
    for(int i=0;i<parameter.lidars_size();++i)
    {
        shutter::lidar_parameter ld_p=parameter.lidars(i);
        mp_lidars[i].AddBroadcastCode(parameter.lidars(i).sncode());
        mp_lidars[i].SetFPS(parameter.lidars(i).fps());
        mp_lidars[i].SetTransformParam(Eigen::Vector3f(ld_p.r(),ld_p.p(),ld_p.y()),
                Eigen::Vector3f(ld_p.tx(),ld_p.ty(),ld_p.tz()));
    }
    m_parameter=parameter;
    return true;
}

safety_excutor* safety_excutor::CreateExcutor(const shutter::shutter_param& parameter)
{
    safety_excutor* excutor=new safety_excutor();
    if(excutor->init(parameter)==false)
        return nullptr;
    return excutor;
}

#include <pcl/visualization/pcl_visualizer.h>
extern pcl::visualization::PCLVisualizer viewer;
bool safety_excutor::verify()
{


    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<m_parameter.lidars_size();++i)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_t=mp_lidars[i].GetCloud();
        (*cloud)+=(*cloud_t);


    }

    printf(" shutter  recieve points %d\n",cloud->size());


    viewer.removeAllPointClouds();
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud, 0,255,0); // green
    viewer.addPointCloud(cloud, single_color2, "cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");


    viewer.spinOnce();

}