clamp_safety/monitor.cpp

//
// Created by zx on 22-8-30.
//
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/extract_clusters.h>


#include <fstream>
#include "lidar/wanji_716N_device.h"
#include <byteswap.h>
#include "tool/pathcreator.h"
#include <glog/logging.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>
#include "clamp_parameter.pb.h"
#include "tool/proto_tool.h"
#include<stdio.h>
#include<stdlib.h>
#include<unistd.h>
#include<string.h>
#include<sys/types.h>
#include<arpa/inet.h>
#include<net/if.h>
#include<string.h>
#include<signal.h>
#include<sys/wait.h>
#include<sys/ioctl.h>
#include<regex.h>
#include "test.hpp"
#include <fstream>

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;

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("clamp_safety");
    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;
}

bool string2point(std::string str,pcl::PointXYZ& point)
{
    std::istringstream iss;
    iss.str(str);
    float value;
    float data[2]={0};
    for(int i=0;i<2;++i)
    {
        if(iss>>value)
        {
            data[i]=value;
        }
        else
        {
            return false;
        }
    }
    point.x=data[0];
    point.y=data[1];
    return true;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr ReadTxtCloud(std::string file)
{
    std::ifstream fin(file.c_str());
    const int line_length=64;
    char str[line_length]={0};
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    while(fin.getline(str,line_length))
    {
        pcl::PointXYZ point;
        if(string2point(str,point))
        {
            printf("%f, %f, %f.\n", point.x, point.y, point.z);
            cloud->push_back(point);
        }
    }
    return cloud;
}

// bool run(Wanji_716N_lidar_device* device,clamp_safety_detect* pdetector,Safety_status* safety)
// {
//   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//   device->get_last_cloud(cloud, std::chrono::system_clock::now());

//   if (cloud->size() < 100)
//   {
//     return false;
//   }
//   pdetector->detect(cloud,*safety);

//   return true;

// }

// void thread_func(Clamp* clamp)
// {
//     while(1)
//     {
//       clamp->mutex.lock();
//         Safety_status safety_t;
//       if(run(&(clamp->lidar),&(clamp->detector),&safety_t))
//           clamp->safety_statu=safety_t;
//       clamp->mutex.unlock();

//       usleep(100*1000);
//     }
// }
#if 1
int main(int argc, char* argv[]) {
    init_glog();
    clamp_safety::clamp_parameter parameter;
    std::string cfg_file = "../setting/parameter.prototxt";
    if(proto_tool::read_proto_param(cfg_file,parameter)==false) {
        LOG(ERROR) << " read prototxt failed";
        return -1;
    }

    // int CLAMP_SIZE = 1;
    int CLAMP_SIZE = parameter.lidars_size();
    Clamp clamps[CLAMP_SIZE];

    for(int i=0;i < CLAMP_SIZE;++i) {
        clamp_safety::LidarParameter lidar = parameter.lidars(i);
        Point2D_tool::Polar_coordinates_box t_polar_coordinates_box;
        Point2D_tool::Point2D_box t_point2D_box;
        Point2D_tool::Point2D_transform t_point2D_transform;
        t_polar_coordinates_box.angle_min = lidar.angle_min();
        t_polar_coordinates_box.angle_max = lidar.angle_max();
        t_polar_coordinates_box.distance_min = lidar.range_min();
        t_polar_coordinates_box.distance_max = lidar.range_max();

        t_point2D_box.x_min = lidar.scan_box_limit().minx();
        t_point2D_box.x_max = lidar.scan_box_limit().maxx();
        t_point2D_box.y_min = lidar.scan_box_limit().miny();
        t_point2D_box.y_max = lidar.scan_box_limit().maxy();

        Error_manager code = clamps[i].lidar.init(lidar.net_config().ip_address(),lidar.net_config().port(),
                t_polar_coordinates_box,t_point2D_box,t_point2D_transform);
        if(code != SUCCESS) {
            LOG(ERROR) << code.get_error_description();
            return -3;
        }
        clamps[i].safety_statu.set_timeout(0.3);
    }

    int Y = 0;
    Test test(&clamps[Y]);
    test.initPlc(parameter);
    test.thread_fun();

    return 0;
}
#endif

#if 0
bool GetAlg(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float &x) {
    float sumx = 0,sumy = 0, w = 0, sumw = 0;

    for(int i=0; i < cloud->size(); i++) {
        w = float(1/pow(cos(atan(fabs(cloud->points[i].x/ cloud->points[i].y))), 2));
        // w = float(sqrt((36.0 - w))/6.0);
        sumw += w;
        sumx += w * cloud->points[i].x;
    }
    x = sumx/sumw;
    // printf("---Debug alg result %.3f.\n", x*1000);
    // return (fabs(x*1000) > 30 && fabs(x*1000) < 150);
    return (fabs(x*1000) < 150);
}

int main (int argc, char** argv) {
    std::fstream txtFile, pointFile;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    std::string name = argv[1];
    txtFile.open(name + ".txt");
    name = "warning/" + name + "/";
    if(NULL == opendir(name.c_str())) {
        mkdir(name.c_str(), 0777);
    }
    char buf[256];
    std::string str;
    pcl::PointXYZ point;
    float x;
    int warning = 0;
    while (txtFile.getline(buf, 256)) {
        str = buf;
        if(std::string::npos == str.find(" ")) {
            // printf("%s\n", buf);
            for (int i = atoi(buf); i > 0; i--) {
                txtFile.getline(buf, 256);
                str = buf;
                string2point(str, point);
                cloud->push_back(point);
            }
            if( GetAlg(cloud, x)) {
                // printf("find warning point.\n");
                warning++;
                pointFile.open(name + std::to_string(warning) + "_" + std::to_string(x) + ".txt", std::ios_base::out | std::ios::app);
                for(int i = 0; i < cloud->size(); i++) {
                    pointFile << cloud->points[i].x << " " << cloud->points[i].y << std::endl;
                }
                usleep(10*1000);
                pointFile.close();
            }
            cloud->clear();
        }
    }
    pointFile.close();
    txtFile.close();
}
#endif