修改科目三道路识别问题

fctom1215

2021-04-28 148254bb1dc170db320bcb208ca79b0e252751d8

 lib/src/main/cpp/test_items/park_edge.cpp

@@ -12,6 +12,7 @@
#include "../test_common/car_sensor.h"
#include "../master/comm_if.h"
#include "area_exam.h"
#include "../test_common/odo_graph.h"

#include <vector>
#include <cstdlib>
@@ -33,16 +34,20 @@
static bool reportParkFail;

static uint32_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static bool occurCrashRedLine1, occurCrashRedLine2, occurCrashRedLine3;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
static int gearAtStop;
static bool occurMoveBack, checkPark, parkSuccess, checkLight;
static uint32_t moveBackTimePoint;
static int testStatus;
static int exitAreaCfm;
static int currGear;
static double odo;

static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
static bool CrashRedLine3(const Polygon *map, const car_model *car);
static bool EnterParking(const Polygon *map, const car_model *car);
static bool ExitParkArea(const Polygon *map, const car_model *car);
static bool ExitParkArea2(const Polygon *map, const car_model *car);
@@ -54,7 +59,7 @@
    testStatus = TESTING;
    mapIndex = index;

    occurCrashRedLine1 = occurCrashRedLine2 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    reportExamTimeout = false;
    reportParkFail = false;
    prevMoveStatus = moveStatus;
@@ -64,6 +69,10 @@
    checkPark = false;
    checkLight = false;
    gearAtStop = -1;
    stopTimepoint = 0;
    odo = ReadOdo();
    currGear = ReadCarStatus(GEAR);
    exitAreaCfm = 0;

    PlayTTS("您已进入侧方停车区域", NULL);
    // 仅当发生倒车，才意味着项目开始
@@ -78,24 +87,55 @@
    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;
    bool is_gear_r = false;

    bool gear_change = false;
    int gear = ReadCarStatus(GEAR);

    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 (testStatus == TESTING) {
        if (ReadCarStatus(GEAR) == GEAR_R) {
            is_gear_r = true;
    if (testStatus == TESTING && gear_change) {
        if (currGear == GEAR_R) {
            if (!occurMoveBack) {
                DEBUG("首次侧方停车");
            } else {
                DEBUG("再次侧方停车");
            }

            {
                checkPark = false;
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                      rtkTime->mss * 10);      // 开始计时
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
            }
        } else {
            is_gear_r = false;
            if (occurMoveBack && !checkPark) {
                // 检查车身入库情况
                DEBUG("检查车身入库情况");
                checkPark = true;
                checkLight = false;
                if (EnterParking(map, car)) {
                    parkStatus = 1;
                    parkSuccess = true;
@@ -129,6 +169,17 @@
        }
    } else {
        occurCrashRedLine2 = false;
    }

    if (CrashRedLine3(map, car)) {
        if (!occurCrashRedLine3 && !occurMoveBack && moveStatus == 1) {
            // 车身压库位线，每次扣10分
            AddExamFault(20407, rtkTime);
            DEBUG("车身压库位线");
            occurCrashRedLine3 = true;
        }
    } else {
        occurCrashRedLine3 = false;
    }

    if (ExitParkArea2(map, car)) {
@@ -166,9 +217,13 @@

    if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
        // 入库后一直前进，车头移出驶离线
        AddExamFault(10103, rtkTime);
        DEBUG("直接驶离测试区,不按考试员指令驾驶");
        testStatus = TEST_FAIL;
        if (++exitAreaCfm >= 4) {       // 避免信号漂移造成的误判
            AddExamFault(10103, rtkTime);
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
            testStatus = TEST_FAIL;
        }
    } else {
        exitAreaCfm = 0;
    }

    if (occurMoveBack) {
@@ -187,17 +242,19 @@
            parkStatus = 0;
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            storeMoveStatusBeforeStop = prevMoveStatus;
            gearAtStop = is_gear_r ? 1 : 0;
            gearAtStop = (currGear == GEAR_R ? 1 : 0);
            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else if (prevMoveStatus == 0) {
            DEBUG("停车时挡位 = %d", gearAtStop);
        } else if (prevMoveStatus == 0 && stopTimepoint > 0) {
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
            uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

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

            if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria)
            && occurMoveBack
            && gearAtStop == (is_gear_r ? 1 : 0)) {
            && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
                // 停车超2秒，每次扣5分
                AddExamFault(20406, rtkTime);
                DEBUG("停车超时");
@@ -326,15 +383,32 @@
        }
    }

    free(car_body.point);
    return ret;
}

static bool CrashRedLine3(const Polygon *map, const car_model *car) {
    bool ret = false;

    if (!occurMoveBack) {
        // 倒车前，车身不得压库位虚线
        Polygon car_body;
        Line red_line;

        car_body.num = car->bodyNum;
        car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
        for (int i = 0; i < car_body.num; ++i) {
            car_body.point[i] = car->carXY[car->body[i]];
        }

        MakeLine(&red_line, &map->point[2], &map->point[5]);
        if (IntersectionOf(red_line, &car_body) != GM_None) {
            ret = true;
        }

        free(car_body.point);
    }

    free(car_body.point);
    return ret;
}