修改了侧方位停车。

fctom1215

2020-02-05 2ea9a1440259ef77e5af3776c022b4473ac34e09

修改了侧方位停车。

 lib/src/main/cpp/driver_test.cpp

@@ -147,6 +147,9 @@
        return;

    MapList[MapNum].id = id;

    if (type == 1) type = 3;            //////////////////////////////////////

    MapList[MapNum].type = type;

    MapList[MapNum].map.num = pointNum;
@@ -380,6 +383,16 @@
        free(brief.body);
        free(brief.point);

        struct RtkTime rtkTime;

        rtkTime.YY = RtkBuffer[index].YY;
        rtkTime.MM = RtkBuffer[index].MM;
        rtkTime.DD = RtkBuffer[index].DD;
        rtkTime.hh = RtkBuffer[index].hh;
        rtkTime.mm = RtkBuffer[index].mm;
        rtkTime.ss = RtkBuffer[index].ss;
        rtkTime.mss = RtkBuffer[index].dss;

        if (TestStart) {
            if (CurrExamMapIndex < 0) {
                CurrExamMapIndex = EnterMap(CarModel, MapList, MapNum);
@@ -398,6 +411,11 @@
                    case MAP_TYPE_STOP_START:
                        break;
                    case MAP_TYPE_PART_EDGE:
                        DEBUG("进入侧方位停车场地");
                        MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartParkEdge(move, &rtkTime);

                        CurrExamStatus = 0;
                        break;
                    case MAP_TYPE_CURVE:
                        break;
@@ -407,16 +425,6 @@
                }
            }
        }

        struct RtkTime rtkTime;

        rtkTime.YY = RtkBuffer[index].YY;
        rtkTime.MM = RtkBuffer[index].MM;
        rtkTime.DD = RtkBuffer[index].DD;
        rtkTime.hh = RtkBuffer[index].hh;
        rtkTime.mm = RtkBuffer[index].mm;
        rtkTime.ss = RtkBuffer[index].ss;
        rtkTime.mss = RtkBuffer[index].dss;

        if (CurrExamMapIndex >= 0) {
            if (CurrExamStatus == 0) {
@@ -429,6 +437,7 @@
                    case MAP_TYPE_STOP_START:
                        break;
                    case MAP_TYPE_PART_EDGE:
                        CurrExamStatus = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_CURVE:
                        break;
@@ -672,7 +681,7 @@

        } else if (mapList[i].type == MAP_TYPE_CURVE) {

        } else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
        } else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM || mapList[i].type == MAP_TYPE_PART_EDGE) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1, enterLine2;

@@ -693,7 +702,7 @@
    bool ret = false;
    if (index < 0 || mapList == NULL || mapNum == 0) return true;

    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM) {
    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM || mapList[index].type == MAP_TYPE_PART_EDGE) {
        // 全车都需不在地图中
        Polygon carBody;


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

@@ -3,182 +3,161 @@
//

#include "park_edge.h"
#include "../jni_log.h"
#include "../Geometry.h"
#include "../driver_test.h"
#include "../common/apptimer.h"
#include "../native-lib.h"
#include "../utils/xconvert.h"

#include <vector>
#include <cstdlib>

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

enum {
    ARRIVED_START,
    PARK_CAR,
    START_CAR
};
using namespace std;

const int PARK_TIMEOUT = 90;
const uint32_t STOP_CAR_TIME = D_SEC(2);

static int prev_run_status;
static int runStatusBeforeStop;
static uint32_t stopTimepoint = 0;
static bool parkTimeout;
static uint64_t stopTimepoint = 0;
static bool occurCrashRedLine1, occurCrashRedLine2;
static bool PETesting = false;
static int currTarget;
static bool carStopEvent;               // 中途停车标记
static bool carParkSuccess;             // 是否停在库位
static int leaveParkCnt;                // 车辆离开库位刚发生的时，检查是否开启转向灯
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
static bool occurMoveBack, parkSuccess;
static uint32_t moveBackTimePoint;

static void ParkEdgeTimeout(union sigval sig);
static bool CrashRedLine1(const Polygon *map, const car_model_cache_t *car);
static bool CrashRedLine2(const Polygon *map, const car_model_cache_t *car);
static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
static bool ExitParkArea(const Polygon *map, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car);
static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(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);

bool EnterParkEdgeArea(const Polygon *car, const Polygon *tire, const Polygon *map)
void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime)
{
    if (IntersectionOf(tire->point[0], map) == GM_Containment &&
        IntersectionOf(tire->point[1], map) == GM_Containment &&
        IntersectionOf(tire->point[4], map) == GM_Containment &&
        IntersectionOf(tire->point[5], map) == GM_Containment) {
        return true;
    }
    return false;
}

void StartParkEdge(void)
{
    prev_run_status = 0;
    parkTimeout = false;
    occurCrashRedLine1 = occurCrashRedLine2 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    PETesting = true;
    currTarget = ARRIVED_START;

    prevMoveStatus = moveStatus;
    parkSuccess = false;
    parkStatus = 0;
    occurMoveBack = false;
    if (moveStatus == -1) {
        occurMoveBack = true;
        moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
    }

    DEBUG("进入侧方停车场地");
}

void StopParkEdge(void)
{
    AppTimer_delete(ParkEdgeTimeout);
    PETesting = false;
}

int TestParkEdge(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status)
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
{
    int status = 0;

    if (!PETesting)
        return 0;

    if (currTarget >= PARK_CAR) {
        if (CrashRedLine1(map, car)) {
            if (!occurCrashRedLine1) {
                occurCrashRedLine1 = true;
                // 车轮压边线，每次扣10分
                err.push_back(23);
            }
        } else {
            occurCrashRedLine1 = false;
    if (CrashRedLine1(map, car)) {
        if (!occurCrashRedLine1) {
            // 车轮压边线，每次扣10分
            AddExamFault(23, rtkTime);
            DEBUG("车轮压边线");
        }
        occurCrashRedLine1 = true;
    } else {
        occurCrashRedLine1 = false;
    }

        if (CrashRedLine2(map, car)) {
            if (!occurCrashRedLine2) {
                occurCrashRedLine2 = true;
                // 车身压库位线，每次扣10分
                err.push_back(24);
            }
        } else {
            occurCrashRedLine2 = false;
    if (CrashRedLine2(map, car)) {
        if (!occurCrashRedLine2) {
            // 车身压库位线，每次扣10分
            AddExamFault(24, rtkTime);
            DEBUG("车身压库位线");
        }
        occurCrashRedLine2 = true;
    } else {
        occurCrashRedLine2 = false;
    }

        if (parkTimeout) {
    if (ExitParkArea(map, car)) {
        if (!parkSuccess || !occurMoveBack) {
            // 直接驶离测试区，认为移库不入
            AddExamFault(21, rtkTime);
            DEBUG("直接驶离测试区,移库不入");
            status = -1;
        } else {
            status = 1;
        }
    }

    if (occurMoveBack) {
        uint32_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);

        if (tp - moveBackTimePoint >= PARK_TIMEOUT) {
            // 超时90秒，不合格
            err.push_back(22);
            AddExamFault(22, rtkTime);
            DEBUG("移库90秒超时");
            status = -1;
        }

        if (prev_run_status != run_status) {
            if (run_status == 0) {
                // 车停了
                runStatusBeforeStop = prev_run_status;
                stopTimepoint = AppTimer_GetTickCount();
            } else {
                // 车动了，且和停车前的运行状态一致
                if (runStatusBeforeStop == run_status && AppTimer_GetTickCount() - stopTimepoint > STOP_CAR_TIME) {
                    // 中途停车，扣5分
                    err.push_back(26);
                }
            }
        }
    }

    if (currTarget == ARRIVED_START) {
        if (ExitTestArea(map, car)) {
            // 直接驶离测试区域
            status = -2;
        } else
        if (run_status < 0) {
            AppTimer_add(ParkEdgeTimeout, D_SEC(PARK_TIMEOUT));
            currTarget = PARK_CAR;
            TextOsd(0, "开始倒库");
        }
    } else if (currTarget == PARK_CAR) {
        if (run_status < 0) {
            carStopEvent = false;
        } else if (run_status == 0) {
            // 立即检查是否停车到位，也许是中途停车，先不管，待发生前进事件后，再断定是否停车到位
            if (!carStopEvent) {
                carStopEvent = true;
                carParkSuccess = EnterParking(map, car);
            }
    if (moveStatus != prevMoveStatus) {
        if (moveStatus == 0) {
            DEBUG("停车了");
            parkStatus = 0;
            stopTimepoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            stopTimepoint = stopTimepoint * 1000 + rtkTime->mss*10;
            storeMoveStatusBeforeStop = prevMoveStatus;
        } else {
            if (carStopEvent) {
                if (!carParkSuccess) {
                    // 不合格：车身出线
                    err.push_back(21);
                    status = -1;
            DEBUG("继续行驶");
            uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            tp = tp * 1000 + rtkTime->mss * 10;

            if (moveStatus == storeMoveStatusBeforeStop) {
                // 同方向再启动，继续判断是否停车超时
                if (tp - stopTimepoint >= STOP_CAR_TIME) {
                    // 停车超2秒，每次扣5分
                    AddExamFault(26, rtkTime);
                    DEBUG("停车超时");
                }
            } else {
                // 倒车切换到前进
                if (moveStatus == 1 && tp - stopTimepoint >= STOP_CAR_TIME) {
                    if (parkStatus == 0) {

                    }

                    if (!parkSuccess) {
                        // 停止后，车身出线，不合格
                        AddExamFault(21, rtkTime);
                        DEBUG("移库不入");
                        status = -1;
                    }
                }
            }
            carStopEvent = false;
            leaveParkCnt = 0;
            currTarget = START_CAR;
            TextOsd(0, "开始出库");
        }
    } else if (currTarget == START_CAR) {
        if (run_status > 0) {
            leaveParkCnt++;

            if (leaveParkCnt == 1) {
                // 未开启转向灯，扣10分
                err.push_back(25);
            if (moveStatus == -1 && !occurMoveBack) {
                DEBUG("开始倒车");
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
            }
        }

            if (ExitParkArea(map, car)) {
                // 项目完成
                status = 1;
                TextOsd(0, "项目完成");
        prevMoveStatus = moveStatus;
    } else if (moveStatus == 0 && parkStatus == 0) {
        uint64_t tp = TimeMakeComposite(2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss);
        tp = tp * 1000 + rtkTime->mss * 10;

        if (tp - stopTimepoint >= STOP_CAR_TIME) {
            if (EnterParking(map, car)) {
                parkStatus = 1;
                parkSuccess = true;
            } else {
                parkStatus = -1;
            }
        }
    }

    if (status != 0) {
        StopParkEdge();
    }

    prev_run_status = run_status;
    return status;
}

static void ParkEdgeTimeout(union sigval sig) {
    AppTimer_delete(ParkEdgeTimeout);
    parkTimeout = true;
}

// 车轮是否压道路边线
static bool CrashRedLine1(const Polygon *map, const car_model_cache_t *car)
static bool CrashRedLine1(const Polygon *map, const car_model *car)
{
    bool ret = false;

@@ -187,8 +166,8 @@

    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]], &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
    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]]);

    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]]);
@@ -203,7 +182,7 @@
}

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

@@ -212,10 +191,10 @@

    Polygon car_body;

    car_body.num = car->desc->body_num;
    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->points[car->desc->body[i]];
        car_body.point[i] = car->carXY[car->body[i]];
    }

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
@@ -230,16 +209,18 @@
    return ret;
}

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

    bool succ = false;

    Polygon parking;
    Polygon car_body;

    car_body.num = car->desc->body_num;
    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->points[car->desc->body[i]];
        car_body.point[i] = car->carXY[car->body[i]];
    }

    MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
@@ -251,24 +232,15 @@
    CleanPolygon(&parking);
    free(car_body.point);

    DEBUG("检查停车到位结束");
    return succ;
}

// 整个车辆都要驶过前库位线
static bool ExitParkArea(const Polygon *map, const car_model_cache_t *car)
static bool ExitParkArea(const Polygon *map, const car_model *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(map->point[4], map->point[5], car->points[i]) != -1)
            return false;
    }
    return true;
}

// 整个车辆都要驶过最前端控制线
static bool ExitTestArea(const Polygon *map, const car_model_cache_t *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(map->point[6], map->point[7], car->points[i]) != -1)
    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;

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

@@ -11,9 +11,7 @@

using namespace std;

bool EnterParkEdgeArea(const Polygon *car, const Polygon *tire, const Polygon *map);
void StartParkEdge(void);
void StopParkEdge(void);
int TestParkEdge(vector<int>&err, const Polygon *map, const car_model_cache_t *car, double speed, int run_status);
void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime);
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_PARK_EDGE_H