puai_wj_2021/tests/vlp16_driver_test.cpp

/*
 * @Description: vlp16驱动测试
 * @Author: yct
 * @Date: 2021-07-26 14:11:26
 * @LastEditTime: 2021-07-28 10:14:20
 * @LastEditors: yct
 */

#include <iostream>
#include <fstream>

#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设备并获取点云
#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");

    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;
    }
}

int main()
{
    // velo_driver_test();
    device_test();

    return 0;
}