Vzense-Tof-3D/detect/detect_manager.cpp

//
// Created by zx on 2023/11/24.
//

#include "detect_manager.h"

Error_manager DetectManager::Init() {
    m_thread = new std::thread(&DetectManager::run, this);
    m_condit.notify_all(true);

    return {};
}

Error_manager DetectManager::Release() {
    stop();
    return {};
}

void DetectManager::run() {
    LOG(INFO) << "detect thread running in " << std::this_thread::get_id();

    DetectResult m_detect_result_record;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vct_wheel_cloud;
    for (int i = 0; i < DeviceAzimuth::DEVICE_AZIMUTH_MAX; i++) {
        vct_wheel_cloud.emplace_back(new pcl::PointCloud<pcl::PointXYZ>);
    }

    auto t_start_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;

    while (m_condit.is_alive()) {
        m_condit.wait();
        // 参数初始化
        cost = std::chrono::steady_clock::now() - t_start_time;
        std::this_thread::sleep_for(std::chrono::milliseconds(100 - std::min<int>(99, cost.count() * 1000)));
        t_start_time = std::chrono::steady_clock::now();

        if (m_condit.is_alive()) {
            std::vector<int> have_cloud;
            DetectResult detect_result;
            JetStream::LabelYolo seg_results;
            if (!LabelYoloDataBuffer::iter()->lock_exe(&LabelYoloDataBuffer::get, seg_results, 10)) {
                continue;
            }
            LOG(INFO) << seg_results.label() << " " << seg_results.boxes(0).lines_size();
            for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                if (seg_results.boxes(device_index).lines().empty()) {
                    LOG(INFO) << device_index << " empty";
                    continue;
                }
                cv::Mat pointMat;
                LOG(INFO) << "b get data" << " empty";
                if (MatDataBuffer::iter()->lock_exe(&MatDataBuffer::get, device_index, seg_results.label(), pointMat, 10)) {
                    cv::imshow(std::to_string(device_index), pointMat);
                    cv::waitKey(1);
                }
                LOG(INFO) << "e get data" << " empty";
                continue;
                have_cloud.push_back(device_index);
                for (int row = 0; row < 480; row++) {
                    for (int col = seg_results.boxes(device_index).lines(row).begin(); col < seg_results.boxes(device_index).lines(row).end(); col++) {
                        vct_wheel_cloud[device_index]->emplace_back(pointMat.at<cv::Vec3f>(row, col)[0],
                                                                    pointMat.at<cv::Vec3f>(row, col)[1],
                                                                    pointMat.at<cv::Vec3f>(row, col)[2]);
                    }
                }
            }
            continue;

            // 点云处理
            if (!have_cloud.empty()) {
                WheelCloudOptimize(vct_wheel_cloud, detect_result);
                cost = std::chrono::steady_clock::now() - t_start_time;
                DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";

                bool ceres_detect_ret = false;
                auto wheel_detect = detect_wheel_ceres::iter();
                // 采用上次测量结果的数据
                if (!m_detect_result_time_lock_data.timeout(0.5)) {
                    detect_result = m_detect_result_time_lock_data();
                    detect_result.car.wheel_width = MIN(detect_result.car.wheel_width + 0.4, 2.5);
                } else {
                    detect_result.car.wheel_width = 3;
                    detect_result.car.wheel_base = 2.7;
                }

                // TODO：测试功能
                wheel_detect->detect_fall(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                          vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                          detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
                                          detect_result.car.theta, detect_result.car.wheel_base,
                                          detect_result.car.wheel_width,detect_result.car.front_wheels_theta, detect_result.car.loss);
                // 根据车轮数采用不同的优化方式
                if (m_detect_result_record.car.wheel_width < 1.0 || m_detect_result_record.car.wheel_base < 2.0 ||
                        have_cloud.size() > 2) {
                    ceres_detect_ret = wheel_detect->detect_2Step(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                    vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                    detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
                                    detect_result.car.theta, detect_result.car.wheel_base,
                                    detect_result.car.wheel_width,detect_result.car.front_wheels_theta, detect_result.car.loss);
                    if ((m_detect_result_record.car.wheel_width < 1.0 && m_detect_result_record.car.wheel_base < 1.0 && have_cloud.size() == 2) || have_cloud.size() < 2) {
                        detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
                        detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
                    } else if (have_cloud.size() == 2) {
                        for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
                            if (!vct_wheel_cloud[device_index]->empty()) {
                                have_cloud.push_back(device_index);
                            }
                        }

                        if (have_cloud[0] % 2 == have_cloud[1] %2 ) {
                            detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
                        } else {
                            detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
                        }
                    }
                } else {
                    ceres_detect_ret = wheel_detect->detect_dynP(vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LF], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RF],
                                         vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_LR], vct_wheel_cloud[DeviceAzimuth::DEVICE_AZIMUTH_RR],
                                         detect_result.car.wheels_center_x, detect_result.car.wheels_center_y,
                                         detect_result.car.theta, m_detect_result_record.car.wheel_base,
                                         m_detect_result_record.car.wheel_width,detect_result.car.front_wheels_theta, detect_result.car.loss);
                    detect_result.car.wheel_base = m_detect_result_record.car.wheel_base;
                    detect_result.car.wheel_width = m_detect_result_record.car.wheel_width;
                }

                // 如果车辆运动，不断刷新运动时间，如果由运动转为静止则刷新运动状态到静止
                float move_distance = detect_result.car.wheels_center_y - m_detect_result_record.car.wheels_center_y;
                if (fabs(move_distance) > 0.1) {
                    move_statu = true;
                } else if (move_statu()) {
                    move_statu = false;
                }

                if (ceres_detect_ret) {
                    // 记录下测量结果
                    m_detect_result_record = detect_result;
                    detect_statu = Measure_OK;
                    m_detect_result_time_lock_data = m_detect_result_record;
                } else {
                    detect_statu = Measure_Failture;
                }

            }
            cost = std::chrono::steady_clock::now() - t_start_time;
            DLOG(INFO) << "cost time is " << cost.count() * 1000 << " ms";
        } else {
            cv::destroyAllWindows();
        }
    }
    LOG(INFO) << "detect thread " << std::this_thread::get_id() << " closed.";
}

void DetectManager::stop() {
    //杀死线程,
    m_condit.kill_all();

    //在线程join结束之后, 就可以可以回收线程内存资源
    m_thread->join();
    delete m_thread;
    m_thread = nullptr;
}

Error_manager DetectManager::WheelCloudOptimize(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> &vct_wheel_cloud, DetectResult &result) {
    //下采样
    pcl::VoxelGrid<pcl::PointXYZ> vox;                     //创建滤波对象
    vox.setLeafSize(0.03f, 0.03f, 0.03f);     //设置滤波时创建的体素体积为1cm的立方体
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (int device_index = 0; device_index < DeviceAzimuth::DEVICE_AZIMUTH_MAX; device_index++) {
        if (vct_wheel_cloud[device_index]->empty()) { continue;}

        vox.setInputCloud(vct_wheel_cloud[device_index]);                   //设置需要过滤的点云给滤波对象
        vox.filter(*vct_wheel_cloud[device_index]);                         //执行滤波处理，存储输出
    }

    return {};
}

MeasureStatu DetectManager::getMeasureResult(DetectManager::DetectResult &result) {
    if (detect_statu() != MeasureStatu::Measure_OK) {
        return detect_statu();
    }
    if (m_detect_result_time_lock_data.timeout()) {
        return MeasureStatu::Measure_Failture;
    }
    result = m_detect_result_time_lock_data();
    return MeasureStatu::Measure_OK;
}

Error_manager DetectManager::carMoveStatus() {
    if (move_statu.timeout(3) && !move_statu()) {
        return {FAILED, NORMAL};
    }
    return {};
}