Measure/Modules/MeasureNodeAlgTest/test.cpp

//
// Created by zx on 2023/10/23.
//

#include "test.h"

int main(int argc, char** argv) {
    ZX::InitGlog("MeasureNodeAlgTest", ETC_PATH"MeasureNodeAlgTest/MeasureNodeAlgTestLog/");

    const char *VELODYNE_MANAGER_PARAMETER_PATH = ETC_PATH"MeasureNode/velodyne_manager.prototxt";
    const char *LEFT_WHEEL_MODEL = ETC_PATH"MeasureNode/left_model.txt";
    const char *RIGHT_WHEEL_MODEL = ETC_PATH"MeasureNode/right_model.txt";

    // 初始化点云获取通信
    CloudDataMqttAsync::instance();

    // 加载区域配置
    velodyne::velodyneManagerParams t_velo_params;
    if (!proto_tool::read_proto_param(VELODYNE_MANAGER_PARAMETER_PATH, t_velo_params)) {
        LOG(ERROR) << "velodyne_manager read_proto_param  failed form " << VELODYNE_MANAGER_PARAMETER_PATH;
        return EXIT_FAILURE;
    }
    LOG(ERROR) << "velodyne_manager read_proto_param  failed form " << VELODYNE_MANAGER_PARAMETER_PATH;

    // 加载模型
    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(LEFT_WHEEL_MODEL, left)) {
        LOG(ERROR) << "cannot read left model form " << LEFT_WHEEL_MODEL;
        return EXIT_FAILURE;
    }
    if (!read_pointcloud(RIGHT_WHEEL_MODEL, right)) {
        LOG(ERROR) << "cannot read right model form " << RIGHT_WHEEL_MODEL;
        return EXIT_FAILURE;
    }

    std::map<int, GroundRegion *> mp_region;
    for (auto &region: t_velo_params.region()) {
        mp_region.insert(std::pair<int, GroundRegion *>(region.region_id(), new GroundRegion()));
        mp_region.find(region.region_id())->second->init(region, left, right);
    }

    while (true) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    return EXIT_SUCCESS;
}