科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

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

@@ -1,6 +1,17 @@
//
// Created by YY on 2019/10/23.
//
//
//               4  ___________________________ 3
//                  |                         |
//                  |                         |
//                  |                         |
//  ________________|                         |___________1
//  6               5                         2
//
//
//  7______________________________________________________0
//

#include "park_edge.h"
#include "../jni_log.h"
@@ -29,7 +40,6 @@

const uint32_t CHECK_PARK_DELAY = 400;

static int mapIndex = 0;
static bool reportExamTimeout;
static bool reportParkFail;

@@ -45,24 +55,23 @@
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);
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);

void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime)
void StartParkEdge(prime_t &prime)
{
    DEBUG("进入侧方停车场地");

    testStatus = TESTING;
    mapIndex = index;

    occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    reportExamTimeout = false;
    reportParkFail = false;
    prevMoveStatus = moveStatus;
    prevMoveStatus = prime.pMotion->move;
    parkSuccess = false;
    parkStatus = 0;
    occurMoveBack = false;
@@ -82,7 +91,7 @@
    }*/
}

int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
int TestParkEdge(prime_t &prime)
{
    vector<double> dtox;
    vector<Line> line_set;
@@ -128,7 +137,7 @@
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                      rtkTime->mss * 10);      // 开始计时
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
            }
        } else {
            if (occurMoveBack && !checkPark) {
@@ -307,7 +316,7 @@
                DEBUG("开始倒车");
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
            }*/
        }

@@ -328,14 +337,43 @@
TEST_END:
    if (testStatus == TEST_FINISH) {
        DEBUG("侧方停车结束");
        MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
        MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
        return 0;
    }
    return 1;
}

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 *car)
static bool CrashRedLine1(const Polygon *map, const car_model_t *car)
{
    bool ret = false;

@@ -360,7 +398,7 @@
}

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

@@ -387,7 +425,7 @@
    return ret;
}

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

    if (!occurMoveBack) {
@@ -408,62 +446,6 @@

        free(car_body.point);
    }

    return ret;
}

static bool EnterParking(const Polygon *map, const car_model *car) {
    DEBUG("检查停车到位...");

    bool succ = false;

    Polygon parking;
    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(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});

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

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

    return succ;
}

// 车头要驶过前库位线
static bool ExitParkArea(const Polygon *map, const car_model *car)
{
    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)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;

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

    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    }

    free(carBody.point);

    return ret;
}