| | |
| | | |
| | | #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; |
| | |
| | | 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) |
| | | { |
| | |
| | | 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; |
| | | // 车轮压边线,不合格 |
| | |
| | | } |
| | | |
| | | 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; |
| | |
| | | } |
| | | |
| | | // 车轮是否压边线 |
| | | 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; |
| | | |