上坡地图修正

fctom1215

2020-02-19 dc2a01d4764efd33a23afcaf4f1d7543dc35c4fa

 lib/src/main/cpp/test_items/park_bottom.cpp

@@ -8,6 +8,7 @@
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"
#include "../utils/xconvert.h"
#include <vector>
#include <cstdlib>

@@ -24,317 +25,211 @@
    THIRD_TOUCH_CTRL_LINE
};

const int PARK_TIMEOUT = 210;
const int PARK_TIMEOUT = D_SEC(210);

static bool PBTesting = false;
static bool trigLeaveTestAreaDetect = false;
static bool leaveTestArea = false;
static bool stopCar2S = false;
static bool testing = false;

static int currTarget;
static bool leftTireCrossLeftLine, leftTireCrossRightLine, rightTireCrossLeftLine, rightTireCrossRightLine;
static char first_ctrl_line_id;
static bool carStopEvent;               // 中途停车标记
static bool carParkSuccess;             // 是否停在库位
static bool parkTimeout;
const uint32_t STOP_CAR_TIME = D_SEC(2);
static uint32_t stopTimepoint;
static int prevMoveDirect, storeMoveDirectBeforeStop;
static bool occurCrashRedLine;
static bool checkPartStatus;
static uint32_t firstReverseTimepoint;
static bool reportExamTimeout, reportParkFail;
static bool crossCtrlLineSw;
static int parkCount;
static char carray[3];
static int darray[3];
static int parkStatus[3];

static void StopCarTimeout(union sigval sig);
static void LeaveTestAreaLongtime(union sigval sig);
static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static bool EnterParking(const Polygon *map, const car_model *car);
static void ParkBottomTimeout(union sigval sig);
static bool CrashRedLine(const Polygon *map, const car_model *car);
static bool ExitParkArea(const Polygon *map, const car_model *car);

void StartParkBottom(void)
void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime)
{
    stopCar2S = false;
    trigLeaveTestAreaDetect = false;
    leaveTestArea = false;
    PBTesting = true;
    parkTimeout = false;
    first_ctrl_line_id = 0;
    currTarget = FIRST_TOUCH_CTRL_LINE;
    leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

    DEBUG("StartParkBottom");
    testing = true;
    memset(carray, 0, sizeof(carray));
    memset(darray, 0, sizeof(darray));
    memset(parkStatus, 0, sizeof(parkStatus));
    prevMoveDirect = moveDirect;

    checkPartStatus = false;
    firstReverseTimepoint = 0;
    reportExamTimeout = false;
    parkCount = 0;
    crossCtrlLineSw = false;
    reportParkFail = false;
    occurCrashRedLine = false;
}

void StopParkBottom(void)
int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    PBTesting = false;
    AppTimer_delete(StopCarTimeout);
    AppTimer_delete(ParkBottomTimeout);
    AppTimer_delete(LeaveTestAreaLongtime);
    currTarget = NONE;
    char crossCtrlLine = 0;
    uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

    DEBUG("ParkBottom End");
}

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

    if (!PBTesting)
        return -2;

    DEBUG("TestParkBottom speed %f dir %d", speed, run_status);

    if (currTarget > FIRST_TOUCH_CTRL_LINE) {
        // 是否超时
        if (parkTimeout) {
            // 不合格：动作超时
            DEBUG("不合格：动作超时");
            err.push_back(10);
            AddExamFault(10, rtkTime);
            status = -1;
    if (ExitParkArea(map, car)) {
        DEBUG("离开场地");
        // 离开场地
        testing = false;
        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) {
            // 倒库不入，不合格
            reportParkFail = true;
            AddExamFault(8, rtkTime);
            DEBUG("倒库不入");
        }
        // 是否压线
        if (CrashRedLine(map, car)) {
            // 不合格：车身出线
            DEBUG("不合格：车身出线");
            err.push_back(7);
        goto TEST_END;
    }

    if (CrashRedLine(map, car)) {
        if (!occurCrashRedLine) {
            occurCrashRedLine = true;
            // 车身出线，不合格
            AddExamFault(7, rtkTime);
            status = -1;
            DEBUG("车轮压线");
        }

        if (trigLeaveTestAreaDetect) {
            if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment &&
                IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_Containment) {
                trigLeaveTestAreaDetect = false;
                AppTimer_delete(LeaveTestAreaLongtime);
            }
        }
    } else {
        occurCrashRedLine = false;
    }

    if (currTarget == FIRST_TOUCH_CTRL_LINE ||
        currTarget == SECOND_TOUCH_CTRL_LINE ||
        currTarget == THIRD_TOUCH_CTRL_LINE) {
        if (run_status > 0) {
            if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
                // 扣5分：中途停车超过2秒
                DEBUG("扣5分：中途停车超过2秒");
                err.push_back(11);
                AddExamFault(11, rtkTime);
            }

            if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
                    (leftTireCrossRightLine && rightTireCrossRightLine))) {
                DEBUG("CrossCtrlLine");
                CrossCtrlLine(map, car, carPrev);
            } else if (currTarget == FIRST_TOUCH_CTRL_LINE || currTarget == SECOND_TOUCH_CTRL_LINE) {
                // 跨过控制线后，车辆持续向前行驶，处理这些乱搞情况
                // 整个车都离开测试区后，如果持续15秒，还没回到测试区，就忽略该测试或者淘汰
                if (leaveTestArea) {
                    if (currTarget == FIRST_TOUCH_CTRL_LINE) {
                        status = -2;
                        DEBUG("来道场玩的");
                    } else {
                        // 不合格：未按规定线路行驶（直接跑出测试区了）
                        DEBUG("不合格：未按规定线路行驶（直接跑出测试区了）");
                        err.push_back(6);
                        AddExamFault(6, rtkTime);
                        status = -1;
                        DEBUG("直接跑出测试区了");
                    }
                } else if (!trigLeaveTestAreaDetect) {
                    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]];
                    }

                    if (IntersectionOf(map, &car_body) == GM_None) {
                        trigLeaveTestAreaDetect = true;
                        AppTimer_delete(LeaveTestAreaLongtime);
                        AppTimer_add(LeaveTestAreaLongtime, D_SEC(15));
                        DEBUG("开始离场计时");
                    }

                    free(car_body.point);
                }
            }

            if (currTarget == THIRD_TOUCH_CTRL_LINE) {
                char the_ctrl_line_crossed = 0;

                if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
                    the_ctrl_line_crossed = 'L';
                } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
                    the_ctrl_line_crossed = 'R';
                }

                if (the_ctrl_line_crossed != 0 && the_ctrl_line_crossed == first_ctrl_line_id) {
                    // 项目完成
                    status = 1;
                } else if (the_ctrl_line_crossed != 0) {
                    // 不合格：未按规定线路行驶（未回到起始点）
                    DEBUG("不合格：未按规定线路行驶（未回到起始点）");
                    err.push_back(6);
                    AddExamFault(6, rtkTime);
                    status = -1;
                }
            }
            if (carStopEvent)
                AppTimer_delete(StopCarTimeout);
            carStopEvent = false;
            stopCar2S = false;
        } else if (run_status < 0) {
            // 左右倒库大纲并未要求谁先完成，故以先越过的控制线为准，下次得越过另外一条
            char the_ctrl_line_crossed = 0;

            if (leftTireCrossLeftLine && rightTireCrossLeftLine) {
                the_ctrl_line_crossed = 'L';
            } else if (leftTireCrossRightLine && rightTireCrossRightLine) {
                the_ctrl_line_crossed = 'R';
            }

            if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
                // 不合格：未按规定线路行驶（试图做2次同方向的倒库）
                DEBUG("不合格：未按规定线路行驶（试图做2次同方向的倒库）");
                err.push_back(6);
    crossCtrlLine = CrossCtrlLine(map, car, carPrev);
    if (crossCtrlLine > 0 && !crossCtrlLineSw) {
        crossCtrlLineSw = true;
        if (parkCount == 0) {
            carray[0] = crossCtrlLine;
        } else if (parkCount == 1) {
            if (carray[0] == crossCtrlLine) {
                // 不按规定线路，顺序形式，不合格
                AddExamFault(6, rtkTime);
                status = -1;
            } else if (the_ctrl_line_crossed > 0 && first_ctrl_line_id == 0) {
                first_ctrl_line_id = the_ctrl_line_crossed;
                // 项目正式开始，210秒内完成
                AppTimer_delete(ParkBottomTimeout);
                AppTimer_add(ParkBottomTimeout, D_SEC(PARK_TIMEOUT));
                currTarget = FIRST_PARK;
                carParkSuccess = false;
                parkTimeout = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

                DEBUG("第一次倒库");
            } else if (the_ctrl_line_crossed > 0) {
                currTarget = SECOND_PARK;
                carParkSuccess = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
                DEBUG("第二次倒库");
            } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
                // 不合格：倒车前，2前轮没驶过控制线
                DEBUG("不合格：倒车前，2前轮没驶过控制线");
                err.push_back(9);
                AddExamFault(9, rtkTime);
                status = -1;
                DEBUG("不按规定线路，顺序形式");
            }
            if (carStopEvent)
                AppTimer_delete(StopCarTimeout);
            carStopEvent = false;
            stopCar2S = false;
        } else {
            if (!carStopEvent) {
                AppTimer_delete(StopCarTimeout);
                AppTimer_add(StopCarTimeout, D_SEC(2));
            carray[1] = crossCtrlLine;
        } else if (parkCount == 2) {
            if (carray[0] != crossCtrlLine) {
                // 不按规定线路，顺序形式，不合格
                AddExamFault(6, rtkTime);
                DEBUG("不按规定线路，顺序形式");
            } else {
                // 离开测试区，停止计时
                DEBUG("离开测试区，停止计时");
                testing = false;
                goto TEST_END;
            }
            carStopEvent = true;
            carray[2] = crossCtrlLine;
        }
    } else if (currTarget == FIRST_PARK || currTarget == SECOND_PARK) {
        if (run_status < 0) {
            if (stopCar2S) {
                // 扣5分：中途停车
                DEBUG("扣5分：中途停车");
                err.push_back(11);
                AddExamFault(11, rtkTime);
    }

    if (testing && darray[0] > 0 && tp - firstReverseTimepoint >= PARK_TIMEOUT) {
        // 完成超时，不合格
        if (!reportExamTimeout) {
            reportExamTimeout = true;
            AddExamFault(10, rtkTime);
            DEBUG("项目超时");
        }
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = tp;
            storeMoveDirectBeforeStop = prevMoveDirect;
            if (prevMoveDirect == -1) {
                checkPartStatus = true;
            }

            carStopEvent = false;
            stopCar2S = false;
        } else if (run_status == 0) {
            // 立即检查是否停车到位，也许是中途停车，先不管，待发生前进事件后，再断定是否停车到位
            if (!carStopEvent) {
                carStopEvent = true;
                carParkSuccess = EnterParking(map, car);
                AppTimer_delete(StopCarTimeout);
                AppTimer_add(StopCarTimeout, D_SEC(2));
            }
            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {
            if (carStopEvent) {
                if (!carParkSuccess) {
                    // 不合格：倒库不入
                    DEBUG("不合格：倒库不入");
                    err.push_back(8);
                    AddExamFault(8, rtkTime);
                    status = -1;
                } else if (currTarget == FIRST_PARK) {
                    currTarget = SECOND_TOUCH_CTRL_LINE;
                    DEBUG("过另一根控制线");
                } else {
                    currTarget = THIRD_TOUCH_CTRL_LINE;
                    DEBUG("再过第一根控制线");
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

            DEBUG("停车时间 %ld", tp - stopTimepoint);

            if (moveDirect == storeMoveDirectBeforeStop) {
                // 同方向再启动，继续判断是否停车超时
                if (tp - stopTimepoint >= STOP_CAR_TIME) {
                    // 停车超2秒，每次扣5分
                    AddExamFault(11, rtkTime);
                    DEBUG("中途停车");
                }
            } else if (moveDirect == -1) {
                // 切换为倒车
                if (darray[parkCount] == 0) {
                    if (!crossCtrlLineSw) {
                        // 倒车前，前轮未驶过控制线
                        AddExamFault(9, rtkTime);
                        DEBUG("倒车前，前轮未驶过控制线");
                    }
                    darray[parkCount] = 1;
                    firstReverseTimepoint = tp;
                }
            } else {
                // 切换为前进
                if (tp - stopTimepoint >= STOP_CAR_TIME) {
                    if (crossCtrlLineSw) {
                        if (checkPartStatus) {
                            if (EnterParking(map, car)) {
                                parkStatus[parkCount] = 1;
                            }
                            checkPartStatus = false;
                        }
                        if (parkStatus[parkCount] != 1) {
                            // 倒库不入，不合格
                            reportParkFail = true;
                            AddExamFault(8, rtkTime);
                            DEBUG("倒库不入");
                        }
                    }

                    crossCtrlLineSw = false;

                    if (parkCount < 2)
                        parkCount++;
                }
            }
            carStopEvent = false;
            stopCar2S = false;
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == -1) {
        if (darray[parkCount] == 0) {
            // 切换为倒车
            if (!crossCtrlLineSw) {
                // 倒车前，前轮未驶过控制线
                AddExamFault(9, rtkTime);
                DEBUG("倒车前，前轮未驶过控制线");
            }
            darray[parkCount] = 1;
            firstReverseTimepoint = tp;
        }
    } else if (moveDirect == 0 && crossCtrlLineSw) {
        if (tp - stopTimepoint >= STOP_CAR_TIME && checkPartStatus) {
            if (EnterParking(map, car)) {
                parkStatus[parkCount] = 1;
            }
            checkPartStatus = false;
        }
    }

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

    return status;
}

static void StopCarTimeout(union sigval sig) {
    AppTimer_delete(StopCarTimeout);

    stopCar2S = true;
}

static void LeaveTestAreaLongtime(union sigval sig) {
    AppTimer_delete(LeaveTestAreaLongtime);
    leaveTestArea = true;
    trigLeaveTestAreaDetect = false;
}

static void ParkBottomTimeout(union sigval sig) {
    AppTimer_delete(ParkBottomTimeout);
    parkTimeout = true;
TEST_END:
    return testing ? 0 : 1;
}

// 检测2前轮是否正向越过左右控制线
static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
{
    Line leftCtrlLine, rightCtrlLine;
    Line track1;
    PointF p1, p2;
    // 过右控制线
    if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) &&
        (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) &&
        (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) {
        return 'R';
    }

    MakeLine(&leftCtrlLine, &(map->point[0]), &(map->point[1]));
    MakeLine(&rightCtrlLine, &(map->point[6]), &map->point[7]);
    // 左前轮，取轮宽的中点
    p1.X = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
    MakeLine(&track1, &p1, &p2);
    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
        leftTireCrossLeftLine = true;
        DEBUG("左轮跨越左控制线");
    // 过左控制线
    if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) &&
        (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) &&
        (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) {
        return 'L';
    }
    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
        leftTireCrossRightLine = true;
        DEBUG("左轮跨越右控制线");
    }
    // 右前轮
    p1.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
    MakeLine(&track1, &p1, &p2);
    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
        rightTireCrossLeftLine = true;
        DEBUG("右轮跨越左控制线");
    }
    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
        rightTireCrossRightLine = true;
        DEBUG("右轮跨越右控制线");
    }

    return 0;
}

static bool EnterParking(const Polygon *map, const car_model *car) {
@@ -357,6 +252,8 @@

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

    DEBUG("检查倒库状态 %s", succ ? "成功" : "失败");

    return succ;
}
@@ -387,3 +284,25 @@
    free(car_body.point);
    return ret;
}

static bool ExitParkArea(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;
}