shutter_verify/main.cpp

//
// Created by zx on 2019/12/28.
//


#include <fcntl.h>
#include <iostream>
#include "tool/pathcreator.h"
#include <glog/logging.h>
#include "verifier.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
#include <livox_sdk.h>
#include "LivoxMid70.h"
#include "snap7_communication_base.h"
#include <thread>
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"
#include <byteswap.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;
}



//读取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;
}

#if VIEW
extern pcl::visualization::PCLVisualizer viewer;
#endif

int Working()
{

    while(1)
    {
        break;
    }

    return 0;
}

float SWAP_float(float x)
{
    int b;
    memcpy(&b,&x,sizeof(float));
    int t= ((((b) & 0xff000000) >> 24) | (((b) & 0x00ff0000) >>  8) |(((b) & 0x0000ff00) <<  8) | (((b) & 0x000000ff) << 24));
    float value=0;
    memcpy(&value,&t,sizeof(float));
    return value;
}

float swap(float value)
{
    char* p=(char*)&value;
    float b=0;
    char* pb=(char*)&b;
    for(int i=0;i<sizeof(float);++i)
    {
        memcpy(pb+i,p+sizeof(float)-1-i,1);
    }
    return b;
}

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

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

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

    safety_excutor* excutor_entrance=safety_excutor::CreateExcutor(setting_parameter.entrance_parameter());
    safety_excutor* excutor_export=safety_excutor::CreateExcutor(setting_parameter.export_parameter());

    Snap7_communication_base 					snap7_client;			//通信的客户端
    Error_manager code=snap7_client.communication_init(setting_parameter.plc_setting().ip());
    if(code!=SUCCESS)
    {
        std::cout<<code<<std::endl;
        return -1;
    }

    usleep(3000*1000);

    //std::thread* thread=new std::thread(Working);
    tagMeasureData  measure_data_entrance;
    tagMeasureData  measure_data_export;
#pragma pack(push,1)
    struct PLCData
    {
        unsigned short   pingpong;
        unsigned short  wheel_path_safety;
        unsigned short  parkspace_safety;
        float left_space;
        float right_space;
        float theta;
        float offset_x;

        unsigned short  wheel_path_safety_export;
        unsigned short  parkspace_safety_export;
        float left_space_export;
        float right_space_export;
        float theta_export;
        float offset_x_export;
    };
#pragma pack(pop)
    int PING=0;
    Snap7_buf buf;
    buf.m_communication_mode = Snap7_buf::Communication_mode::LOOP_COMMUNICATION;
    while(1)
    {
        memset(&measure_data_entrance,0,sizeof(measure_data_entrance));
        memset(&measure_data_export,0,sizeof(measure_data_export));
        if(excutor_entrance!= nullptr)
        {
            printf(" entrance <<<<<<  ");
            excutor_entrance->verify(measure_data_entrance);
        }
        if(excutor_export!= nullptr)
        {
            printf(" export   >>>>>>  ");
            excutor_export->verify(measure_data_export);
        }


        struct PLCData plc_data;
        plc_data.pingpong=bswap_16((PING++)%100000);
        plc_data.offset_x=SWAP_float(measure_data_entrance.offset_x);
        plc_data.left_space=SWAP_float(measure_data_entrance.left_space);
        plc_data.right_space=SWAP_float(measure_data_entrance.right_space);
        plc_data.theta=SWAP_float(measure_data_entrance.theta);
        plc_data.wheel_path_safety=bswap_16(measure_data_entrance.wheel_path_safety);
        plc_data.parkspace_safety=bswap_16(measure_data_entrance.parkspcae_safety);

        plc_data.offset_x_export=SWAP_float(measure_data_export.offset_x);
        plc_data.left_space_export=SWAP_float(measure_data_export.left_space);

        plc_data.right_space_export=SWAP_float(measure_data_export.right_space);
        plc_data.theta_export=SWAP_float(measure_data_export.theta);
        plc_data.wheel_path_safety_export=bswap_16(measure_data_export.wheel_path_safety);
        plc_data.parkspace_safety_export=bswap_16(measure_data_export.parkspcae_safety);


        printf("\n ------sizeof float:%d-------------------l_space:%.3f  r_space:%.3f  wheel_safety: %d   parkspace_safety: %d\n",
               sizeof(float),
               measure_data_entrance.left_space,measure_data_entrance.right_space,
               measure_data_entrance.wheel_path_safety,measure_data_entrance.parkspcae_safety);

        buf.m_id=setting_parameter.plc_setting().dbnumber();
        buf.m_size=sizeof(plc_data);
        buf.m_start_index=setting_parameter.plc_setting().start_id();
        buf.mp_buf_reverse=&plc_data;

        code=snap7_client.write_data_buf(buf);


        if(code!=SUCCESS)
        {
            snap7_client.communication_uninit();
            usleep(500*1000);
            snap7_client.communication_init(setting_parameter.plc_setting().ip());
            usleep(500*1000);
            std::cout<<code<<std::endl;
        }


        usleep(50*1000);
    }

    return 0;
}