坐标

yy1717

2024-02-28 27fc91fbe8f88b6885356e68828cfe1ce1db7601

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

@@ -13,393 +13,82 @@

#include <vector>
#include <cstdlib>
#include <tuple>

using namespace std;

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

static bool testing = false;
static int mapIndex = 0;
static uint32_t stopTimepoint = 0;

static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
static struct scan_window_t {
    int leftStart;
    int leftEnd;
    int rightStart;
    int rightEnd;
} scanWindow;

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_t *car, struct scan_window_t *zone, int &who);
static bool CrashRedLine(prime_t &prime);

void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
void StartDrivingCurve(prime_t &prime)
{
    DEBUG("进入曲线行驶场地");

    testing = true;
    mapIndex = index;

    prevMoveDirect = moveDirect;
    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }
    reportStopCarTimeout = false;
    crashRedLine = false;

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

    MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
    MA_EnterMap(prime.examing_area.idx, MAP_TYPE_CURVE, 1);
}

// 曲线场地关键点
// 入口左右两点，出口左右两点，前半部大小圆圆心和直径，后半部大小圆圆心和直径，实际测量大小圆圆心偏差在20cm内
int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
void TestDrivingCurve(prime_t &prime)
{
    // 判断是否在范围内，如果4个车轮都不在曲线内，视为车辆离开场地
    vector<PointF> pxs = {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
                          car->carXY[car->left_rear_tire[TIRE_OUTSIDE]],
                          car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
                          car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]};
    if (prime.examing_area.type != MAP_TYPE_CURVE)
        return;

    int collide = 0;

    for (auto px: pxs) {
        if (IntersectionOfLine(map->front_half_small_circle_centre, map->back_half_small_circle_centre, px) == RELATION_RIGHT) {
            // 位于前半部,实际场地左右边线起点、终点和圆心基本不会共线，可能差异近一米，所以要分开计算
            // 判断的点超过左右起点，即在线的左侧
            if (AngleOf(map->front_half_small_circle_centre, map->left_start_point, px) < 180) {
                if (DistanceOf(map->front_half_small_circle_centre, px) < map->front_half_small_circle_radius) {
                    collide = 1;
                    continue;
                }
            }
            if (AngleOf(map->front_half_big_circle_centre, map->right_start_point, px) < 180) {
                if (DistanceOf(map->front_half_big_circle_centre, px) > map->front_half_big_circle_radius) {
                    collide = 2;
                    continue;
                }
            }
        } else {
            // 判断的点不超过左右终点，即在线的左侧
            if (AngleOf(map->back_half_small_circle_centre, map->right_end_point, px) < 180) {
                if (DistanceOf(map->back_half_small_circle_centre, px) < map->back_half_small_circle_radius) {
                    collide = 2;
                    continue;
                }
            }
            if (AngleOf(map->back_half_big_circle_centre, map->left_end_point, px) < 180) {
                if (DistanceOf(map->back_half_big_circle_centre, px) > map->back_half_big_circle_radius) {
                    collide = 1;
                    continue;
                }
            }
        }
    }

}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    Polygon tireRect;
    int who = 0;
    vector<double> dtox;
    vector<Line> line_set;
    int s;

    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]]});

    // 更新车头扫描线
    if (!UpdateStartLine(&scanWindow, map, map2, &tireRect)) {
        DEBUG("离开场地");
        testing = false;
        goto TEST_END;
    }

    // 更新车尾扫描线
    UpdateEndLine(false, &scanWindow, map, map2, &tireRect);

    // 计算边距
    s = scanWindow.leftStart;
    for (int e = scanWindow.leftStart - 1; e >= scanWindow.leftEnd; --e) {
        Line redLine;
        MakeLine(&redLine, &map->point[s], &map->point[e]);
        line_set.push_back(redLine);
        s = e;
    }

    s = scanWindow.rightStart;
    for (int e = scanWindow.rightStart - 1; e >= scanWindow.rightEnd; --e) {
        Line redLine;
        MakeLine(&redLine, &map2->point[s], &map2->point[e]);
        line_set.push_back(redLine);
        s = e;
    }

    DistanceOfTire2X(dtox, car, line_set);

    MA_SendDistance(dtox[0], dtox[1]);

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

    if (CrashRedLine(map, map2, car, &scanWindow, who)) {
    if (CrashRedLine(prime)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 车轮压边线，不合格
            AddExamFault(20601, rtkTime);
            DEBUG("车轮压边线");
            if (who == 1) {
                PlayTTS("压左曲线", NULL);
            } else if (who == 2) {
                PlayTTS("压右曲线", NULL);
            }
            AddExamFault(20601);
        }
        crashRedLine = true;
    } else {
        crashRedLine = false;
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarTimeout = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {

        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.curve_pause_criteria) && !reportStopCarTimeout) {
            // 停车超2秒，不合格
            AddExamFault(20602, rtkTime);
            DEBUG("中途停车");
            reportStopCarTimeout = true;
        }
    }

TEST_END:
    CleanPolygon(&tireRect);

    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0);
    }

    return testing ? 1 : 0;
}

static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect)
void StopDrivingCurve(prime_t &prime)
{
    Line start;
    if (prime.examing_area.type != MAP_TYPE_CURVE)
        return;
    DEBUG("离开曲行驶是场地");

    bool update = true;
    int direct = 0;
    int tempLeft = scanWindow.leftStart, tempRight = scanWindow.rightStart;
    prime.examing_area.type = MAP_TYPE_NONE;
}

    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;
// 曲线场地关键点
// 入口左右两点，出口左右两点，前半部大小圆圆心和直径，后半部大小圆圆心和直径，实际测量大小圆圆心偏差在20cm内
static bool CrashRedLine(prime_t &prime)
{
    // 判断是否在范围内，4个车轮只要满足在小圆外，大圆内，就说明没有压线
    PointF tires[] = {prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                          prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]};

                if (scanWindow.leftStart > 0) {
                    update = true;
                    scanWindow.leftStart--;
                }
                if (scanWindow.rightStart > 0) {
                    update = true;
                    scanWindow.rightStart--;
                }
    for (int i = 0; i < 4; i++) {
        if (IntersectionOfLine(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre,
                               std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) == REL_POS_RIGHT) {
            // 前半部分
            if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_small_circle_radius
                && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].front_half_big_circle_radius) {

                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++;
                }
                return true;
            }
        } else {
            if (!crash) {
                crash = true;
                if (mode) {
                    zone->leftStart = zone->leftEnd;
                    zone->rightStart = zone->rightEnd;

            if (DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_centre, tires[i]) > std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_small_circle_radius
                && DistanceOf(std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_centre, tires[i]) < std::get<MAP_TYPE_CURVE>(prime.maps)[prime.examing_area.idx].back_half_big_circle_radius) {

                    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 true;
            }
        }
    }

    return crash;
}

bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;
    Polygon bigMap;

    bigMap.num = map->num + map2->num;
    bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));

    int i = 0;
    for (; i < map->num; ++i) {
        bigMap.point[i] = map->point[i];
    }
    for (int j = map2->num; j > 0; --j) {
        bigMap.point[i++] = map2->point[j-1];
    }

    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, &bigMap) == GM_None) {
        ret = true;
    }

    free(carBody.point);
    free(bigMap.point);

    return ret;
}

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

    Line frontTireAxial, rearTireAxial;
    Line redLine;

    MakeLine(&frontTireAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    int s = zone->leftStart;
    for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
        MakeLine(&redLine, &map->point[s], &map->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            who = 1;
            return true;
        }
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            who = 1;
            return true;
        }
        s = e;
    }

    s = zone->rightStart;
    for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
        MakeLine(&redLine, &map2->point[s], &map2->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            who = 2;
            return true;
        }
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            who = 2;
            return true;
        }
        s = e;
    }

    return ret;
    return false;
}