Vzense-Tof-3D/communication/communication_manager.cpp

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

#include "communication_manager.h"

Error_manager CommunicationManager::Init(const CommunicationManagerConfig &config) {
    m_config = config;
    if (config.grpc_enable()) {
        m_grpc_server = new StreamRpcServer;
        m_grpc_server->Listenning(config.grpc_server_ip(), 9876);
    }

    if (config.rabbitmq_enable()) {
        std::string file_path = ETC_PATH PROJECT_NAME + config.rabbitmq_config_file();
        RabbitmqCommunicationTof3D::get_instance_pointer()->rabbitmq_init_from_protobuf(file_path);
    }

    if (config.mqtt_enable()) {
        std::string file_path = ETC_PATH PROJECT_NAME + config.mqtt_config_file();
        Tof3DMqttAsyncClient::iter()->init(file_path);
    }

//    offset.CopyFrom(config.offset());
//    out_info.CopyFrom(config.outof());

    condition = new Thread_condition();
    run_t = new std::thread(&CommunicationManager::run, this);
    condition->notify_all(true);

    return {};
}

void CommunicationManager::run() {
    auto t_start_time = std::chrono::steady_clock::now();
    std::chrono::duration<double> cost = std::chrono::steady_clock::now() - t_start_time;
    while (condition->is_alive()) {
        condition->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 (condition->is_alive()) {
            if (++heart > 999) {
                heart = 0;
            }

            DetectManager::DetectResult detect_measure_info;

            MeasureStatu measure_status = getMeasureData(detect_measure_info);

            detect_measure_info.car.uniform_x = detect_measure_info.car.wheels_center_x * cos(detect_measure_info.car.theta) - detect_measure_info.car.wheels_center_y * sin(detect_measure_info.car.theta);
            detect_measure_info.car.uniform_y = detect_measure_info.car.wheels_center_x * sin(detect_measure_info.car.theta) + detect_measure_info.car.wheels_center_y * cos(detect_measure_info.car.theta);

            if (m_config.grpc_enable()) {
                sendGrpcData(measure_status, detect_measure_info);
            }

            if (m_config.rabbitmq_enable()) {
                sendRabbitmqData(measure_status, detect_measure_info);
            }
        }
    }
}

MeasureStatu CommunicationManager::getMeasureData(DetectManager::DetectResult &detect_measure_info) {
    // 设备状态
    if (DeviceTof3D::iter()->getDeviceStatus() != SUCCESS) {
        return MeasureStatu::Lidar_Disconnect;
    }

    return DetectManager::iter()->getMeasureResult(detect_measure_info);
}

Error_manager CommunicationManager::sendGrpcData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result) {
    // 保存grpc数据中间块
    TransitData::MeasureInfo grpc_measure_info;
    grpc_measure_info.x = measure_result.car.wheels_center_x;
    grpc_measure_info.y = measure_result.car.wheels_center_y;
    grpc_measure_info.transx = measure_result.car.uniform_x;
    grpc_measure_info.transy = measure_result.car.uniform_y;
    grpc_measure_info.theta = measure_result.car.theta;
    grpc_measure_info.width = measure_result.car.wheel_width;
    grpc_measure_info.wheelbase = measure_result.car.wheel_base;
    grpc_measure_info.ftheta = measure_result.car.front_wheels_theta;

    addOffset(measure_result);
    grpc_measure_info.border_plc = outOfRangeDetection(measure_statu, measure_result, true);
    grpc_measure_info.border_display = outOfRangeDetection(measure_statu, measure_result, false);
    removeOffset(measure_result);

    grpc_measure_info.error.append("heart: " + std::to_string(heart));
    TransitData::get_instance_pointer()->setMeasureInfo(grpc_measure_info);

    return Error_manager();
}

Error_manager CommunicationManager::sendMqttData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result) {
    return Error_manager();
}

Error_manager CommunicationManager::sendRabbitmqData(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result) {
    // 发送rabbitmq消息
    measure_info info;
    measure_buffer rabbit_measure_info;

    info.set_ground_status(measure_statu);
    // 如果地面雷达状态已经被修改过，直接返回
    if (info.ground_status() == MeasureStatu::Measure_OK || info.ground_status() == MeasureStatu::Measure_Border) {
        addOffset(measure_result);
        auto out_of_statu = outOfRangeDetection(measure_statu, measure_result, true);
        if (out_of_statu != Range_status::Range_correct) {
            info.set_border_statu(out_of_statu);
            info.set_ground_status(MeasureStatu::Measure_Border);
        }
        info.set_cx(measure_result.car.uniform_x);
        info.set_cy(measure_result.car.uniform_y);
        info.set_theta(-measure_result.car.theta * 180.0 / M_PI);
        info.set_length(0);
        info.set_width(measure_result.car.wheel_width);
        info.set_wheelbase(measure_result.car.wheel_base);
        info.set_front_theta(measure_result.car.front_wheels_theta * 180.0 / M_PI);
        if (DetectManager::iter()->carMoveStatus() == FAILED) {
            info.set_motion_statu(1);    // 静止3秒
            info.set_is_stop(1);
        } else {
            info.set_motion_statu(0);    // 运动
            info.set_is_stop(0);
        }
        info.set_move_distance(measure_result.car.forward_dis);
    }

    rabbit_measure_info.mutable_measure_info_to_plc_forward()->CopyFrom(info);
    rabbit_measure_info.mutable_measure_info_to_plc_reverse()->CopyFrom(info);
    rabbit_measure_info.mutable_measure_info_to_terminal()->CopyFrom(info);
    if (info.ground_status() == MeasureStatu::Measure_OK || info.ground_status() == MeasureStatu::Measure_Border) {
        auto out_of_statu = outOfRangeDetection(measure_statu, measure_result, false);
        if (out_of_statu != Range_status::Range_correct) {
            rabbit_measure_info.mutable_measure_info_to_terminal()->set_border_statu(out_of_statu);
            rabbit_measure_info.mutable_measure_info_to_terminal()->set_ground_status(MeasureStatu::Measure_Border);
        }
    }

    RabbitmqCommunicationTof3D::get_instance_pointer()->encapsulate_status_msg(rabbit_measure_info.DebugString(), 0);
    return {};
}

Range_status CommunicationManager::outOfRangeDetection(const MeasureStatu &measure_statu, DetectManager::DetectResult &measure_result, bool isPlc) {
    // 测量给出超界，说明车辆未到达指定区域，需要前进
    if (measure_statu == MeasureStatu::Measure_Border) {
        return Range_status::Range_back;
    }
    if (measure_statu != MeasureStatu::Measure_OK && measure_statu != MeasureStatu::Measure_Border) {
        return Range_status::Range_correct;
    }

    outOfRangeInfo out_info = isPlc ? m_config.plc_out_info() : m_config.display_out_info();
    // 车位位于y负方向, 判断后轮超界情况, 4.80是搬运器大Y的运动极限, 2.7是同侧两个小y夹持杆中心距离
    // 判断车辆轴距超界情况
    if (measure_result.car.wheel_base < out_info.car_min_wheelbase() ||    // 轴距过小
        measure_result.car.wheel_base > out_info.car_max_wheelbase()) {    // 轴距过长
        last_range_statu |= Range_status::Range_car_wheelbase;
    } else if (measure_result.car.wheel_base > out_info.car_min_wheelbase() + 0.01 &&    // 轴距过小
               measure_result.car.wheel_base < out_info.car_max_wheelbase() - 0.01) {
        last_range_statu &= (~Range_status::Range_car_wheelbase);
    }
    if (last_range_statu & Range_status::Range_car_wheelbase) {
        return Range_status::Range_car_wheelbase;
    }

    // 判断车辆宽度超界情况
    if (measure_result.car.wheel_width < out_info.car_min_width() ||   // 车宽过窄
        measure_result.car.wheel_width > out_info.car_max_width()) {   // 车宽过宽
        last_range_statu |= Range_status::Range_car_width;       // 车辆超宽
    } else if (measure_result.car.wheel_width > out_info.car_min_width() + 0.01 &&   // 车宽过窄
               measure_result.car.wheel_width < out_info.car_max_width() - 0.01) {
        last_range_statu &= (~Range_status::Range_car_width);
    }
    if (last_range_statu & Range_status::Range_car_width) {
        return Range_status::Range_car_width;
    }

    // 判断车辆旋转角超界情况
    float theta = -measure_result.car.theta * 180 / M_PI;
    if (theta < out_info.turnplate_angle_limit_min_clockwise()) {
        last_range_statu |= Range_status::Range_angle_clock;
        last_range_statu &= (~Range_status::Range_angle_anti_clock);
    } else if (theta > out_info.turnplate_angle_limit_max_clockwise()) {
        last_range_statu |= Range_status::Range_angle_anti_clock;
        last_range_statu &= (~Range_status::Range_angle_clock);
    } else if (theta > out_info.turnplate_angle_limit_min_clockwise() &&
            theta < out_info.turnplate_angle_limit_max_clockwise()) {
        last_range_statu &= (~Range_status::Range_angle_clock);
        last_range_statu &= (~Range_status::Range_angle_anti_clock);
    }
    if (last_range_statu & Range_status::Range_angle_clock) {
        return Range_status::Range_angle_clock;
    }
    if (last_range_statu & Range_status::Range_angle_anti_clock) {
        return Range_status::Range_angle_anti_clock;
    }

    // 方向盘调正
    float front_wheels_theta = measure_result.car.front_wheels_theta * 180 / M_PI;
    DLOG(INFO) << "front_wheels_theta " << front_wheels_theta;
    if (fabs(front_wheels_theta) > 10) {
        last_range_statu |= Range_status::Range_steering_wheel_nozero;
    } else if (fabs(front_wheels_theta) < 7) {
        last_range_statu &= (~Range_status::Range_steering_wheel_nozero);
    }
    if (last_range_statu & Range_status::Range_steering_wheel_nozero) {
        return Range_status::Range_steering_wheel_nozero;
    }

    // 左右超界
    if (measure_result.car.uniform_x < out_info.border_minx()) {
        last_range_statu |= Range_status::Range_left;
        last_range_statu &= (~Range_status::Range_right);
    } else if (measure_result.car.uniform_x > out_info.border_maxx()) {
        last_range_statu |= Range_status::Range_right;
        last_range_statu &= (~Range_status::Range_left);
    } else if (measure_result.car.uniform_x > out_info.border_minx() + 0.01 &&
               measure_result.car.uniform_x < out_info.border_maxx() - 0.01) {
        last_range_statu &= (~Range_status::Range_left);
        last_range_statu &= (~Range_status::Range_right);
    }
    if (last_range_statu & Range_status::Range_left) {
        return Range_status::Range_left;
    }
    if (last_range_statu & Range_status::Range_right) {
        return Range_status::Range_right;
    }

    // 后超界
    double t_center_y = fabs(measure_result.car.uniform_y);
    DLOG(INFO) << t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) << " " << out_info.plc_border_maxy();
    DLOG(INFO) << t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) << " " << out_info.plc_border_miny();
    if (t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) > out_info.plc_border_maxy() ||     // 前轮无法抱到
        t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) > out_info.plc_border_miny()) {     // 后轮无法抱到
        last_range_statu |= Range_status::Range_back;       // 后超界
    } else if (t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) < out_info.plc_border_maxy() - 0.010 &&     // 前轮无法抱到
               t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) < out_info.plc_border_miny() - 0.010) {
        last_range_statu &= (~Range_status::Range_back);
    }
    if (last_range_statu & Range_status::Range_back) {
        // 前进距离
        measure_result.car.forward_dis = MAX((t_center_y - 4.80 + 0.5 * (measure_result.car.wheel_base - 2.7) - (out_info.plc_border_maxy() - 0.010)),
                                         (t_center_y - 4.80 - 0.5 * (measure_result.car.wheel_base - 2.7) - (out_info.plc_border_miny() - 0.010)));
        return Range_status::Range_back;
    } 

    return Range_status::Range_correct;
}

Error_manager CommunicationManager::addOffset(DetectManager::DetectResult &measure_result) {
    auto offset = m_config.offset();
    measure_result.car.uniform_x += offset.x();
    measure_result.car.wheels_center_x += offset.x();
    measure_result.car.uniform_y += offset.y();
    measure_result.car.wheels_center_y += offset.y();
    measure_result.car.wheel_base += offset.wheelbase();
    measure_result.car.theta += offset.theta();
    measure_result.car.width += offset.width();
    measure_result.car.wheel_width += offset.width();
    return {};
}

Error_manager CommunicationManager::removeOffset(DetectManager::DetectResult &measure_result) {
    auto offset = m_config.offset();
    measure_result.car.uniform_x -= offset.x();
    measure_result.car.wheels_center_x -= offset.x();
    measure_result.car.uniform_y -= offset.y();
    measure_result.car.wheels_center_y -= offset.y();
    measure_result.car.wheel_base -= offset.wheelbase();
    measure_result.car.theta -= offset.theta();
    measure_result.car.width -= offset.width();
    measure_result.car.wheel_width -= offset.width();
    return {};
}