shutter_verify/main.cpp

//
// Created by zx on 2019/12/28.
//
#include <fcntl.h>
#include <iostream>
#include "plc/plc_communicator.h"
//#include "laser/Laser.h"
#include "plc/plc_communicator.h"
#include "tool/pathcreator.h"
#include <glog/logging.h>
#include "verifier.h"
#include "proto_tool.h"
#include <livox_sdk.h>
#include "LivoxMid70.h"
using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;
#include "setting.pb.h"
#include "safety_excutor.h"
GOOGLE_GLOG_DLL_DECL void shut_down_logging(const char* data, int size)
{
    time_t tt;
    time( &tt );
    tt = tt + 8*3600;  // transform the time zone
    tm* t= gmtime( &tt );
    char buf[255]={0};
    sprintf(buf,"./%d%02d%02d-%02d%02d%02d-dump.txt",
            t->tm_year + 1900,
            t->tm_mon + 1,
            t->tm_mday,
            t->tm_hour,
            t->tm_min,
            t->tm_sec);

    FILE* tp_file=fopen(buf,"w");
    fprintf(tp_file,data,strlen(data));
    fclose(tp_file);

}

void init_glog()
{
    time_t tt = time(0);//时间cuo
    struct tm* t = localtime(&tt);

    char strYear[255]={0};
    char strMonth[255]={0};
    char strDay[255]={0};

    sprintf(strYear,"%04d", t->tm_year+1900);
    sprintf(strMonth,"%02d", t->tm_mon+1);
    sprintf(strDay,"%02d", t->tm_mday);

    char buf[255]={0};
    getcwd(buf,255);
    char strdir[255]={0};
    sprintf(strdir,"%s/log/%s/%s/%s", buf,strYear,strMonth,strDay);
    PathCreator creator;
    creator.Mkdir(strdir);

    char logPath[255] = { 0 };
    sprintf(logPath, "%s/", strdir);
    FLAGS_max_log_size = 100;
    FLAGS_logbufsecs = 0;
    google::InitGoogleLogging("shutter_verify");
    google::SetStderrLogging(google::INFO);
    google::SetLogDestination(0, logPath);
    google::SetLogFilenameExtension("zxlog");
    google::InstallFailureSignalHandler();
    google::InstallFailureWriter(&shut_down_logging);
    FLAGS_colorlogtostderr = true;        // Set log color
    FLAGS_logbufsecs = 0;                // Set log output speed(s)
    FLAGS_max_log_size = 1024;            // Set max log file size(GB)
    FLAGS_stop_logging_if_full_disk = true;
}

std::mutex   mut;
pcl::visualization::PCLVisualizer viewer("Cloud");



//读取protobuf 配置文件
//file:文件
//parameter:要读取的配置
bool read_proto_param(std::string file, ::google::protobuf::Message& parameter)
{
    int fd = open(file.c_str(), O_RDONLY);
    if (fd == -1) return false;
    FileInputStream* input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &parameter);
    delete input;
    close(fd);
    return success;
}

int Working()
{

    while(1)
    {
        break;
    }

    /*Error_manager code;

    Eigen::Vector4f plane,plane1;
    plane[0]=0;
    plane[1]=1;
    plane[2]=0.2;
    plane[3]=2;

    plane1[0]=0;
    plane1[1]=1;
    plane1[2]=-0.2;
    plane1[3]=-1.3;
    float maxd=0.02,mind=-0.08,maxd1=-0.02,mind1=-0.12;

    shuttler_verifier left_shutter;
    shuttler_verifier right_shutter;
    left_shutter.set_condition(plane1,maxd1,mind1);
    right_shutter.set_condition(plane,maxd,mind);

    verifier shutter(left_shutter,right_shutter);
    mut.lock();
    left_shutter.create_plane(viewer);
    right_shutter.create_plane(viewer);
    mut.unlock();

    //创建雷达  循环采集
    //读laser配置
    Laser_proto::Laser_parameter_all laser_parameters;
    if(!proto_tool::read_proto_param("./setting/laser.prototxt",laser_parameters))
    {
        LOG(ERROR)<<"read laser parameter failed";
        return -1;
    }

    int laser_cout=laser_parameters.laser_parameters_size();
    if(laser_cout==0)
    {
        LOG(ERROR)<<" no lidars ";
        return 0;
    }
    std::vector<Laser_base*>  plasers;
    plasers.resize(laser_cout);
    for(int i=0;i<laser_parameters.laser_parameters_size();++i)
    {
        plasers[i] = LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(), i,
                                                 laser_parameters.laser_parameters(i));
        if (plasers[i] != NULL)
        {
            if (plasers[i]->connect_laser() != SUCCESS)
            {
                char description[255] = {0};
                sprintf(description, "Laser %d connect failed...", i);
                LOG(ERROR)<<description;
            }
        }
    }
    for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
    {
        std::string type = laser_parameters.laser_parameters(i).type();
        if (type.find("Livox") != type.npos ||type.find("Horizon") != type.npos)
        {
            if (Start() == false)
            {
                Uninit();
                LOG(ERROR)<<"Livox laser init failed...";
                return -2;
            }
            break;
        }
    }

    usleep(3000*1000);

    LOG(INFO)<<"init ok...";

    //采集
    int frames=0;
    while(1)
    {
        auto t_start_point = std::chrono::system_clock::now();

        pcl::PointCloud<pcl::PointXYZ> scan_cloud;
        std::mutex cloud_lock;
        std::vector<Laser_task *> laser_task_vector;
        for (int i = 0; i < plasers.size(); ++i)
        {
            int frame_count = laser_parameters.laser_parameters(i).frame_num();
            if (plasers[i] != nullptr)
            {

                //创建扫描任务,
                Laser_task *laser_task = new Laser_task();
                //
                laser_task->task_init(TASK_CREATED, &scan_cloud, &cloud_lock);
                laser_task->set_task_frame_maxnum(frame_count);
                laser_task_vector.push_back(laser_task);
                //发送任务单给雷达
                code = plasers[i]->execute_task(laser_task);
                if (code != SUCCESS)
                {
                    LOG(ERROR) << " capture failed :" << code.get_error_description();
                    return -3;
                }
            }

        }


        //等待雷达完成任务单
        double second=0.0;
        while (1)
        {
            //判断是否强制退出
            //判断雷达任务单是否全部完成
            bool tb_laser_complete = true;
            for (int i = 0; i < laser_task_vector.size(); ++i)
            {
                tb_laser_complete &= (TASK_OVER == laser_task_vector[i]->get_statu());
            }
            if (tb_laser_complete)
            {
                break;
            }
            //计算扫描时间,若超时,并且没有点,则返回错误.
            auto t_end_point = std::chrono::system_clock::now();
            auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
            second = double(duration.count()) *
                    std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
            if (second > 0.5)
            {
                LOG(WARNING)<<"capture time out";
                break;
            }
            usleep(1000*10);
        }
        //检查雷达任务完成情况,是否是正常完成

        //释放扫描任务单
        for (int i = 0; i < laser_task_vector.size(); ++i) {
            if (laser_task_vector[i] != 0) {
                delete laser_task_vector[i];
                laser_task_vector[i] = NULL;
            }
        }
        LOG(INFO) << " frame :"<<++frames<<",laser scanning over cloud size:" << scan_cloud.size()
                  <<" , time:"<<second;


        //处理点云--------------------------------------------------------------------------


        int color[3]={0,255,0};
        if(shutter.verify(scan_cloud.makeShared())!=SUCCESS)
        {
            color[0]=255;
            color[1]=0;
            color[2]=0;
        }

        mut.lock();
        viewer.removeAllPointClouds();
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(scan_cloud.makeShared(), color[0],color[1],color[2]); // green
        viewer.addPointCloud(scan_cloud.makeShared(), single_color, "livox");
        viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "livox");
        mut.unlock();
        usleep(10*1000);
    }

*/


}

int main(int argc,char* argv[])
{
    //初始化日志
    init_glog();
    viewer.addCoordinateSystem(2.0,0,0,0,"car_center");

    int ret = livox::LdsLidar::InitLdsLidar();


    shutter::shutter_param  shutter_param;
    if(read_proto_param("./setting/setting.prototxt",shutter_param)==false)
    {
        printf("  read proto failed \n");
        return -1;
    }

    safety_excutor* excutor=safety_excutor::CreateExcutor(shutter_param);
    if(excutor== nullptr)
    {
        printf(" create safety_excutor failed\n");
        return -1;
    }



    /*const float DEGREE=M_PI/180.0;
    float transform1[]={-2.31,161.58,-64,2.512,0,0};
    float transform2[]={176.45,28.05,71.85,0,0,0};


    std::string sn1="3GGDJ8S00100921";
    std::string sn2="3GGDJ8X00100541";
    Eigen::Vector3f rpy1;
    rpy1<<-2.31*DEGREE,161.58*DEGREE,-64*DEGREE;
    Eigen::Vector3f rpy2;
    rpy2<<176.45*DEGREE,28.05*DEGREE,71.85*DEGREE;

    Eigen::Vector3f t1;
    t1<<1.256,0,0;
    Eigen::Vector3f t2;
    t2<<-1.256,0,0;


    read_lidar.SetTransformParam(rpy1,t1);
    read_lidar1.SetTransformParam(rpy2,t2);

    if(read_lidar.AddBroadcastCode(sn1)!=0)
        printf("%s  connect failed\n",sn1.c_str());
    if(read_lidar1.AddBroadcastCode(sn2)!=0)
    {
        printf("%s  connect failed\n",sn2.c_str());

    }
    read_lidar.SetFPS(10.0);
    read_lidar1.SetFPS(10.0);*/
    usleep(3000*1000);




    //lidar1.AddSNCode(sn1);
    //lidar2.AddSNCode(sn2);

    std::thread* thread=new std::thread(Working);

    while(1)
    {
        excutor->verify();
        /**/
        usleep(33*1000);
    }

    return 0;
}