Measure/Modules/ClampSafety/main.cpp

//
// Created by zx on 22-8-30.
//
#include <cstdio>
#include <unistd.h>
#include <cstring>
#include <fstream>
#include <byteswap.h>
#include <glog/logging.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <google/protobuf/text_format.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>

#include "detect/clamp_safety_detect.h"
#include "plc/snap7_clamp.h"
#include "lidar/wanji_716N_device.h"
#include "tool/pathcreator.h"
#include "lidar/lidarsJsonConfig.h"
#include "defines.hpp"

#if __VIEW__PCL
pcl::visualization::PCLVisualizer g_viewer;
#endif

void shut_down_logging(const char *data, size_t 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(nullptr);//时间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(google::INFO, logPath);
    google::SetLogFilenameExtension("log");
    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;
}

typedef struct {
    Wanji_716N_lidar_device lidar;
    clamp_safety_detect detector;
    Safety_status safety_statu;
    std::mutex mutex;
} Clamp;

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() < 150) {
        return false;
    }
    pdetector->detect(cloud, *safety);
    return true;
}

void thread_func(Clamp *clamp) {
    while (true) {
        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);
    }
}

int main() {
    //初始化日志
    init_glog();

    // 雷达数量
    const int CLAMP_SIZE = 4;
    //调试模式下，先打开调试窗口
#if __VIEW__PCL
    int v1,v2,v3,v4;
    //窗口参数分别对应 x_min, y_min, x_max, y_max, viewport
    g_viewer.createViewPort(0.0, 0.5, 0.5, 1, v1);
    g_viewer.createViewPort(0.5, 0.5, 1.0, 1, v2);
    g_viewer.createViewPort(0.0, 0, 0.5, 0.5, v3);
    g_viewer.createViewPort(0.5, 0, 1.0, 0.5, v4);
    g_viewer.addText("192.168.0.2", 10,10, 20, 0,1,0, "ip v1", v1);
    g_viewer.addText("192.168.0.x2", 10,10, 20, 0,1,0, "ip v2", v2);
    g_viewer.addText("192.168.0.x3", 10,10, 20, 0,1,0, "ip v3", v3);
    g_viewer.addText("192.168.0.x4", 10,10, 20, 0,1,0, "ip v4", v4);

    g_viewer.addCoordinateSystem(0.2,"v1_axis",v1);
    g_viewer.addCoordinateSystem(0.2,"v2_axis",v2);
    g_viewer.addCoordinateSystem(0.2,"v3_axis",v3);
    g_viewer.addCoordinateSystem(0.2,"v4_axis",v4);
#endif
    //雷达
    Clamp clamps[CLAMP_SIZE];

    LOG(INFO) << "load etc file from " << ETC_PATH"/etc/clamp_safety_lidars.json";
    lidarsJsonConfig config(ETC_PATH"/etc/clamp_safety_lidars.json");
    for (int i = 0; i < CLAMP_SIZE; ++i) {
        lidarsJsonConfig::LidarConfig lidar;
        config.getLidarConfig(i, lidar);
        lidar.info();
        Point2D_tool::Polar_coordinates_box t_polar_coordinates_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;

        Point2D_tool::Point2D_box t_point2D_box;
        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 EXIT_FAILURE;
        }
        clamps[i].safety_statu.set_timeout(0.3);
    }

    //调试状态下，绑定识别器输出窗口
#if __VIEW__PCL
    if (parameter.lidars_size()>0)
        clamps[0].detector.set_viewer(&g_viewer,v1);
    if (parameter.lidars_size()>1)
        clamps[1].detector.set_viewer(&g_viewer,v2);
    if (parameter.lidars_size()>2)
        clamps[2].detector.set_viewer(&g_viewer,v3);
    if (parameter.lidars_size()>3)
        clamps[3].detector.set_viewer(&g_viewer,v4);

    while(1)
    {
        usleep(100*1000);
        auto t1=std::chrono::steady_clock::now();
        //调试状态下，顺序执行，因为viewer不支持多线程
        if (parameter.lidars_size()>0)
            run(&(clamps[0].lidar),&(clamps[0].detector),&(clamps[0].safety_statu));
        if (parameter.lidars_size()>1)
            run(&(clamps[1].lidar),&(clamps[1].detector),&(clamps[1].safety_statu));
        if (parameter.lidars_size()>2)
            run(&(clamps[2].lidar),&(clamps[2].detector),&(clamps[2].safety_statu));
        if (parameter.lidars_size()>3)
            run(&(clamps[3].lidar),&(clamps[3].detector),&(clamps[3].safety_statu));
            auto t2=std::chrono::steady_clock::now();
            auto duration = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);

            double time= double(duration.count()) * std::chrono::microseconds::period::num /
                    std::chrono::microseconds::period::den;

        //printf("total time :%.3f s\n",time);
        g_viewer.spinOnce();
    }
#else
    // 和plc建立连接，一直等待连接成功
//    auto snap7_client = &Snap7Clamp::get_instance_references();
//    snap7_client->communication_init();
//    Snap7Clamp::Snap7_communication_statu status = snap7_client->get_status();
//    while (status == Snap7Clamp::SNAP7_COMMUNICATION_READY || status == Snap7Clamp::SNAP7_COMMUNICATION_RECEIVE) {
//        sleep(1);
//        status = snap7_client->get_status();
//    }

    // 启动四个线程，分别采集四个雷达的数据。
    std::thread threads[CLAMP_SIZE];
    for (int i = 0; i < CLAMP_SIZE; ++i) {
        threads[i] = std::thread(thread_func, &clamps[i]);
    }

    int heart = 0;
    while (true) {
        usleep(100 * 1000);
        heart = (heart + 1) % 255;
//        snap7_client->plcData.pingpong = heart;

        for (int i = 0; i < 4; i++) {
            std::unique_lock<std::mutex> t_lock(clamps[i].mutex);
//            std::unique_lock<std::mutex> t_lock1(snap7_client->m_data_lock);
            if (!clamps[i].safety_statu.is_timeout()) {
                LOG(WARNING) << "id: " << i << ", we: " << clamps[i].safety_statu.wheel_exist << ", cx: " << clamps[i].safety_statu.cx * 1000;
//                LOG(INFO) << "orid " << i;
//                LOG(INFO) << "wheel_exist " << clamps[i].safety_statu.wheel_exist;
//                LOG(INFO) << "cx " << clamps[i].safety_statu.cx * 1000;
//                LOG(INFO) << "gap " << clamps[i].safety_statu.gap * 1000;
//                LOG(INFO) << "clamp_completed " << clamps[i].safety_statu.clamp_completed;
//                snap7_client->plcData.wheels[i].wheel_exist = clamps[i].safety_statu.wheel_exist;
////                snap7_client->plcData.wheels[i].offset = heart;
////                snap7_client->plcData.wheels[i].gap = 255 - heart;
//                snap7_client->plcData.wheels[i].offset = clamps[i].safety_statu.cx * 1000;
//                snap7_client->plcData.wheels[i].gap = clamps[i].safety_statu.gap * 1000;
//                snap7_client->plcData.wheels[i].clamp_completed = clamps[i].safety_statu.clamp_completed;
            } else {
//                snap7_client->plcData.clear();
            }
        }
    }

#endif

    return EXIT_SUCCESS;
}