update

yy1717

2020-01-15 eced3d013f06b623a49cb9deaba42218c4e37bb8

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

@@ -40,10 +40,10 @@

static void StopCarTimeout(union sigval sig);
static void LeaveTestAreaLongtime(union sigval sig);
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car);
static bool EnterParking(const Polygon *map, const car_model_cache_t *car);
static void 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_cache_t *car);
static bool CrashRedLine(const Polygon *map, const car_model *car);

void StartParkBottom(void)
{
@@ -56,7 +56,7 @@
    currTarget = FIRST_TOUCH_CTRL_LINE;
    leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

    TextOsd(0, "ParkBottom");
    DEBUG("StartParkBottom");
}

void StopParkBottom(void)
@@ -67,10 +67,10 @@
    AppTimer_delete(LeaveTestAreaLongtime);
    currTarget = NONE;

    TextOsd(0, "ParkBottom End");
    DEBUG("ParkBottom End");
}

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

@@ -84,6 +84,8 @@
        if (parkTimeout) {
            // 不合格：动作超时
            err.push_back(10);


            status = -1;
        }
        // 是否压线
@@ -94,8 +96,8 @@
        }

        if (trigLeaveTestAreaDetect) {
            if (IntersectionOf(car->points[car->desc->front_left_tire[TIRE_OUTSIDE]], map) == GM_Containment &&
                IntersectionOf(car->points[car->desc->front_right_tire[TIRE_OUTSIDE]], map) == GM_Containment) {
            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);
            }
@@ -114,29 +116,27 @@
            if (!((leftTireCrossLeftLine && rightTireCrossLeftLine) ||
                    (leftTireCrossRightLine && rightTireCrossRightLine))) {
                DEBUG("CrossCtrlLine");
                CrossCtrlLine(map, car);
                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;
                        TextOsd(0, "来道场玩的");
                        DEBUG("来道场玩的");
                    } else {
                        // 不合格：未按规定线路行驶（直接跑出测试区了）
                        err.push_back(6);
                        status = -1;
                        TextOsd(0, "直接跑出测试区了");
                        DEBUG("直接跑出测试区了");
                    }
                } else if (!trigLeaveTestAreaDetect) {
                    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]];
                    }

                    if (IntersectionOf(map, &car_body) == GM_None) {
@@ -196,12 +196,12 @@
                parkTimeout = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;

                TextOsd(0, "第一次倒库");
                DEBUG("第一次倒库");
            } else if (the_ctrl_line_crossed > 0) {
                currTarget = SECOND_PARK;
                carParkSuccess = false;
                leftTireCrossLeftLine = leftTireCrossRightLine = rightTireCrossLeftLine = rightTireCrossRightLine = false;
                TextOsd(0, "第二次倒库");
                DEBUG("第二次倒库");
            } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
                // 不合格：倒车前，2前轮没驶过控制线
                err.push_back(9);
@@ -243,10 +243,10 @@
                    status = -1;
                } else if (currTarget == FIRST_PARK) {
                    currTarget = SECOND_TOUCH_CTRL_LINE;
                    TextOsd(0, "过另一根控制线");
                    DEBUG("过另一根控制线");
                } else {
                    currTarget = THIRD_TOUCH_CTRL_LINE;
                    TextOsd(0, "再过第一根控制线");
                    DEBUG("再过第一根控制线");
                }
            }
            carStopEvent = false;
@@ -279,25 +279,21 @@
}

// 检测2前轮是否正向越过左右控制线
static void CrossCtrlLine(const Polygon *map, const car_model_cache_t *car)
static void CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
{
    Line leftCtrlLine, rightCtrlLine;
    Line track1;
    PointF p1, p2;

    car_model_cache_t *prev_car = GetCarModelCache(1);
    if (prev_car == NULL)
        return;

    MakeLine(&leftCtrlLine, &map->point[0], &map->point[1]);
    MakeLine(&rightCtrlLine, &map->point[6], &map->point[7]);

    // 左前轮，取轮宽的中点
    p1.X = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->points[car->desc->front_left_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
    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->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->points[prev_car->desc->front_left_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_left_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[prev_car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->left_front_tire[TIRE_INSIDE]].Y) / 2;

    MakeLine(&track1, &p1, &p2);
    
@@ -312,11 +308,11 @@
    }

    // 右前轮
    p1.X = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].X + car->points[car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
    p1.Y = (car->points[car->desc->front_right_tire[TIRE_OUTSIDE]].Y + car->points[car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
    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->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].X + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->points[prev_car->desc->front_right_tire[TIRE_OUTSIDE]].Y + prev_car->points[prev_car->desc->front_right_tire[TIRE_INSIDE]].Y) / 2;
    p2.X = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].X + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].X) / 2;
    p2.Y = (prev_car->carXY[prev_car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[prev_car->right_front_tire[TIRE_INSIDE]].Y) / 2;

    MakeLine(&track1, &p1, &p2);

@@ -330,16 +326,16 @@
    }
}

static bool EnterParking(const Polygon *map, const car_model_cache_t *car) {
static bool EnterParking(const Polygon *map, const car_model *car) {
    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]});
@@ -354,7 +350,7 @@
    return succ;
}

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

@@ -363,10 +359,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) {