适应河南测试反馈，倒库，侧方，判定由挡位改变触发，坡起增加上方点。

fctom1215

2021-04-26 08e28643083ff1bee05ad872c98c708dc16dda35

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

@@ -20,6 +20,12 @@

using namespace std;

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

const uint32_t CHECK_PARK_DELAY = 400;

static int mapIndex = 0;
@@ -30,9 +36,10 @@
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
static bool occurMoveBack, parkSuccess;
static int gearAtStop;
static bool occurMoveBack, checkPark, parkSuccess, checkLight;
static uint32_t moveBackTimePoint;
static bool testing = false;
static int testStatus;

static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
@@ -44,7 +51,7 @@
{
    DEBUG("进入侧方停车场地");

    testing = true;
    testStatus = TESTING;
    mapIndex = index;

    occurCrashRedLine1 = occurCrashRedLine2 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
@@ -54,12 +61,16 @@
    parkSuccess = false;
    parkStatus = 0;
    occurMoveBack = false;
    checkPark = false;
    checkLight = false;
    gearAtStop = -1;

    PlayTTS("您已进入侧方停车区域", NULL);
    // 仅当发生倒车，才意味着项目开始
    if (moveStatus == -1) {
    /*if (moveStatus == -1) {
        occurMoveBack = true;
        moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }
    }*/
}

int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
@@ -67,9 +78,39 @@
    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;
    bool is_gear_r = false;

    // 首次挂倒挡， 才意味着项目开始
    if (testStatus == TESTING) {
        if (ReadCarStatus(GEAR) == GEAR_R) {
            is_gear_r = true;
            if (!occurMoveBack) {
                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;
                if (EnterParking(map, car)) {
                    parkStatus = 1;
                    parkSuccess = true;
                } else {
                    // 停止后，车身出线，不合格
                    AddExamFault(20401, rtkTime);
                    parkSuccess = false;
                    DEBUG("移库不入");
                }
            }
        }
    }

    if (CrashRedLine1(map, car)) {
        if (!occurCrashRedLine1 && occurMoveBack) {
        if (!occurCrashRedLine1 /*&& occurMoveBack*/) {
            // 车轮压边线，每次扣10分
            AddExamFault(20403, rtkTime);
            DEBUG("车轮压边线");
@@ -80,7 +121,7 @@
    }

    if (CrashRedLine2(map, car)) {
        if (!occurCrashRedLine2 && occurMoveBack) {
        if (!occurCrashRedLine2 /*&& occurMoveBack*/) {
            // 车身压库位线，每次扣10分
            AddExamFault(20404, rtkTime);
            DEBUG("车身压库位线");
@@ -90,14 +131,20 @@
        occurCrashRedLine2 = false;
    }

    if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
        if (!parkSuccess && occurMoveBack && !reportParkFail) {
    if (ExitParkArea2(map, car)) {
        /*if (!parkSuccess && occurMoveBack && !reportParkFail) {
            // 直接驶离测试区，认为移库不入
            AddExamFault(10103, rtkTime);
            reportParkFail = true;
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
        }*/
        if (occurMoveBack && !checkPark) {
            // 倒车直接驶离测试区
            AddExamFault(10103, rtkTime);
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
        }
        testing = false;

        testStatus = TEST_FINISH;
        goto TEST_END;
    }

@@ -117,6 +164,13 @@
    DistanceOfTire2X(dtox, car, line_set);
    MA_SendDistance(dtox[0], dtox[1]);

    if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
        // 入库后一直前进，车头移出驶离线
        AddExamFault(10103, rtkTime);
        DEBUG("直接驶离测试区,不按考试员指令驾驶");
        testStatus = TEST_FAIL;
    }

    if (occurMoveBack) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

@@ -133,15 +187,33 @@
            parkStatus = 0;
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            storeMoveStatusBeforeStop = prevMoveStatus;

            gearAtStop = is_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 {
        } else if (prevMoveStatus == 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);

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

            if (moveStatus == 1 && checkPark && !checkLight) {
                // 在这里检查转向灯状态
                checkLight = true;
                if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
                    // 不开转向灯，扣10分
                    AddExamFault(20405, rtkTime);
                    DEBUG("未开启转向灯");
                }
            }

            /*if (moveStatus == storeMoveStatusBeforeStop) {
                // 同方向再启动，继续判断是否停车超时
                if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) {
                    // 停车超2秒，每次扣5分
@@ -172,18 +244,18 @@
                        DEBUG("未开启转向灯");
                    }
                }
            }
            }*/

            if (moveStatus == -1 && !occurMoveBack) {
           /* if (moveStatus == -1 && !occurMoveBack) {
                DEBUG("开始倒车");
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
            }
            }*/
        }

        prevMoveStatus = moveStatus;
    } else if (moveStatus == 0 && parkStatus == 0) {
    } /*else if (moveStatus == 0 && parkStatus == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
@@ -194,13 +266,15 @@
                parkStatus = -1;
            }
        }
    }
    }*/

TEST_END:
    if (!testing && occurMoveBack) {
    if (testStatus == TEST_FINISH) {
        DEBUG("侧方停车结束");
        MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
        return 0;
    }
    return testing ? 1 : 0;
    return 1;
}

// 车轮是否压道路边线
@@ -234,7 +308,7 @@
    bool ret = false;

    Line red_line;
    const int red_lines[][2] = {{2, 3}, {3, 4}, {4, 5}};
    const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}};

    Polygon car_body;

@@ -249,6 +323,14 @@
        if (IntersectionOf(red_line, &car_body) != GM_None) {
            ret = true;
            break;
        }
    }

    if (!occurMoveBack) {
        // 倒车前，车身不得压库位虚线
        MakeLine(&red_line, &map->point[2], &map->point[5]);
        if (IntersectionOf(red_line, &car_body) != GM_None) {
            ret = true;
        }
    }

@@ -282,14 +364,12 @@
    return succ;
}

// 整个车辆都要驶过前库位线
// 车头要驶过前库位线
static bool ExitParkArea(const Polygon *map, const car_model *car)
{
    for (int i = 0; i < car->bodyNum; ++i) {
        if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->body[i]]) != -1)
            return false;
    }
    return true;
    if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[ car->axial[AXIAL_FRONT] ]) == -1)
            return true;
    return false;
}

static bool ExitParkArea2(const Polygon *map, const car_model *car)