// // Created by YY on 2019/11/4. // #include "driving_curve.h" #include "../jni_log.h" #include "../driver_test.h" #include "../common/apptimer.h" #include "../utils/xconvert.h" #include "../master/comm_if.h" #include "../native-lib.h" #include "area_exam.h" #include #include using namespace std; #define DEBUG(fmt, args...) LOGD(" <%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 *car, struct scan_window_t *zone, int &who); void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime) { 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); } int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime) { Polygon tireRect; int who = 0; vector dtox; vector 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) { crashRedLine = true; // 车轮压边线,不合格 AddExamFault(27, rtkTime); DEBUG("车轮压边线"); if (who == 1) { PlayTTS("压左曲线", NULL); } else if (who == 2) { PlayTTS("压右曲线", NULL); } } } 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(28, 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) { 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; 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 *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; }