坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

 lib/src/main/cpp/test_items/park_bottom.cpp

@@ -24,27 +24,15 @@
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include "../test_common/odo_graph.h"
#include "../teach/train.h"
#include "../teach/teach.h"
#include <vector>
#include <cstdlib>
#include <tuple>

#define DEBUG(fmt, args...)     LOGD("<park_bottom> <%s>: " fmt, __func__, ##args)

using namespace std;

enum {
    NONE,
    FIRST_TOUCH_CTRL_LINE,
    FIRST_PARK,
    SECOND_TOUCH_CTRL_LINE,
    SECOND_PARK,
    THIRD_TOUCH_CTRL_LINE
};

enum {
    TESTING,
    TEST_FAIL,          // 因触发某些规则，在车身未完全立场情况下，提前终止部分测试
    TEST_FINISH
};

typedef enum {
    NONE_CTRL_LINE,
@@ -52,151 +40,157 @@
    RIGHT_CTRL_LINE
} ctrl_line_t;

static bool reverseCar = false;

const uint32_t CHECK_PARK_DELAY = 400;

static uint32_t stopTimepoint;
static int prevMoveDirect;
static bool occurCrashRedLine;

static uint32_t firstReverseTimepoint;

static int parkCount;
static char carray[3];
static int darray[3];
static int parkStatus[3];

static int gearAtStop;
static int currGear;
static int reverseCnt = 0;
static bool stopFlag;
static ctrl_line_t prevCrossedCtrlLine;
static double odo;

static bool checkCtrlLine, checkParkspace;
static int handleGearOb, handleMoveOb;

static bool BodyCollidingLine(prime_t &prime);
static ctrl_line_t CrossCtrlLine(prime_t &prime);
static bool EnterParking(prime_t &prime);
static bool CrashRedLine(prime_t &prime);
static bool CheckParkspace(prime_t &prime);
static void MoveOb(move_status_t curr, move_status_t prev, double distance);
static void GearOb(int gear, int prevGear, double move_distance);

enum {
    PREPARE_REVERSE,
    REVERSE_LEFT,
    REVERSE_RIGHT,
    OBVERSE_LEFT,
    OBVERSE_RIGHT
};

/*
1. 进入 --- 首次倒库前
2. 倒库 ---- 停车到位前（右倒库）
3. 左出库
4. 倒库 ---- 停车到位前（左倒库）
5. 右出库(离开场地)
*/

void StartParkBottom(prime_t &prime)
{
    DEBUG("StartParkBottom");

    reverseCar = false;
    memset(carray, 0, sizeof(carray));
    memset(darray, 0, sizeof(darray));
    memset(parkStatus, 0, sizeof(parkStatus));
    prevMoveDirect = prime.pMotion->move;

    firstReverseTimepoint = 0;

    DEBUG("进入倒库场地");
    stopFlag = false;
    parkCount = 0;

    reverseCnt = 0;
    checkCtrlLine = false;
    checkParkspace = false;
    occurCrashRedLine = false;

    currGear = ReadCarStatus(GEAR);
    prime.examing_area.stage = PREPARE_REVERSE;
    testAll();
    LoadStageTips(prime);
    prevCrossedCtrlLine = NONE_CTRL_LINE;
    stopTimepoint = 0;

    odo = ReadOdo();

    handleGearOb = RegisterShiftObserver(GearOb);
    handleMoveOb = RegisterCarMoveObserver(MoveOb);
    PlayTTS("您已进入倒车入库区域", NULL);
}

static void ParkTimeout(apptimer_var_t var)
{
    DEBUG("项目超时");
    AddExamFault(20105);
}

static void MoveOb(move_status_t curr, move_status_t prev, double distance)
{
    // 车辆由运动到停止，记录一下，当换挡发生时，避免原地来回换挡造成误判
    if (curr == STOP && distance > 0.5) {
        stopFlag = true;
    }
}

static void GearOb(int gear, int prevGear, double move_distance)
{
    DEBUG("GearOb %d %f", gear, move_distance);

    if (gear == GEAR_R && stopFlag) {   // 移入倒挡
        stopFlag = false;
        reverseCnt++;
        if (reverseCnt == 1) {
            AppTimer_delete(ParkTimeout);
            AppTimer_add(ParkTimeout, GetPrime().examParam.park_bottom_limit_time);
        }
        checkCtrlLine = true;
    } else if (prevGear == GEAR_R && stopFlag) {    // 从倒挡移出
        stopFlag = false;
        checkParkspace = true;
    }
}

void StopParkBottom(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
        return;

    DEBUG("离开倒库场地");

    if (parkCount < 2) {
        DEBUG("直接驶离测试区,不按考试员指令驾驶");
        AddExamFault(10103);
    }
    AppTimer_delete(ParkTimeout);
    prime.examing_area.type = MAP_TYPE_NONE;
    UnregisterShiftObserver(handleGearOb);
    UnregisterCarMoveObserver(handleMoveOb);
}

void TestParkBottom(prime_t &prime)
{
    ctrl_line_t crossCtrlLine = NONE_CTRL_LINE;
    uint32_t tp = AppTimer_GetTickCount();
    if (prime.examing_area.type != MAP_TYPE_PARK_BUTTOM)
        return;

    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;
    train(prime);

    bool gear_change = false;
    int gear = ReadCarStatus(GEAR);
    if (checkCtrlLine) {
        ctrl_line_t crossCtrlLine = CrossCtrlLine(prime);

    if (gear == GEAR_R) {
        if (currGear != GEAR_R) {
            gear_change = true;
            currGear = GEAR_R;
        }
    } else if (currGear == GEAR_R) {
        gear_change = true;
        currGear = gear;
    }

    if (gear_change) {
        // 检查上一次挡位的行驶距离，过小就放弃，避开学员原地挂挡重试
        double run_distance = ReadOdo() - odo;

        DEBUG("2次挡位运行距离 %f", run_distance);

        if (run_distance < 1) {
            gear_change = false;
            DEBUG("2次挡位运行距离过小，忽略");
        }
        odo = ReadOdo();
    }

    if (gear_change) {
        if (currGear == GEAR_R) {
            // 挂倒挡,检测是否过控制线
            DEBUG("开始挂倒挡");
            if (!reverseCar) {
                DEBUG("开始首轮入库");
                reverseCar = true;
                MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }
            crossCtrlLine = CrossCtrlLine(prime);

            if (parkCount >= 2) {
                DEBUG("开始次轮入库");
                parkCount = 0;
                MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }

            if (crossCtrlLine == NONE_CTRL_LINE) {
                // 倒车前，前轮未驶过控制线
                AddExamFault(20104);
                DEBUG("倒车前，前轮未驶过控制线");
            } else if (crossCtrlLine == prevCrossedCtrlLine) {
                // 重复跨越同一控制线，不按规定线路，顺序形式，不合格
                AddExamFault(20101);
                DEBUG("不按规定线路，顺序形式， 同 %c 侧", prevCrossedCtrlLine);
            } else {
                prevCrossedCtrlLine = crossCtrlLine;
                DEBUG("开始 %c 侧 倒库", prevCrossedCtrlLine);
            }
        if (crossCtrlLine == NONE_CTRL_LINE) {
            // 倒车前，前轮未驶过控制线
            DEBUG("倒车前，前轮未驶过控制线");
            AddExamFault(20104);
        } else if (crossCtrlLine == prevCrossedCtrlLine) {
            // 重复跨越同一控制线，不按规定线路，顺序形式，不合格
            AddExamFault(20101);
            DEBUG("不按规定线路，顺序形式， 同 %d 侧", prevCrossedCtrlLine);
        } else {
            // 从倒挡移出，检测是否入库
            DEBUG("从倒挡移出");

            parkCount++;

            DEBUG("库位检查次数 = %d", parkCount);

            if (EnterParking(prime)) {
                DEBUG("倒库成功");
            } else {
                AddExamFault(20103);
                DEBUG("倒库不入");
            }
            prevCrossedCtrlLine = crossCtrlLine;
            DEBUG("开始 %d 侧 倒库", prevCrossedCtrlLine);
        }

        if (DeltaYaw(YawOf(std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4]), prime.pModeling[prime.curr_modeling_index].yaw) < 90) {
            DEBUG("右侧倒库");
            prime.examing_area.stage = REVERSE_RIGHT;
        } else {
            DEBUG("左侧倒库");
            prime.examing_area.stage = REVERSE_LEFT;
        }
        LoadStageTips(prime);
        checkCtrlLine = false;
    }

    if (ExitParkArea(prime)) {
        // 离开场地
        DEBUG("离开场地");
        if (parkCount < 2) {
            AddExamFault(10103);
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
    if (checkParkspace) {
        if (CheckParkspace(prime)) {
            DEBUG("倒库成功");
        } else {
            DEBUG("倒库不入");
            AddExamFault(20103);
        }
        prime.curr_exam_map.type = 0;
        goto TEST_END;
        parkCount++;
        if (prime.examing_area.stage == REVERSE_RIGHT) {
            prime.examing_area.stage = OBVERSE_LEFT;       // 从左侧倒库
        } else {
            prime.examing_area.stage = OBVERSE_RIGHT;      // 离开场地
        }
        LoadStageTips(prime);
        checkParkspace = false;
    }

    if (BodyCollidingLine(prime) >= 0) {
    if (BodyCollidingLine(prime)) {
        if (!occurCrashRedLine) {
            occurCrashRedLine = true;
            // 车身出线，不合格
@@ -207,76 +201,35 @@
        occurCrashRedLine = false;
    }

    if (prime.pMotion->move != prevMoveDirect) {
        if (prime.pMotion->move == STOP) {
            stopTimepoint = tp;
            gearAtStop = (currGear == GEAR_R ? 1 : 0);
            DEBUG("停车了");
            DEBUG("停车时挡位 = %d", gearAtStop);
        } else if (prevMoveDirect == STOP && stopTimepoint > 0) {
            DEBUG("继续行驶");

            DEBUG("停车时间 %ld", tp - stopTimepoint);
            DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0);

            if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
                && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
                // 停车超2秒，每次扣5分
                AddExamFault(20106);
                DEBUG("中途停车");
            }
        }

        prevMoveDirect = prime.pMotion->move;
    }
TEST_END:
    DEBUG("倒库结束");
    MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 0);
    teach(prime);
}

static int BodyCollidingLine(prime_t &prime)
// 车身或车轮皆不得碰触实线
static bool BodyCollidingLine(prime_t &prime)
{
    vector<Line> lines;
    MakePolygon car_body(prime.pModel->body.size());

    Polygon car_body;

    car_body.num = prime.pModel->body.size();
    car_body.point = new PointF[car_body.num];

    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
    }

    Line line;
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6]);
    lines.push_back(line);
    Line frontAxle, rearAxle;

    int idx = 0;
    MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);

    for (auto line: lines) {
        if (IntersectionOf(line, &car_body) != GM_None) {
            break;
    const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        Line red_line;
        MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]);
        if (IntersectionOf(red_line, frontAxle) == GM_Intersection
            || IntersectionOf(red_line, rearAxle) == GM_Intersection
            || IntersectionOf(red_line, car_body.GetPolygon()) != GM_None) {
            return true;
        }
        idx++;
    }
    delete []car_body.point;
    return idx < lines.size()? idx : -1;
    return false;
}

// 检测2前轮是否正向越过左右控制线
@@ -291,95 +244,37 @@
    MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);

    MAKE_LINE(left_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0]);
    MAKE_LINE(right_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
    MAKE_LINE(left_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[1], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[0]);
    MAKE_LINE(right_ctrl, std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[6], std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[7]);


    if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection &&
            IntersectionOf(right_trace, left_ctrl) == GM_Intersection &&
            IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == RELATION_LEFT) {
            IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == REL_POS_LEFT) {
        return LEFT_CTRL_LINE;
    }

    if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection &&
        IntersectionOf(right_trace, right_ctrl) == GM_Intersection &&
        IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == RELATION_RIGHT) {
        IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == REL_POS_RIGHT) {
        return RIGHT_CTRL_LINE;
    }

    return NONE_CTRL_LINE;
}

// 需要库入口线宽计算在内
static bool EnterParking(prime_t &prime) {
    bool succ = false;
static bool CheckParkspace(prime_t &prime) {
    MakePolygon car_body(prime.pModel->body.size());

    Polygon park_area;
    Polygon car_body;

    car_body.num = prime.pModel->body.size();
    car_body.point = new PointF[car_body.num];
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        car_body.AddPoint(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]]);
    }

    PointF p8 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
                prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
                YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]));
    MakePolygon park_area({std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[8],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[3],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[4],
                           std::get<MAP_TYPE_PARK_BUTTOM>(prime.maps)[prime.examing_area.idx].points[9]});

    PointF p9 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
                prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
                YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]));

    MakePolygon(&park_area, {p8, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], p9});

    if (IntersectionOf(&car_body, &park_area) == GM_Containment) {
        succ = true;
    }

    CleanPolygon(&park_area);
    free(car_body.point);

    DEBUG("检查倒库状态 %s", succ ? "成功" : "失败");

    return succ;
    return (bool) (IntersectionOf(car_body.GetPolygon(), park_area.GetPolygon()) == GM_Containment);
}

// 4个车轮和车头点不在场地中
bool ExitParkArea(prime_t &prime)
{
    Polygon polygon;

    polygon.num = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map.size();
    polygon.point = new PointF[polygon.num];
    for (int i = 0; i < polygon.num; ++i) {
        polygon.point[i] = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[i];
    }

    int num = 0;

    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
                       &polygon) == GM_None) {
        num++;
    }

    delete []polygon.point;

    return num == 5? true : false;
}