角度修正。

fctom1215

2020-03-08 25bdbfbf6f4c9f1530be74fc194ad1dd0a13e8c9

 lib/src/main/cpp/test_items/driving_curve.cpp

@@ -15,10 +15,6 @@

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

enum {
    DRIVING_ON_CURVE
};

const uint32_t STOP_CAR_TIME = D_SEC(2);

static bool testing = false;
@@ -27,15 +23,16 @@
static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
static struct calc_zone_t {
static struct scan_window_t {
    int leftStart;
    int leftEnd;
    int rightStart;
    int rightEnd;
} calcZone;
} scanWindow;

static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car);
static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone);

void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -50,141 +47,31 @@
    reportStopCarTimeout = false;
    crashRedLine = false;

    calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
    scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    Line start, end;
    Line axial;
    Polygon tireRect;

    MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
    MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
                            car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
                            car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
                            car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]});

//    DEBUG("START 线 %d -- %d", calcZone.leftStart, calcZone.rightStart);
//    DEBUG("END 线 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);

    MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);

    if (IntersectionOf(start, axial) == GM_None) {
        // 向起点查找
        int tempLeft = calcZone.leftStart, tempRight = calcZone.rightStart;

        while (calcZone.leftStart > 0 || calcZone.rightStart > 0) {
            if (calcZone.leftStart > 0)
                tempLeft = calcZone.leftStart - 1;
            if (calcZone.rightStart > 0)
                tempRight = calcZone.rightStart - 1;
//            DEBUG("START 向起点查找 %d -- %d", tempLeft, tempRight);
            MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]);

            if (IntersectionOf(start, axial) == GM_Intersection) {
                // 保持之前的线
                break;
            } else {
                calcZone.leftStart = tempLeft;
                calcZone.rightStart = tempRight;

                if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) {
                    // 车辆丢失，重新搜索
                    calcZone.leftStart = calcZone.rightStart = 0;
                    calcZone.leftEnd = calcZone.rightEnd = 0;

                    DEBUG("车辆丢失，重新搜索");

                    while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
                        MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);

                        if (IntersectionOf(start, axial) == GM_Intersection) {
                            while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
                                MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
                                if (IntersectionOf(start, axial) == GM_None) {
                                    break;
                                }
                                if (calcZone.leftStart < map->num)
                                    calcZone.leftStart++;
                                if (calcZone.rightStart < map2->num)
                                    calcZone.rightStart++;
                            }
                            break;
                        }

                        calcZone.leftEnd = calcZone.leftStart;
                        calcZone.rightEnd = calcZone.rightStart;

                        if (calcZone.leftStart < map->num)
                            calcZone.leftStart++;
                        if (calcZone.rightStart < map2->num)
                            calcZone.rightStart++;
                    }

                    if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
                        // 离开场地
                        DEBUG("离开曲线场地");
                        testing = false;
                        goto TEST_END;
                    }

                    break;
                }
            }
        }
    } else {
        // 向终点查找
        do {
            if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
                break;
            }
            if (calcZone.leftStart < map->num)
                calcZone.leftStart++;
            if (calcZone.rightStart < map2->num)
                calcZone.rightStart++;
//            DEBUG("START 向终点查找 %d -- %d", calcZone.leftStart, calcZone.rightStart);
            MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
        } while (IntersectionOf(start, axial) == GM_Intersection);
    // 更新车头扫描线
    if (!UpdateStartLine(&scanWindow, map, map2, &tireRect)) {
        DEBUG("离开场地");
        testing = false;
        goto TEST_END;
    }

    MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
    // 更新车尾扫描线
    UpdateEndLine(false, &scanWindow, map, map2, &tireRect);

    if (IntersectionOf(end, axial) == GM_None) {
        // 向终点查找
        int tempLeft = calcZone.leftEnd, tempRight = calcZone.rightEnd;
        while (calcZone.leftEnd < map->num || calcZone.rightEnd < map2->num) {
            if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
                break;
            }
            if (calcZone.leftEnd < map->num)
                tempLeft = calcZone.leftEnd + 1;
            if (calcZone.rightEnd < map2->num)
                tempRight = calcZone.rightEnd + 1;
//            DEBUG("END 向终点查找 %d -- %d", tempLeft, tempRight);
            MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);
    DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);

            if (IntersectionOf(end, axial) == GM_Intersection) {
                // 保持之前的线
                break;
            } else {
                calcZone.leftEnd = tempLeft;
                calcZone.rightEnd = tempRight;
            }
        }
    } else {
        // 向起点查找
        do {
            if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
                break;
            }
            if (calcZone.leftEnd > 0)
                calcZone.leftEnd--;
            if (calcZone.rightEnd > 0)
                calcZone.rightEnd--;
//            DEBUG("END 向起点查找 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
            MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
        } while (IntersectionOf(end, axial) == GM_Intersection);
    }

    DEBUG("calcZone leftStart %d leftEnd %d rightStart %d rightEnd %d", calcZone.leftStart, calcZone.leftEnd, calcZone.rightStart, calcZone.rightEnd);

    if (CrashRedLine(map, map2, car, &calcZone)) {
    if (CrashRedLine(map, map2, car, &scanWindow)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 车轮压边线，不合格
@@ -217,10 +104,142 @@
    }

TEST_END:
    CleanPolygon(&tireRect);
    return testing ? 1 : 0;
}

static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car)
static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
{
    Line start;

    bool update = true;
    int direct = 0;
    int tempLeft = scanWindow.leftStart, tempRight = scanWindow.rightStart;

    while (update) {
        update = false;
        MakeLine(&start, &map->point[scanWindow.leftStart], &map2->point[scanWindow.rightStart]);

        if (IntersectionOf(start, tireRect) == GM_None) {
            if (direct != 1) {
                direct = -1;
                // 入场方向扫描
                tempLeft = scanWindow.leftStart;
                tempRight = scanWindow.rightStart;

                if (scanWindow.leftStart > 0) {
                    update = true;
                    scanWindow.leftStart--;
                }
                if (scanWindow.rightStart > 0) {
                    update = true;
                    scanWindow.rightStart--;
                }

                if (scanWindow.leftStart <= scanWindow.leftEnd && scanWindow.rightStart <= scanWindow.rightEnd) {
                    DEBUG("车辆丢失，重新搜索 %d %d", scanWindow.leftStart, scanWindow.rightStart);

                    // 车辆丢失，重新搜索
                    update = false;
                    scanWindow.leftEnd = scanWindow.rightEnd = 0;

                    if (UpdateEndLine(true, &scanWindow, map, map2, tireRect)) {
                        DEBUG("匹配成功 %d %d", scanWindow.leftStart, scanWindow.leftEnd);
                        direct = 0;
                        update = true;
                    } else {
                        DEBUG("匹配失败");
                        return false;
                    }
                }
            }
        } else {
            if (direct != -1) {
                // 出场方向扫描
                direct = 1;
                if (scanWindow.leftStart < map->num - 1) {
                    update = true;
                    scanWindow.leftStart++;
                }
                if (scanWindow.rightStart < map2->num - 1) {
                    update = true;
                    scanWindow.rightStart++;
                }
            } else {
                scanWindow.leftStart = tempLeft;
                scanWindow.rightStart = tempRight;
            }
        }
    }

    return true;
}

static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
{
    bool update = true;
    bool crash = false;

    int direct = 0;
    int tempLeft = zone->leftEnd;
    int tempRight = zone->rightEnd;

    Line end;

    while (update) {
        update = false;
        MakeLine(&end, &map->point[zone->leftEnd], &map2->point[zone->rightEnd]);

        if (IntersectionOf(end, tireRect) == GM_None) {
            if (direct != -1) {
                // 出场方向扫描
                direct = 1;
                tempLeft = zone->leftEnd;
                tempRight = zone->rightEnd;

                if (zone->leftEnd < map->num - 1) {
                    update = true;
                    zone->leftEnd++;
                }
                if (zone->rightEnd < map2->num - 1) {
                    update = true;
                    zone->rightEnd++;
                }
            }
        } else {
            if (!crash) {
                crash = true;
                if (mode) {
                    zone->leftStart = zone->leftEnd;
                    zone->rightStart = zone->rightEnd;


                    DEBUG("第一次接触 %d %d %d", zone->leftStart, zone->leftEnd, tempLeft);
                }
            }

            if (direct != 1) {
                // 入场方向扫描
                direct = -1;
                if (zone->leftEnd > 0) {
                    update = true;
                    zone->leftEnd--;
                }
                if (zone->rightEnd > 0) {
                    update = true;
                    zone->rightEnd--;
                }
            } else {
                zone->leftEnd = tempLeft;
                zone->rightEnd = tempRight;
            }
        }
    }

    return crash;
}

bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car)
{
// 全车都需不在地图中
    bool ret = false;
@@ -257,7 +276,7 @@
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone)
{
    bool ret = false;