坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

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

@@ -6,7 +6,7 @@
//                  |                         |
//                  |                         |
//                  |                         |
//  ________________|                         |___________1
//  ________________|9                       8|___________1
//  6               5                         2
//
//
@@ -27,425 +27,176 @@

#include <vector>
#include <cstdlib>
#include <tuple>

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

using namespace std;

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

const uint32_t CHECK_PARK_DELAY = 400;

static bool reportExamTimeout;
static bool reportParkFail;
static bool occurCrashRedLine;
static bool firstMoveBack, checkLight;

static uint32_t stopTimepoint = 0;
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 void ParkTimeout(apptimer_var_t val);
static bool CrashRedLine(prime_t &prime);
static bool CheckParkspace(prime_t &prime);
static bool OverpassParkspace(prime_t &prime);
static void MotionChange(move_status_t mv);

static bool CrashRedLine1(const Polygon *map, const car_model_t *car);
static bool CrashRedLine2(const Polygon *map, const car_model_t *car);
static bool CrashRedLine3(const Polygon *map, const car_model_t *car);
static bool EnterParking(const Polygon *map, const car_model_t *car);
static bool ExitParkArea(const Polygon *map, const car_model_t *car);
static bool ExitParkArea2(const Polygon *map, const car_model_t *car);
enum {
    PE_PREPARE_PARK,
    PE_PARKING,
    PE_EXIT
};

void StartParkEdge(prime_t &prime)
{
    DEBUG("进入侧方停车场地");

    testStatus = TESTING;

    occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    reportExamTimeout = false;
    reportParkFail = false;
    prevMoveStatus = prime.pMotion->move;
    parkSuccess = false;
    parkStatus = 0;
    occurMoveBack = false;
    checkPark = false;
    occurCrashRedLine = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    firstMoveBack = false;
    checkLight = false;
    gearAtStop = -1;
    stopTimepoint = 0;
    odo = ReadOdo();
    currGear = ReadCarStatus(GEAR);
    exitAreaCfm = 0;

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

int TestParkEdge(prime_t &prime)
void StopParkEdge(prime_t &prime)
{
    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;
    if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
        return;
    DEBUG("离开侧方停车场地");

    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 (!check_parking_space) {
        DEBUG("直接驶离测试区,不按考试员指令驾驶");
        AddExamFault(10103);
    }
    prime.examing_area.type = MAP_TYPE_NONE;

    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 && 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_PARK_EDGE, 1);
            }
        } else {
            if (occurMoveBack && !checkPark) {
                // 检查车身入库情况
                DEBUG("检查车身入库情况");
                checkPark = true;
                checkLight = false;
                if (EnterParking(map, car)) {
                    parkStatus = 1;
                    parkSuccess = true;
                } else {
                    // 停止后，车身出线，不合格
                    AddExamFault(20401, rtkTime);
                    parkSuccess = false;
                    DEBUG("移库不入");
                }
            }
        }
    }

    if (CrashRedLine1(map, car)) {
        if (!occurCrashRedLine1 /*&& occurMoveBack*/) {
            // 车轮压边线，每次扣10分
            AddExamFault(20403, rtkTime);
            DEBUG("车轮压边线");
            occurCrashRedLine1 = true;
        }
    } else {
        occurCrashRedLine1 = false;
    }

    if (CrashRedLine2(map, car)) {
        if (!occurCrashRedLine2 /*&& occurMoveBack*/) {
            // 车身压库位线，每次扣10分
            AddExamFault(20404, rtkTime);
            DEBUG("车身压库位线");
            occurCrashRedLine2 = true;
        }
    } 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)) {
        /*if (!parkSuccess && occurMoveBack && !reportParkFail) {
            // 直接驶离测试区，认为移库不入
            AddExamFault(10103, rtkTime);
            reportParkFail = true;
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
        }*/
        if (occurMoveBack && !checkPark) {
            // 倒车直接驶离测试区
            AddExamFault(10103, rtkTime);
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
        }

        testStatus = TEST_FINISH;
        goto TEST_END;
    }

    // 距离检测
    MakeLine(&distance_line, &map->point[0], &map->point[7]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[1], &map->point[2]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[2], &map->point[3]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[3], &map->point[4]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[4], &map->point[5]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[5], &map->point[6]);
    line_set.push_back(distance_line);
    DistanceOfTire2X(dtox, car, line_set);
    MA_SendDistance(dtox[0], dtox[1]);

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

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

        if (!reportExamTimeout && tp - moveBackTimePoint >= examParam.park_edge_limit_time) {
            // 超时90秒，不合格
            AddExamFault(20402, rtkTime);
            reportExamTimeout = true;
            DEBUG("移库90秒超时");
        }
    }

    if (moveStatus != prevMoveStatus) {
        if (moveStatus == 0) {
            parkStatus = 0;
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            storeMoveStatusBeforeStop = prevMoveStatus;
            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);
            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 == (currGear == 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分
                    AddExamFault(20406, rtkTime);
                    DEBUG("停车超时");
                }
            } else {
                // 倒车切换到前进
                if (moveStatus == 1 && tp - stopTimepoint >= CHECK_PARK_DELAY) {
                    if (parkStatus == 0) {
                        if (EnterParking(map, car)) {
                            parkStatus = 1;
                            parkSuccess = true;
                        }
                    }

                    if (!parkSuccess && !reportParkFail) {
                        // 停止后，车身出线，不合格
                        AddExamFault(20401, rtkTime);
                        reportParkFail = true;
                        DEBUG("移库不入");
                    }

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

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

        prevMoveStatus = moveStatus;
    } /*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) {
            if (EnterParking(map, car)) {
                parkStatus = 1;
                parkSuccess = true;
            } else {
                parkStatus = -1;
            }
        }
    }*/

TEST_END:
    if (testStatus == TEST_FINISH) {
        DEBUG("侧方停车结束");
        MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
        return 0;
    }
    return 1;
    AppTimer_delete(ParkTimeout);
}

int EnterParkEdge(prime_t &prime) {
    if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
        return -1;
    }

    PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
    PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];

    Line left_trace, right_trace;

    MAKE_LINE(left_trace, lp1, lp2);
    MAKE_LINE(right_trace, rp1, rp2);

    // 不同地区左右倒库次序可能不同，所以两个方向都可以进入
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
        Line left_entrance_line;

        MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);

        if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection
            && IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) {
            return i;
        }
    }
    return -1;
}

// 车轮是否压道路边线
static bool CrashRedLine1(const Polygon *map, const car_model_t *car)
static void ParkTimeout(apptimer_var_t val)
{
    bool ret = false;
    // 超时90秒，不合格
    DEBUG("移库90秒超时");
    AddExamFault(20402);
}

    Line red_line;
    const int red_lines[][2] = {{0, 7}, {1, 2}, {5, 6}};
void TestParkEdge(prime_t &prime)
{
    if (prime.examing_area.type != MAP_TYPE_PARK_EDGE)
        return;
    // 检测后轮是否驶过库位前段
    if (!overpass_parkingspace) {
        overpass_parkingspace = OverpassParkspace(prime);
    }

    if (prime.pMotion->move == BACKWARD && prime.sensor.gear == GEAR_R) {
        if (!firstMoveBack) {
            // 开始挂倒挡倒车
            firstMoveBack = true;
            if (!overpass_parkingspace) {
                DEBUG("后轮没有开过库位线就开始倒车");
                AddExamFault(10103);
            }
            prime.examing_area.stage = PE_PARKING;
            AppTimer_delete(ParkTimeout);
            AppTimer_add(ParkTimeout, prime.examParam.park_edge_limit_time);
        }
    }

    //从倒车状态，看是否从倒挡退出(如果只是看停车状态，无法和中途停车有效区分)
    if (firstMoveBack && prime.sensor.gear != GEAR_R) {
        if (!check_parking_space) {
            check_parking_space = true;
            if (!CheckParkspace(prime)) {
                // 停止后，车身出线，不合格
                DEBUG("车身出线");
                AddExamFault(20401);
            }
            prime.examing_area.stage = PE_EXIT;
        }
    }

    // 检测左转向灯
    if (check_parking_space && prime.pMotion->move == FORWARD && !checkLight) {
        checkLight = true;
        if (prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT) {
            // 不开转向灯，扣10分
            DEBUG("未开启转向灯");
            AddExamFault(20405);
        }
    }

    if (CrashRedLine(prime)) {
        if (!occurCrashRedLine) {
            // 车身压库位线，每次扣10分
            DEBUG("车身压库位线");
            AddExamFault(20404);
            occurCrashRedLine = true;
        }
    } else {
        occurCrashRedLine = false;
    }
}

// 车轮或车身是否压道路边线
static bool CrashRedLine(prime_t &prime)
{
    const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};

    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
    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 (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
        if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
            break;
        }
    }

    return ret;
}

// 车身是否压库位线
static bool CrashRedLine2(const Polygon *map, const car_model_t *car)
{
    bool ret = false;

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

    Polygon car_body;

    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]];
    MakePolygon body(prime.pModel->body.size());
    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
    }

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
        if (IntersectionOf(red_line, &car_body) != GM_None) {
            ret = true;
            break;
        }
    }

    free(car_body.point);
    return ret;
}

static bool CrashRedLine3(const Polygon *map, const car_model_t *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]];
        MAKE_LINE(red_line, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get<MAP_TYPE_PARK_EDGE>(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, body.GetPolygon()) != GM_None) {
            return true;
        }

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

        free(car_body.point);
    }

    return ret;
    return false;
}

static bool CheckParkspace(prime_t &prime) {
    DEBUG("检查停车到位...");

    MakePolygon area({std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[8],
                      std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[3],
                      std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4],
                      std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[9]});
    MakePolygon car_body(prime.pModel->body.size());
    for (int i = 0; i < prime.pModel->body.size(); ++i) {
        car_body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]);
    }

    return (IntersectionOf(car_body.GetPolygon(), area.GetPolygon()) == GM_Containment)? true : false;
}

static bool OverpassParkspace(prime_t &prime)
{
    Line parkspace_top;

    MAKE_LINE(parkspace_top, std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[4], std::get<MAP_TYPE_PARK_EDGE>(prime.maps)[prime.examing_area.idx].points[5]);

    if (IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT
        && IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT) {
        return true;
    }
    return false;
}