CT_project/测量节点/main.cpp

//
// Created by zx on 2020/6/18.
//
#include <pcl/visualization/pcl_visualizer.h> 

#include <iostream>
#include "./error_code/error_code.h"
#include "./wanji_lidar/LogFiles.h"
#include <glog/logging.h>

#include "./communication/communication_socket_base.h"

#include "./tool/thread_pool.h"
#include "./system/system_communication.h"
#include "./system/system_executor.h"

// #include "./wanji_lidar/wanji_lidar_device.h"
#include "./tool/proto_tool.h"
#include "./wanji_lidar/wanji_manager.h"
#include "./velodyne_lidar/velodyne_manager.h"

#define LIVOX_NUMBER	     2
#define POINT_DEBUG

#ifdef POINT_DEBUG
std::shared_ptr<pcl::visualization::PCLVisualizer> gp_visualizer;
#endif

const char* DefaultLogPath = "log/";
const char* DefaultLogName = "zxlog";
const int DefalutThreadsSize = 4;

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 GoogleLogInit() {
    google::InitGoogleLogging("LidarMeasurement");
    google::SetStderrLogging(google::INFO);
    google::SetLogDestination(google::INFO, DefaultLogPath);
    google::SetLogFilenameExtension(DefaultLogName);
    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;
}

void test_whole_process()
{
    System_executor::get_instance_references().execute_for_measure("key_for_test_only", 0, std::chrono::system_clock::now());
}

int main(int argc,char* argv[])
{
    std::cout << "Welcome using zhi xiang software." << std::endl;

    Error_manager ec;
    GoogleLogInit();

#ifdef POINT_DEBUG
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::visualization::PCLVisualizer viewer("Viewer");      //创建viewer对象
    gp_visualizer = std::shared_ptr<pcl::visualization::PCLVisualizer>(&viewer);
    gp_visualizer->addCoordinateSystem();
    gp_visualizer->setBackgroundColor(0, 0, 0);
    gp_visualizer->initCameraParameters();
    gp_visualizer->addPointCloud(t_cloud, "region");
#endif

    // 定义服务的终端id
    int t_terminal_id = 0;
    if ( argc == 2 )
    {
        t_terminal_id = atoi(argv[1]);
    }
    std::cout << " t_terminal_id = " << t_terminal_id << std::endl;

    // 初始化
    if(WJ_VELO == 0 || WJ_VELO == 2)
    {
        ec = Wanji_manager::get_instance_references().wanji_manager_init(t_terminal_id);
        std::cout << "wanji_manager = " << Wanji_manager::get_instance_references().check_status().to_string() << std::endl;
        if (ec != SUCCESS)
        {
            LOG(ERROR) << "wanji_manager init failed: " << ec.to_string();
            return -1;
        }
    }
    if(WJ_VELO == 1 || WJ_VELO == 2)
    {
        ec = Velodyne_manager::get_instance_references().velodyne_manager_init(t_terminal_id);

        if (ec != SUCCESS)
        {
            std::cout << "veodyne_manager = " << ec.to_string() << std::endl;
            std::cout << "veodyne_manager = " << Velodyne_manager::get_instance_references().check_status().to_string() << std::endl;
            LOG(ERROR) << "velodyne_manager init failed: " << ec.to_string();
            return -1;
        }
    }

    ec = System_executor::get_instance_references().system_executor_init(DefalutThreadsSize, t_terminal_id);
    std::cout << "System_executor = " << System_executor::get_instance_references().get_system_executor_status() << std::endl;
    if(ec != SUCCESS)
    {
        LOG(ERROR) << "system executor init failed: " << ec.to_string();
        return -1;
    }

    ec = System_communication::get_instance_references().communication_init();
    if(ec != SUCCESS)
    {
        LOG(ERROR) << "system communication init failed: " << ec.to_string();
        return -1;
    }
    System_communication::get_instance_references().set_encapsulate_cycle_time(110);

    ec = System_communication_mq::get_instance_references().rabbitmq_init();
    if(ec != SUCCESS)
    {
        LOG(ERROR) << "system communication mq init failed: " << ec.to_string();
        return -1;
    }
    System_communication_mq::get_instance_references().set_encapsulate_status_cycle_time(100);

    test_whole_process();

    char ch='x' ;
    pcl::PointCloud<pcl::PointXYZ>::Ptr t_region_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);

    while(ch != 'q') {
#ifdef POINT_DEBUG
        std::map<int, Ground_region *> t_ground_region_map = Velodyne_manager::get_instance_references().get_ground_region_map();
        for (auto iter = t_ground_region_map.begin(); iter != t_ground_region_map.end(); ++iter)
        {
            if (iter->first == DISP_TERM_ID)
            {
                t_region_cloud->clear();
                // 获取区域点云
                iter->second->get_region_cloud(t_region_cloud, Ground_region::Region_cloud_type::filtered);
                // gp_visualizer->showCloud(t_region_cloud);
                gp_visualizer->updatePointCloud(t_region_cloud, "region");
                // LOG(INFO) << "pcl cloud updated." <<t_region_cloud->size();
            }
        }
        gp_visualizer->spinOnce();
        usleep(100);
#else
        std::cout << "please input 'q' to quit system." << std::endl;
        std::cin >> ch;
#endif
    }

    // 反初始化
    System_communication::get_instance_references().communication_uninit();
    System_communication_mq::get_instance_references().rabbitmq_uninit();

    if(WJ_VELO == 1 || WJ_VELO == 2)
        Velodyne_manager::get_instance_references().Velodyne_manager_uninit();
    System_executor::get_instance_references().system_executor_uninit();
    if(WJ_VELO == 0 || WJ_VELO == 2)
        Wanji_manager::get_instance_references().wanji_manager_uninit();

    return 0;
}