CT_project/测量节点/tests/vlp16_driver_test.cpp

/*
 * @Description: vlp16驱动测试
 * @Author: yct
 * @Date: 2021-07-26 14:11:26
 * @LastEditTime: 2022-01-20 13:57:25
 * @LastEditors: yct
 */

#include <iostream>
#include <fstream>

// velodyne driver seperation test
#include "../velodyne_lidar/velodyne_driver/input.h"
#include "../velodyne_lidar/velodyne_driver/rawdata.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);

}



bool velo_driver_test()
{
    velodyne_driver::InputSocket sock;
    if (!sock.init("", 2368))
    {
        std::cout << "sock init failed" << std::endl;
        return false;
    }

    velodyne_rawdata::RawData data;
    data.setup("VLP16", "/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/VLP16db.yaml", 1.2, 0.1, 0, 360);

    int count = 0;
    unsigned char pkt[1206];
    std::vector<Lidar_point> t_point;
    while (count++ < 50000)
    {
        int ret = sock.getPacket(pkt);
        if (ret != 0)
            std::cout << "get pack: " << ret << std::endl;
        else
        {
            // t_point.clear();
            // memset(&pkt[1205], 0, 1);
            // printf("%s\n", pkt);
            data.unpack(pkt, t_point);
            std::cout << "point size: " << t_point.size() << std::endl;
            if(t_point.size() > 30000)
            {
                std::ofstream out("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/point.txt");
                if(out.is_open())
                {
                    for (size_t i = 0; i < t_point.size(); i++)
                    {
                        char buf[255] = {0};
                        sprintf(buf, "%.3f %.3f %.3f %.6f\n", t_point[i].x, t_point[i].y, t_point[i].z, t_point[i].timestamp);
                        out << buf;
                    }
                    out.close();
                    return true;
                }
                else
                {
                    std::cout << "cannot open file to write cloud." << std::endl;
                }
            }
        }
        usleep(500);
    }
    sock.uninit();
    return true;
}

// velodyne driver combination test
#include "../velodyne_lidar/velodyne_driver/velodyne_lidar_device.h"
void device_test()
{
    velodyne::velodyneLidarParams params;
    params.set_ip("");
    params.set_port(2368);
    params.set_model("VLP16");
    params.set_calibrationfile("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/VLP16db.yaml");
    params.mutable_calib()->set_y(1.57);

    Velodyne_lidar_device t_device;
    t_device.init(params);

    usleep(1000 * 1000);

    std::mutex p_mutex;
    pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    Error_manager ec = t_device.get_new_cloud_and_wait(&p_mutex, p_cloud, std::chrono::steady_clock::now(), 6000);

    if (ec == SUCCESS)
    {
        std::ofstream out("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/device_point.txt");
        if (out.is_open())
        {
            for (size_t i = 0; i < p_cloud->size(); i++)
            {
                char buf[255] = {0};
                sprintf(buf, "%.3f %.3f %.3f\n", p_cloud->points[i].x, p_cloud->points[i].y, p_cloud->points[i].z);
                out << buf;
            }
            std::cout << "cloud written" << std::endl;
            out.close();
        }
        else
        {
            std::cout << "cannot open file to write cloud." << std::endl;
        }
    }else{
        std::cout << "get cloud failed" << std::endl;
    }
    t_device.uninit();
}

// ground region class function test
#include "../velodyne_lidar/ground_region.h"
#include "../tool/measure_filter.h"
#include "../tool/point_tool.h"
#include "../tool/proto_tool.h"

void region_detect()
{
    std::string exce = "../tests/region_4_26x_g.txt", envi = "../tests/region_4_env_g.txt";
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_exce(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_envi(new pcl::PointCloud<pcl::PointXYZ>);
    bool res = read_pointcloud(exce, cloud_exce);
    res &= read_pointcloud(envi, cloud_envi);
    if (!res)
    {
        std::cout << "cannot open file to read cloud." << std::endl;
        return;
    }
    else
    {
        velodyne::velodyneManagerParams t_velo_params;
        std::string prototxt_path = "../setting/velodyne_manager.prototxt";
        if (!proto_tool::read_proto_param(prototxt_path, t_velo_params) || t_velo_params.region_size()<2)
        {
            return;
        }

        // 初始化region
        velodyne::Region param;
        param.CopyFrom(t_velo_params.region(1));
        pcl::PointCloud<pcl::PointXYZ>::Ptr left = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr right = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        if (!read_pointcloud("../setting/left_model.txt", left) || !read_pointcloud("../setting/right_model.txt", right))
        {
            std::cout << "cannot read left/right model " << std::endl;
            return;
        }
        Ground_region t_region;
        std::cout << "before init" << std::endl;
        t_region.init(param, left, right);
        std::cout << "after init" << std::endl;
        // 测试region功能
        // 手动检测，测试计算耗时
        for (size_t i = 0; i < 600; i++)
        {
            detect_wheel_ceres3d::Detect_result t_result;
            Error_manager ec = t_region.detect(cloud_exce, t_result);
            std::cout << ec.to_string() << std::endl;
            usleep(1);

            ec = t_region.detect(cloud_envi, t_result);
            std::cout << ec.to_string() << std::endl;
            usleep(1);
        }

        // 检测自动识别功能
        // std::cout << "---------------" << std::endl;
        // for (size_t i = 0; i < 12; i++)
        // {
        //     t_region.update_cloud(t_cloud);
        //     usleep(1000*100);
        // }

        // Common_data::Car_wheel_information t_wheel_info;

        // ec = Measure_filter::get_instance_references().get_filtered_wheel_information(0, t_wheel_info);
        // if (ec == SUCCESS)
        // {
        //     std::cout << t_wheel_info.to_string() << std::endl;
        // }
        // else { std::cout << ec.to_string() << std::endl; }
    }
}

// velodyne manager test
#include "../velodyne_lidar/velodyne_manager.h"
void velo_manager_test()
{
    Velodyne_manager_task t_velodyne_manager_task;
	Common_data::Car_wheel_information	t_car_information_by_velodyne;
	t_velodyne_manager_task.task_init(TASK_CREATED,"",NULL,std::chrono::milliseconds(3000),
								   0, std::chrono::system_clock::now());

    Error_manager ec = Velodyne_manager::get_instance_references().velodyne_manager_init(0);
	std::cout << "veodyne_manager = " << Velodyne_manager::get_instance_references().check_status().to_string() << std::endl;
    if(ec != SUCCESS)
    {
        std::cout << "manager init failed: " << ec.to_string() << std::endl;
        return;
    }
    usleep(1000 * 500);

    Velodyne_manager::get_instance_references().execute_task(&t_velodyne_manager_task);
    ec = Velodyne_manager::get_instance_references().execute_task(&t_velodyne_manager_task);
    std::cout << "manager execute base check result: " << ec.to_string() << std::endl;

    // 等待处理完成，来自于system_executor
    while (t_velodyne_manager_task.is_task_end() == false)
    {
        if (t_velodyne_manager_task.is_over_time())
        {
            //超时处理。取消任务。
            Velodyne_manager::get_instance_pointer()->cancel_task(&t_velodyne_manager_task);
            ec.error_manager_reset(Error_code::VELODYNE_MANAGER_TASK_OVER_TIME, Error_level::MINOR_ERROR,
                                        " t_velodyne_manager_task is_over_time ");
            t_velodyne_manager_task.set_task_error_manager(ec);
        }
        else
        {
            //继续等待
            std::this_thread::sleep_for(std::chrono::microseconds(1));
            std::this_thread::yield();
        }
    }

    //提取任务单 的错误码
    ec = t_velodyne_manager_task.get_task_error_manager();
    if (ec == Error_code::SUCCESS)
    {
        t_car_information_by_velodyne = t_velodyne_manager_task.get_car_wheel_information();
    }
    else
    {
        LOG(INFO) << " velodyne_manager_task error :::::::" << t_velodyne_manager_task.get_task_error_manager().to_string();
    }

    std::cout << "--------------------------"<< std::endl;
    Velodyne_manager::get_instance_references().Velodyne_manager_uninit();
}

// 测试通信功能
#include "../system/system_communication.h"
#include "../system/system_executor.h"
void message_test()
{
    nnxx::socket socket{nnxx::SP, nnxx::BUS};
    // socket.bind("tcp://127.0.0.1:7000");
    socket.connect("tcp://192.168.1.233:30010");
    int n = 0;
    message::Base_msg base_msg;

    message::Ground_detect_request_msg t_ground_detect_request;
    message::Base_info t_base_info;
    t_base_info.set_msg_type(message::Message_type::eGround_detect_request_msg);
    t_base_info.set_timeout_ms(4000);
    t_base_info.set_sender(message::Communicator::eEmpty);
    t_base_info.set_receiver(message::Communicator::eGround_measurer);
    t_ground_detect_request.mutable_base_info()->CopyFrom(t_base_info);
    t_ground_detect_request.set_command_key("key for test only");
    t_ground_detect_request.mutable_id_struct()->set_terminal_id(0);

    usleep(1000 * 1000);
    // std::cout << "try to send" << std::endl;
    // socket.send(t_ground_detect_request.SerializeAsString());
    // usleep(1000 * 100);

    while (true)
    {

        // for (size_t i = 0; i < 10; i++)
        // {
            // 接收消息
            std::string t_receive_string = socket.recv<std::string>(1);
            if (t_receive_string.length() > 0)
            {
                bool result = base_msg.ParseFromString(t_receive_string);
                // 接收并打印车位状态信息
                std::cout << "====check result====================" << result << "============================" << std::endl;
                std::cout << "cycle " << n << std::endl;
                if (base_msg.base_info().msg_type() == message::Message_type::eGround_detect_response_msg)
                {
                    message::Ground_detect_response_msg t_ground_detect_response;
                    std::cout << "----------------- check result -----------------" << std::endl;
                    t_ground_detect_response.ParseFromString(t_receive_string);
                    std::cout << "result: "<<t_ground_detect_response.DebugString()<< std::endl;
                    // std::cout << sift_response.DebugString() << std::endl;
                    // continue;
                }else if(base_msg.base_info().msg_type() == message::Message_type::eGround_status_msg)
                {
                    message::Ground_status_msg t_status_msg;
                    std::cout << "----------------- heartbeat -----------------" << std::endl;
                    t_status_msg.ParseFromString(t_receive_string);
                    std::cout << "heartbeat: "<<t_status_msg.DebugString()<< std::endl;
                }
            }
        // }

        // std::this_thread::yield();
        n++;
        usleep(1000 * 1);
    }
}

int main()
{
    	const char* logPath = "./log/";
	google::InitGoogleLogging("LidarMeasurement");
	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;


    // velo_driver_test();
    // device_test();
    // region_detect();
    // velo_manager_test();
    message_test();

    return 0;
}