puai_wj_2021/tests/vlp16_driver_test.cpp

/*
 * @Description: vlp16驱动测试
 * @Author: yct
 * @Date: 2021-07-26 14:11:26
 * @LastEditTime: 2021-11-23 15:38:49
 * @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"

#include "../laser/Laser.h"
#include <livox_sdk.h>
bool g_exit = false;
void region_detect()
{
    // ************ region detect algorithm time counting  ****************
    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;

        // ************ add livox sampling func test ****************

        auto sampling_func = [=]()
        {
            Error_manager code;
            //读laser配置
            Laser_proto::Laser_parameter_all laser_parameters;
            if (!proto_tool::read_proto_param("../setting/laser.prototxt", laser_parameters))
            {
                LOG(ERROR) << "read laser parameter failed";
                return;
            }

            int laser_cout = laser_parameters.laser_parameters_size();
            if (laser_cout == 0)
            {
                LOG(ERROR) << " no lidars ";
                return;
            }
            std::vector<Laser_base *> plasers;
            plasers.resize(laser_cout);
            for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
            {
                plasers[i] = LaserRegistory::CreateLaser(laser_parameters.laser_parameters(i).type(), i,
                                                         laser_parameters.laser_parameters(i));
                if (plasers[i] != NULL)
                {
                    if (plasers[i]->connect_laser() != SUCCESS)
                    {
                        char description[255] = {0};
                        sprintf(description, "Laser %d connect failed...", i);
                        LOG(ERROR) << description;
                    }
                }
            }
            for (int i = 0; i < laser_parameters.laser_parameters_size(); ++i)
            {
                std::string type = laser_parameters.laser_parameters(i).type();
                if (type.find("Livox") != type.npos || type.find("Horizon") != type.npos)
                {
                    if (Start() == false)
                    {
                        Uninit();
                        LOG(ERROR) << "Livox laser init failed...";
                        return;
                    }
                    break;
                }
            }

            usleep(3000 * 1000);

            LOG(INFO) << "init ok...";

            //采集
            int cycles = 360000;
            int frames = 0;
            while (cycles-->0)
            {
                auto t_start_point = std::chrono::system_clock::now();

                pcl::PointCloud<pcl::PointXYZ> scan_cloud;
                std::mutex cloud_lock;
                std::vector<Laser_task *> laser_task_vector;
                for (int i = 0; i < plasers.size(); ++i)
                {
                    int frame_count = laser_parameters.laser_parameters(i).frame_num();
                    if (plasers[i] != nullptr)
                    {

                        //创建扫描任务,
                        Laser_task *laser_task = new Laser_task();
                        //
                        laser_task->task_init(TASK_CREATED, &scan_cloud, &cloud_lock);
                        laser_task->set_task_frame_maxnum(frame_count);
                        laser_task_vector.push_back(laser_task);
                        //发送任务单给雷达
                        code = plasers[i]->execute_task(laser_task);
                        if (code != SUCCESS)
                        {
                            LOG(ERROR) << " capture failed :" << code.get_error_description();
                            break;
                        }
                    }
                }
                if(code != SUCCESS)
                {
                    for (int i = 0; i < laser_task_vector.size(); ++i)
                    {
                        if (laser_task_vector[i] != 0)
                        {
                            delete laser_task_vector[i];
                            laser_task_vector[i] = NULL;
                        }
                    }
                    usleep(500 * 1000);
                    continue;
                }

                //等待雷达完成任务单
                double second = 0.0;
                while (1)
                {
                    //判断是否强制退出
                    //判断雷达任务单是否全部完成
                    bool tb_laser_complete = true;
                    for (int i = 0; i < laser_task_vector.size(); ++i)
                    {
                        tb_laser_complete &= (TASK_OVER == laser_task_vector[i]->get_task_statu());
                    }
                    if (tb_laser_complete)
                    {
                        break;
                    }
                    //计算扫描时间,若超时,并且没有点,则返回错误.
                    auto t_end_point = std::chrono::system_clock::now();
                    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_point - t_start_point);
                    second = double(duration.count()) *
                             std::chrono::milliseconds::period::num / std::chrono::milliseconds::period::den;
                    if (second > 0.5)
                    {
                        LOG(WARNING) << "capture time out";
                        break;
                    }
                    usleep(1000 * 10);
                }
                //检查雷达任务完成情况,是否是正常完成

                //释放扫描任务单
                for (int i = 0; i < laser_task_vector.size(); ++i)
                {
                    if (laser_task_vector[i] != 0)
                    {
                        delete laser_task_vector[i];
                        laser_task_vector[i] = NULL;
                    }
                }
                LOG(INFO) << " frame :" << ++frames << ", cloud size: " << scan_cloud.size()
                          << " , time:" << second<<" sec";
                usleep(200 * 1000);
            }
        };
        std::thread *sampling_thread = new std::thread(sampling_func);
        sampling_thread->detach();

        // 手动检测，测试计算耗时
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
        cv::RNG rnd(std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now().time_since_epoch()).count());
        for (size_t i = 0; i < 50000; i++)
        {
            temp_cloud->clear();
            temp_cloud->operator+=(*cloud_exce);
            // 添加随机变换
            Eigen::Vector2d trans_offset(rnd.uniform(-0.05, 0.05), rnd.uniform(-0.05, 0.05));
            Eigen::Rotation2Dd rot(rnd.uniform(-4 * M_PI / 180.0, 4 * M_PI / 180.0));
            for (size_t i = 0; i < temp_cloud->size(); i++)
            {
                Eigen::Vector2d t_point(temp_cloud->points[i].x, temp_cloud->points[i].y);
                t_point = rot.toRotationMatrix() * t_point + trans_offset;
                temp_cloud->points[i].x = t_point.x();
                temp_cloud->points[i].y = t_point.y();
            }

            detect_wheel_ceres3d::Detect_result t_result;
            Error_manager ec = t_region.detect(temp_cloud, t_result);
            if(ec!=SUCCESS) 
                LOG(ERROR) << ec.to_string();
            usleep(1000*10);

            // 单轮中仅随机变换一次
            temp_cloud->clear();
            temp_cloud->operator+=(*cloud_envi);
            for (size_t i = 0; i < temp_cloud->size(); i++)
            {
                Eigen::Vector2d t_point(temp_cloud->points[i].x, temp_cloud->points[i].y);
                t_point = rot.toRotationMatrix() * t_point + trans_offset;
                temp_cloud->points[i].x = t_point.x();
                temp_cloud->points[i].y = t_point.y();
            }
            ec = t_region.detect(temp_cloud, t_result);
            if(ec!=SUCCESS) 
                LOG(ERROR) << ec.to_string();
            usleep(1000*10);
        }

        g_exit = true;
        if(sampling_thread!=nullptr)
        {
            if(sampling_thread->joinable()){
                sampling_thread->join();
            }
            delete sampling_thread;
            sampling_thread = nullptr;
        }

        // 检测自动识别功能
        // 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.2.113: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.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;


    // ************ add livox sampling preparation ****************

    // velo_driver_test();
    // device_test();
    region_detect();
    // velo_manager_test();
    // message_test();
    g_exit = true;
    getchar();
    return 0;
}