LidarMeasure/test/wj_lidar_test.cpp

// #include "../wj_lidar/wj_lidar_encapsulation.h"
#include "../wj_lidar/fence_controller.h"
// #include <pcl/visualization/cloud_viewer.h>
#include <fstream>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <boost/filesystem.hpp>

/**
 * @description: 递归读取文件夹下所有文件
 * @param path: 待遍历文件夹名
 * @param filename_list: 存放获取文件名的数组
 * @return: 是否获取到文件名
 */
bool recursive_file_read(std::string path, std::vector<std::string> &filename_list)
{
    filename_list.clear();
    boost::filesystem::path myPath(path);
    boost::filesystem::recursive_directory_iterator endIter;
    int file_count = 0;
    for (boost::filesystem::recursive_directory_iterator iter(myPath); iter != endIter; iter++)
    {
        if (!boost::filesystem::is_directory(*iter))
        {
            filename_list.push_back(iter->path().string());
            file_count++;
        }
    }
    
    if(file_count > 0)
        return true;
    else
        return false;
}

void create_mark(double cx, double cy, pcl::PointCloud<pcl::PointXYZ>::Ptr &mark_cloud)
{
    int width = 10;
    int length = 25;
    double resolution = 0.01;
    // mark_cloud->clear();
    for (int i = 0; i < width; i++)
    {
        pcl::PointXYZ t_point = pcl::PointXYZ((i - width / 2.0)*resolution + cx, cy, -1);
        mark_cloud->push_back(t_point);
    }
    for (int i = 0; i < length; i++)
    {
        pcl::PointXYZ t_point = pcl::PointXYZ(cx, (i - length / 2.0)*resolution + cy, -1);
        mark_cloud->push_back(t_point);
    }
    // std::cout<<"cx cy: "<<cx<<", "<<cy<<std::endl;
}

void save_cloud_txt(std::string txt, pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud)
{
    std::ofstream os;
    os.open(std::string("./").append(txt), std::ios::out);
    if(!os.is_open())
    {
        std::cout << "write, failed to open " << txt << std::endl;
        return;
    }    
    for (int i = 0; i < pCloud->points.size(); i++)
    {
        pcl::PointXYZ point = pCloud->points[i];
        char buf[255];
        memset(buf, 0, 255);
        sprintf(buf, "%f %f %f\n", point.x, point.y, point.z);
        os.write(buf, strlen(buf));
    }
    char cwd[255];
    getcwd(cwd, 255);
    std::cout << "file write to " << cwd <<"/"<< txt << std::endl;
    os.close();
}

bool read_pointcloud(std::string filename, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
    // std::lock_guard<std::mutex> lck(m_cloud_mutex);
    pointcloud->clear();
    std::ifstream in(filename);
    if (!in.is_open())
    {
        std::cout << "failed to open file " << filename << std::endl;
        return false;
    }
    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: " << filename << std::endl;
                return false;
            }
            pcl::PointXYZ t_cloud;
            t_cloud.x = stod(str_vector[0]);
            t_cloud.y = stod(str_vector[1]);
            t_cloud.z = stod(str_vector[2]);
            pointcloud->push_back(t_cloud);
        }
    }
    in.close();
    std::cout << "file read finished with points " << pointcloud->size() << std::endl;
    return true;
}

bool ReadProtoParam(std::string path, wj::wjLidarParams &params)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &params);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

bool ReadProtoParam(std::string path, wj::wjManagerParams &params)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &params);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

bool ReadProtoParam(std::string path, Hardware_limit::Hardware_parameter &params)
{
    int fd = open(path.c_str(), O_RDONLY);
    if (fd == -1)
        return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &params);
    // std::cout<<m_global_param.data_path()<<std::endl;
    delete input;
    close(fd);
    return success;
}

void lidar_test()
{
    Wj_lidar_encapsulation *lidar = new Wj_lidar_encapsulation();
    wj::wjLidarParams params;
    bool result = ReadProtoParam("../src/test/wj_lidar_settings.prototxt", params);
    LOG(INFO) << "读配置结果" << result;
    // params.mutable_net_config()->set_ip_address("192.168.0.2");
    // params.mutable_net_config()->set_port(2110);
    if (lidar != 0)
    {
        Error_manager code = lidar->initialize(params);
        LOG(INFO) << code.to_string();
        // usleep(1000 * 1);
        if (code == SUCCESS)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            for (int i = 0; i < 10; i++)
            {
                LOG(INFO) << "connect status: " << lidar->get_connection_status();
                code = lidar->get_cloud(cloud, std::chrono::steady_clock::now(), 3000);
                if (code == SUCCESS)
                {
                    char temp[100];
                    memset(temp, 0, 100);
                    sprintf(temp, "cloud_%d.txt", i);
                    if (cloud->size() > 0)
                    {
                        save_cloud_txt(temp, cloud);
                        LOG(INFO) << "cloud saved to " << std::string(temp);
                    }
                }
                else
                {
                    LOG(WARNING) << code.to_string();
                }
                usleep(1000 * 30);
                LOG(INFO) << "end of cycle";
            }
        }
    }
    delete lidar;
}
void measure_task(std::vector<Region_worker *> workers, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Wj_lidar_Task* task);

void all_lidar_test()
{
    std::string manager_path = "setting/wj_manager_settings.prototxt";
    std::string hardware_path = "setting/limit.prototxt";
    std::string cloud_name = "/home/youchen/data/puai_sample200624/fence_wj/combined/conb.txt";
    wj::wjManagerParams params;
    Hardware_limit::Hardware_parameter hardware_parameter;
    bool result = ReadProtoParam(manager_path, params);
    result &= ReadProtoParam(hardware_path, hardware_parameter);

    if(!result){
        LOG(INFO) << "读配置结果" << result;
        return;
    }

    // init lidar instances
    std::vector<Wj_lidar_encapsulation *> lidars_vector;
    for (int i = 0; i < params.wj_lidar_size(); ++i)
    {
        lidars_vector.push_back(new Wj_lidar_encapsulation());
        Error_manager code = lidars_vector[i]->initialize(params.wj_lidar(i));
        // 错误码信息已在内部打印
        if (code != SUCCESS)
        {
            LOG(WARNING) << Error_manager(Error_code::WJ_MANAGER_UNINITIALIZED,NORMAL,"万集雷达连接失败").to_string();
            return ;
        }
    }

    for (size_t j = 0; j < 20000; j++)
    {
        LOG(INFO) << "\ncycle " << std::to_string(j);
        pcl::PointCloud<pcl::PointXYZ>::Ptr total_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        for (int i = 0; i < lidars_vector.size(); ++i)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
            if (lidars_vector[i]->get_cloud(cloud, std::chrono::steady_clock::now()) == SUCCESS)
            {
                total_cloud->operator+=(*cloud);
            }
        }
        LOG(INFO) << "--------callback get cloud, size: " << total_cloud->size();
    }
}

void detect_test()
{
    std::string manager_path = "setting/wj_manager_settings.prototxt";
    std::string hardware_path = "setting/limit.prototxt";
    std::string cloud_directory_name = "/home/youchen/Documents/measure/MainStructure/elecfence_ws/cloud";
    std::vector<std::string> cloud_filename_strs;
    wj::wjManagerParams params;
    Hardware_limit::Hardware_parameter hardware_parameter;
    bool result = ReadProtoParam(manager_path, params);
    result &= ReadProtoParam(hardware_path, hardware_parameter);
    result &= recursive_file_read(cloud_directory_name, cloud_filename_strs);

    if(!result){
        LOG(INFO) << "读配置结果" << result;
        return;
    }
    //创建limit模块
    Verify_result*  p_verify_result_tool=new Verify_result(hardware_parameter);
    std::vector<Region_worker *> region_workers_vector;
    
    // init region_detector instances
    for (int j = 0; j < params.regions_size(); ++j)
    {
        region_workers_vector.push_back(new Region_worker(j, params.regions(j), p_verify_result_tool));
    }
    LOG(INFO) << "created wj region workers";
    for (size_t i = 0; i < cloud_filename_strs.size(); i++)
    {
        LOG(INFO) << "\ncycle "<<std::to_string(i); 
        for (int j = 3; j < 4; j++)
        {
            pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            bool read_result = read_pointcloud(cloud_filename_strs[i], cloud);
            if (!read_result)
            {
                LOG(WARNING) << "read cloud error";
                return;
            }
            Wj_lidar_Task* task = new Wj_lidar_Task();
            task->init();
            wj_command command;
            command.terminal_id = 5;//j*2-1;
            command.command_time = std::chrono::steady_clock::now();
            command.timeout_in_milliseconds = 1000;
            task->set_command(command);
            // LOG(WARNING) << "000 " << std::to_string(command.terminal_id);
            measure_task(region_workers_vector, cloud, task);
            // LOG(WARNING) << "2 cloud use count: " <<cloud.use_count();
            delete task;
        }
        usleep(1000*10);
    }
    
    // getchar();
    for (int i = 0; i < region_workers_vector.size(); ++i)
    {
        if(region_workers_vector[i] != nullptr)
        {
            delete region_workers_vector[i];
        }
    }
}

#include <pcl/common/transforms.h>
void measure_task(std::vector<Region_worker *> workers, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Wj_lidar_Task* task)
{
    static int save_count = 0;
    if(task == nullptr)
    {
        LOG(WARNING) << " empty task";
        return;
    }
    wj_command command_temp;
    Error_manager code = task->get_command(command_temp);
    int term_id = command_temp.terminal_id;

    // LOG(INFO) << "measure task entered. " << std::to_string(command_temp.terminal_id);
    // 创建保存结果容器
    wj_measure_result wj_measure_temp;
    wj_measure_temp.terminal_id = term_id;
    wj_measure_temp.correctness = false;

    // LOG(WARNING) << "111 " << std::to_string(workers.size());
    for (int i = 0; i < workers.size(); ++i)
    {
        // LOG(WARNING) << (workers[i] == nullptr);
        if (workers[i] != nullptr && workers[i]->get_id() == term_id)
        {
            char cloud_txt_filename[255];
            memset(cloud_txt_filename, 0, 255);
            bool correctness = false;

            // LOG(INFO) << "--------callback find terminal------" << term_id;
            double x = 0, y = 0, c = 0, wheelbase = 0, width = 0, ftheta;
            if (cloud->size() <= 0)
            {
                LOG(INFO) << "empty cloud";
                return;
            }
            // 获取最新点云并保存到total_cloud中
            // LOG(INFO) << "--------callback get cloud, size: " << cloud->size();

            // 获取检测结果
            std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now();
            Error_manager code = workers[i]->get_wheel_result(cloud, x, y, c, wheelbase, width, ftheta);
            std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
            LOG(INFO) << "solve time: " << std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0).count()<<"";

            // LOG(WARNING) << "1 cloud use count: " <<cloud.use_count();
            time_t tt = time(0);
            struct tm *tc = localtime(&tt);
            char buf[255] = {0};
            memset(buf, 0, 255);
            // sprintf(buf, "%d-%02d-%02d_%02d:%02d:%02d", tc->tm_year + 1900,
            //         tc->tm_mon + 1, tc->tm_mday, tc->tm_hour, tc->tm_min, tc->tm_sec);
            sprintf(buf, "%03d", save_count);
            save_count++;
            // 根据是否成功生成对应文件名
            if (code == SUCCESS)
            {
                correctness = true;
                sprintf(cloud_txt_filename, "./result/%s_%d.txt", buf, term_id);
            }
            else
            {
                // LOG(WARNING) << "get wheel result failed: " << code.to_string();
                correctness = false;
                sprintf(cloud_txt_filename, "./result/%s%s_%d.txt", "failed_", buf, term_id);
            }

            // save cloud txt
            // 存入中心点与四轮中心(左前，左后，右前，右后)
            Eigen::Affine3f transform = Eigen::Affine3f::Identity();
            transform.translation() << x, y, 0.0;
            transform.rotate(Eigen::AngleAxisf(c/180.0*M_PI - M_PI_2, Eigen::Vector3f::UnitZ()));

            pcl::PointCloud<pcl::PointXYZ>::Ptr t_total_append_cloud=pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::PointCloud<pcl::PointXYZ>::Ptr t_append_cloud=pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
            create_mark(-(width-0.15)/2.0, -wheelbase/2.0, t_append_cloud);
            create_mark(-(width-0.15)/2.0, +wheelbase/2.0, t_append_cloud);
            create_mark(+(width-0.15)/2.0, -wheelbase/2.0, t_append_cloud);
            create_mark(+(width-0.15)/2.0, +wheelbase/2.0, t_append_cloud);
            pcl::PointXYZ center(0, 0, 0);
            t_append_cloud->push_back(center);
            pcl::transformPointCloud(*t_append_cloud, *t_append_cloud, transform);

            t_total_append_cloud->operator+=(*t_append_cloud);

            save_cloud_txt(cloud_txt_filename, (cloud->operator+=(*t_append_cloud)).makeShared());

            wj_measure_temp.x = x * 1000.0;
            wj_measure_temp.y = y * 1000.0;
            wj_measure_temp.angle = c;
            wj_measure_temp.wheel_base = wheelbase * 1000.0;
            wj_measure_temp.width = width * 1000.0;
            wj_measure_temp.correctness = correctness;
            if (wj_measure_temp.correctness)
            {
                task->set_result(wj_measure_temp);
                char description[255] = {0};
                sprintf(description, "WJ center(%.1f, %.1f) size:(%.1f, %.1f)  angle:%.2f front_ang:%.2f",
                        x * 1000.0, y * 1000.0, wheelbase * 1000.0, width * 1000.0, c, ftheta);
                LOG(INFO) << description;
                return;
            }else
            {
                LOG(INFO) << "term: "+ std::to_string(term_id) +" measure failed.";
            }
            LOG(INFO) << "==================================================";
        }
    }
    
}

int main(int argc, char *argv[])
{
    // lidar_test();
    // usleep(1000 * 1000);

    // lidar_test();
    // usleep(1000 * 1000);

    // lidar_test();
    // usleep(1000 * 1000);
    // std::cout<<std::hex<<-2146828280<<std::endl;
    detect_test();

    LOG(INFO) << "end";
    // getchar();
    return 0;
}