Vzense-Tof-3D/communication/rabbitmq/Boundary.cpp

//
// Created by zx on 23-11-27.
//

#include "Boundary.h"
#include <pcl/point_types.h>
#include <fcntl.h>
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>

using google::protobuf::io::FileInputStream;
using google::protobuf::io::FileOutputStream;
using google::protobuf::io::ZeroCopyInputStream;
using google::protobuf::io::CodedInputStream;
using google::protobuf::io::ZeroCopyOutputStream;
using google::protobuf::io::CodedOutputStream;
using google::protobuf::Message;

bool Boundary::read_proto_param(std::string prototxt_path, ::google::protobuf::Message &protobuf_parameter) {
    int fd = open(prototxt_path.c_str(), O_RDONLY);
    if (fd == -1) return false;
    FileInputStream *input = new FileInputStream(fd);
    bool success = google::protobuf::TextFormat::Parse(input, &protobuf_parameter);
    delete input;
    close(fd);
    return success;
}

bool Boundary::init(const std::string &file) {
    return read_proto_param(file, limit_);
}

bool Boundary::init(JetStream::Limit limit) {
    limit_ = limit;
    return true;
}

Boundary::~Boundary() {}

BoundRet Boundary::limit_boundary(JetStream::MeasureInfo info, JetStream::Boundary bound) {
    //判断角度
    if (info.theta() <= bound.minangle() / 180.0 * M_PI) return BoundRetRightAngle;
    if (info.theta() >= bound.maxangle() / 180.0 * M_PI) return BoundRetLeftAngle;
    //计算四轮位置
    pcl::PointXYZ pt[4];
    pt[0] = pcl::PointXYZ(-info.width() / 2.0, info.wheelbase() / 2.0, 0);
    pt[1] = pcl::PointXYZ(info.width() / 2.0, info.wheelbase() / 2.0, 0);
    pt[2] = pcl::PointXYZ(-info.width() / 2.0, -info.wheelbase() / 2.0, 0);
    pt[3] = pcl::PointXYZ(info.width() / 2.0, -info.wheelbase() / 2.0, 0);
    for (int i = 0; i < 4; ++i) {
        //计算四轮位置
        float x = pt[i].x * cos(info.theta()) - pt[i].y * sin(info.theta()) + info.x();
        float y = pt[i].x * sin(info.theta()) + pt[i].y * cos(info.theta()) + info.y();
        //判断是否在盖板上
        // printf(" %f  %f\n", x * x + y * y, bound.radius() * bound.radius());
        if (x * x + y * y > bound.radius() * bound.radius()) {
            if (fabs(y) > fabs(x)) {
                if (y > 0) return BoundRetFront;
                else return BoundRetBack;
            } else {
                if (x > 0) return BoundRetRight;
                else return BoundRetLeft;
            }
        }
        //判断左右
        if (x < bound.left()) return BoundRetLeft;
        if (x > bound.right()) return BoundRetRight;
    }
    //判断摆正后，后轮是否能够的着
    if (info.trans_y() - 0.5 * info.wheelbase() <= bound.buttom()) return BoundRetBack;

    if (fabs(info.ftheta()) > bound.fwheelangle()) return BoundRetWheelAngle;

    return BoundRetNormal;

}

void Boundary::limit(JetStream::MeasureInfo &info) {

    float trans_x = info.x() * cos(info.theta()) - info.y() * sin(info.theta());
    float trans_y = info.x() * sin(info.theta()) + info.y() * cos(info.theta());
    info.set_trans_x(trans_x);
    info.set_trans_y(trans_y);
    //判断轴距和宽度是否超
    if (info.width() >= limit_.maxwidth()) {
        info.set_border_plc(int(BoundRetWidth));
        info.set_border_display(int(BoundRetWidth));
        return;
    }
    if (info.wheelbase() >= limit_.maxwheelbase()) {
        info.set_border_plc(int(BoundRetWheelBase));
        info.set_border_display(int(BoundRetWheelBase));
        return;
    }

    info.set_border_plc(int(limit_boundary(info, limit_.plc_limit())));
    info.set_border_display(int(limit_boundary(info, limit_.display_limit())));

}

void Boundary::transMeasureInfo(JetStream::MeasureInfo &in, measure_buffer &out) {
    measure_info info;
    info.CopyFrom(out.measure_info_to_plc_forward());
    // 如果地面雷达状态已经被修改过，直接返回
    if (info.ground_status() != Measure_OK) {
        out.mutable_measure_info_to_plc_reverse()->CopyFrom(info);
        out.mutable_measure_info_to_terminal()->CopyFrom(info);
        return;
    }

    // 设备状态
    if (in.border_display() != Range_status::Range_correct || in.border_plc() != Range_status::Range_correct) {
        info.set_ground_status((MeasureStatu::Measure_Border));
    }

    info.set_cx(in.trans_x());                                 // 车辆中心坐标x
    info.set_cy(in.trans_y());
    info.set_theta(in.theta() * 180.0 / M_PI);
    info.set_length(0);
    info.set_width(in.width());
    info.set_wheelbase(in.wheelbase());
    info.set_front_theta(in.ftheta() * 180.0 / M_PI);
    info.set_border_statu(in.border_plc());
    info.set_is_stop(0);

    if (DetectManager::iter()->carMoveStatus() == FAILED) {
        info.set_motion_statu(1);    // 静止3秒
    } else {
        info.set_motion_statu(0);    // 运动
    }
    info.set_move_distance(0);

    out.mutable_measure_info_to_plc_forward()->CopyFrom(info);
    out.mutable_measure_info_to_plc_reverse()->CopyFrom(info);
    out.mutable_measure_info_to_terminal()->CopyFrom(info);
    out.mutable_measure_info_to_terminal()->set_border_statu(in.border_display());
}

void Boundary::transMeasureInfo(JetStream::MeasureInfo &in, TransitData::MeasureInfo &out) {
    out.x = in.x();
    out.y = in.y();
    out.transx = in.trans_x();
    out.transy = in.trans_y();
    out.theta = in.theta();
    out.width = in.width();
    out.wheelbase = in.wheelbase();
    out.ftheta = in.ftheta();
    out.border_plc = in.border_plc();
    out.border_display = in.border_display();
    out.error = in.error();
}

void Boundary::transMeasureInfo(DetectManager::DetectResult &in, JetStream::MeasureInfo &out) {
    out.set_x(in.car.wheels_center_x);
    out.set_y(in.car.wheels_center_y);
    out.set_theta(in.car.theta);
    out.set_width(in.car.wheel_width);
    out.set_wheelbase(in.car.wheel_base);
    out.set_ftheta(in.car.front_wheels_theta);
}