puai_wj_2021/tests/vlp16_driver_test.cpp

/*
 * @Description: vlp16驱动测试
 * @Author: yct
 * @Date: 2021-07-26 14:11:26
 * @LastEditTime: 2021-08-01 15:51:10
 * @LastEditors: yct
 */

#include <iostream>
#include <fstream>

// velodyne driver seperation test
#include "../velodyne_lidar/velodyne_driver/input.h"
#include "../velodyne_lidar/velodyne_driver/rawdata.h"
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"
void region_detect()
{
    std::ifstream in("/home/youchen/extra_space/chutian/measure/puai_wj_2021/build/comb_calib.txt");
    if(!in.is_open())
    {
        std::cout << "cannot open file to read cloud." << std::endl;
        return;
    }
    else
    {
        // 准备点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr t_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        while(!in.eof())
        {
            std::string t_linestr;
            if (getline(in, t_linestr))
            {
                std::vector<std::string> str_vector;
                std::stringstream ss(t_linestr);
                std::string num_str;
                while (getline(ss, num_str, ' '))
                {
                    str_vector.push_back(num_str);
                }
                if (str_vector.size() != 3)
                {
                    std::cout << "unsupported point cloud / cannot find point x y z value " << std::endl;
                    return;
                }
                pcl::PointXYZ t_point;
                t_point.x = stod(str_vector[0]);
                t_point.y = stod(str_vector[1]);
                t_point.z = stod(str_vector[2]);
                t_cloud->push_back(t_point);
            }
        }
        // 初始化region
        velodyne::Region param;
        param.set_minx(-3.10);
        param.set_maxx(0);
        param.set_miny(-2.75);
        param.set_maxy(3.08);
        param.set_minz(0.01);
        param.set_maxz(0.2);
        param.set_region_id(0);
        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("/home/youchen/extra_space/chutian/measure/puai_wj_2021/setting/left_model.txt", left) || !read_pointcloud("/home/youchen/extra_space/chutian/measure/puai_wj_2021/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功能
        detect_wheel_ceres3d::Detect_result t_result;
        Error_manager ec = t_region.detect(t_cloud, t_result);
        std::cout << ec.to_string() << std::endl;
        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()
{
    // velo_driver_test();
    // device_test();
    // region_detect();
    // velo_manager_test();
    message_test();

    return 0;
}