shutter_verify/safety_excutor.cpp

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

#include "safety_excutor.h"
#include <pcl/filters/passthrough.h>
#include <pcl/common/common.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()];
    const float DEGREE=M_PI/180.0;
    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()*DEGREE,ld_p.p()*DEGREE,ld_p.y()*DEGREE),
                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;
}

#if VIEW
#include <pcl/visualization/pcl_visualizer.h>
pcl::visualization::PCLVisualizer viewer("Cloud");
#endif

pcl::PointCloud<pcl::PointXYZ>::Ptr  safety_excutor::PassThroughCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                                                      const shutter::box_param& box)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inner(new pcl::PointCloud<pcl::PointXYZ>);
//限制 x y z
    pcl::PassThrough<pcl::PointXYZ> passX;
    pcl::PassThrough<pcl::PointXYZ> passY;
    pcl::PassThrough<pcl::PointXYZ> passZ;
    passX.setInputCloud(cloud);
    passX.setFilterFieldName("x");
    passX.setFilterLimits(box.minx(), box.maxx());
    passX.filter(*cloud_inner);

    passY.setInputCloud(cloud_inner);
    passY.setFilterFieldName("y");
    passY.setFilterLimits(box.miny(), box.maxy());
    passY.filter(*cloud_inner);

    passY.setInputCloud(cloud_inner);
    passY.setFilterFieldName("z");
    passY.setFilterLimits(box.minz(), box.maxz());
    passY.filter(*cloud_inner);
    return cloud_inner;

}


bool safety_excutor::verify(tagMeasureData& data)
{

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

    shutter::box_param box=m_parameter.box();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_inner=PassThroughCloud(cloud,box);

    shutter::box_param box1=m_parameter.verify_box1();
    shutter::box_param box2=m_parameter.verify_box2();
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_warnning1=PassThroughCloud(cloud_inner,box1);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_warnning2=PassThroughCloud(cloud_inner,box2);


    bool safe1=(cloud_warnning1->size()<=20),safe2=((cloud_warnning2->size()<=20));
    //std::cout<<"  v1:"<<cloud_warnning1->size()<<"      v2:"<<cloud_warnning2->size()<<std::endl;

    data.wheel_path_safety=(unsigned short)(safe1&safe2&connected);

    shutter::box_param box_parkspace,box_left,box_right;
    if(box1.maxx()<box2.minx())
    {
        box_left=box1;
        box_right=box2;
    }
    else
    {
        box_left=box2;
        box_right=box1;
    }
    box_parkspace=box1;
    box_parkspace.set_minx(box_left.maxx());
    box_parkspace.set_maxx(box_right.minx());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_parkspace=PassThroughCloud(cloud_inner,box_parkspace);
    data.parkspcae_safety=(cloud_parkspace->size()<=5)?(connected&true):(connected&false);

    //计算缝隙与角度
    if(cloud_inner->size()!=0)
    {


        pcl::PointXYZ minp, maxp;
        pcl::getMinMax3D(*cloud_inner, minp, maxp);
        float l = minp.x - box_left.maxx();
        float r = box_right.minx() - maxp.x;
        data.left_space = (l >= 0) ? l : 0;
        data.right_space = (r >= 0) ? r : 0;

        //优化求解角度与偏移
        data.theta = 0;
        data.offset_x = 0;
    }
    else
    {
        data.left_space = 2.0;
        data.right_space = 2.0;

        //优化求解角度与偏移
        data.theta = 0;
        data.offset_x = 0;
    }

#if VIEW


    viewer.removePointCloud(m_parameter.ShortDebugString()+"cloud");
    viewer.removeShape(m_parameter.ShortDebugString()+"cube1");
    viewer.removeShape(m_parameter.ShortDebugString()+"cube2");
    //绘制矩形
    Eigen::AngleAxisf rotation = Eigen::AngleAxisf(0, Eigen::Vector3f::UnitZ());

    viewer.addCube(Eigen::Vector3f((box1.minx()+box1.maxx())/2.0,(box1.miny()+box1.maxy())/2.0,(box1.minz()+box1.maxz())/2.0),
            Eigen::Quaternionf(rotation), box1.maxx()-box1.minx(), box1.maxy()-box1.miny(),box1.maxz()-box1.minz(),
                   m_parameter.ShortDebugString()+"cube1");
    viewer.addCube(Eigen::Vector3f((box2.minx()+box2.maxx())/2.0,(box2.miny()+box2.maxy())/2.0,(box2.minz()+box2.maxz())/2.0),
                   Eigen::Quaternionf(rotation), box2.maxx()-box2.minx(), box2.maxy()-box2.miny(),box2.maxz()-box2.minz(),
                   m_parameter.ShortDebugString()+"cube2");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0.5, 0, m_parameter.ShortDebugString()+"cube1");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, m_parameter.ShortDebugString()+"cube1");

    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0.5, 1, m_parameter.ShortDebugString()+"cube2");
    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, m_parameter.ShortDebugString()+"cube2");



    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloud,
            255*int(!data.wheel_path_safety),255*int(data.wheel_path_safety),255*int(!data.parkspcae_safety));
    viewer.addPointCloud(cloud, single_color2, m_parameter.ShortDebugString()+"cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, m_parameter.ShortDebugString()+"cloud");

    viewer.spinOnce();
#endif
    printf(" shutter  recieve points %d l_space:%.3f  r_space:%.3f  wheel_safety: %d   parkspace_safety: %d\n",
            cloud->size(),data.left_space,data.right_space,data.wheel_path_safety,data.parkspcae_safety);
    //保存一帧
    /*static bool saved=false;
    if(saved==false)
    {
        FILE* p=fopen("./cloud.txt","w");
        for(int i=0;i<cloud->size();++i)
        {
            fprintf(p,"%f %f %f\n",cloud->points[i].x,cloud->points[i].y,cloud->points[i].z);
        }
        fclose(p);
        saved=true;
    }*/

    return data.wheel_path_safety;

}


bool safety_excutor::pointInRectangle(pcl::PointXYZ point,const shutter::box_param& box)
{
    float x=point.x;
    float y=point.y;
    float z=point.z;
    if(x>box.minx()&&x<box.maxx()&&y>box.miny()&&y<box.maxy()&&z>box.minz()&&z<box.maxz())
        return true;
    return false;
}