// // Created by YY on 2019/10/23. // #include "park_bottom.h" #include "../common/apptimer.h" #include "../test_common/Geometry.h" #include "../native-lib.h" #include "../jni_log.h" #include "../driver_test.h" #include "../utils/xconvert.h" #include "../master/comm_if.h" #include "area_exam.h" #include #include #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) using namespace std; enum { NONE, FIRST_TOUCH_CTRL_LINE, FIRST_PARK, SECOND_TOUCH_CTRL_LINE, SECOND_PARK, THIRD_TOUCH_CTRL_LINE }; static bool testing = false, reverseCar = false; static int mapIndex = 0; const uint32_t CHECK_PARK_DELAY = 400; static uint32_t stopTimepoint; static int prevMoveDirect, storeMoveDirectBeforeStop; static bool occurCrashRedLine; static bool checkPartStatus; static uint32_t firstReverseTimepoint; static bool reportExamTimeout, reportParkFail; static bool crossCtrlLineSw; static int parkCount; static char carray[3]; static int darray[3]; static int parkStatus[3]; static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car); static bool EnterParking(const Polygon *map, const car_model *car); static bool CrashRedLine(const Polygon *map, const car_model *car, int &who); static bool ExitParkArea(const Polygon *map, const car_model *car); void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime) { DEBUG("StartParkBottom"); testing = true; reverseCar = false; mapIndex = index; memset(carray, 0, sizeof(carray)); memset(darray, 0, sizeof(darray)); memset(parkStatus, 0, sizeof(parkStatus)); prevMoveDirect = moveDirect; checkPartStatus = false; firstReverseTimepoint = 0; reportExamTimeout = false; parkCount = 0; crossCtrlLineSw = false; reportParkFail = false; occurCrashRedLine = false; } int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime) { char crossCtrlLine = 0; uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); int who = 0; vector dtox; vector line_set; Line distance_line; if (ExitParkArea(map, car)) { DEBUG("离开场地"); // 离开场地 testing = false; if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) { // 倒库不入,不合格 reportParkFail = true; AddExamFault(20103, rtkTime); DEBUG("倒库不入"); } goto TEST_END; } // 距离检测 MakeLine(&distance_line, &map->point[0], &map->point[7]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[1], &map->point[2]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[2], &map->point[3]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[3], &map->point[4]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[4], &map->point[5]); line_set.push_back(distance_line); MakeLine(&distance_line, &map->point[5], &map->point[6]); line_set.push_back(distance_line); DistanceOfTire2X(dtox, car, line_set); MA_SendDistance(dtox[0], dtox[1]); if (CrashRedLine(map, car, who)) { if (!occurCrashRedLine && reverseCar) { occurCrashRedLine = true; // 车身出线,不合格 AddExamFault(10116, rtkTime); DEBUG("车轮压线"); if (who == 1) { PlayTTS("压左库位线", NULL); } else if (who == 2) { PlayTTS("压右库位线", NULL); } } } else { occurCrashRedLine = false; } crossCtrlLine = CrossCtrlLine(map, car, carPrev); if (crossCtrlLine > 0 && !crossCtrlLineSw) { crossCtrlLineSw = true; if (parkCount == 0) { carray[0] = crossCtrlLine; } else if (parkCount == 1) { if (carray[0] == crossCtrlLine) { // 不按规定线路,顺序形式,不合格 AddExamFault(20101, rtkTime); DEBUG("不按规定线路,顺序形式"); } carray[1] = crossCtrlLine; } else if (parkCount == 2) { if (carray[0] != crossCtrlLine) { // 不按规定线路,顺序形式,不合格 AddExamFault(20101, rtkTime); DEBUG("不按规定线路,顺序形式"); } else { // 离开测试区,停止计时 DEBUG("离开测试区,停止计时"); testing = false; goto TEST_END; } carray[2] = crossCtrlLine; } } if (testing && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) { // 完成超时,不合格 if (!reportExamTimeout) { reportExamTimeout = true; AddExamFault(20105, rtkTime); DEBUG("项目超时"); } } if (moveDirect != prevMoveDirect) { if (moveDirect == 0) { stopTimepoint = tp; storeMoveDirectBeforeStop = prevMoveDirect; if (prevMoveDirect == -1) { checkPartStatus = true; // 每次倒车停止,触发入库检查 } DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); } else { DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); DEBUG("停车时间 %ld", tp - stopTimepoint); if (moveDirect == storeMoveDirectBeforeStop) { // 同方向再启动,继续判断是否停车超时 if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria) && reverseCar) { // 停车超2秒,每次扣5分 AddExamFault(20106, rtkTime); DEBUG("中途停车"); } } else if (moveDirect == -1) { // 切换为倒车 if (!reverseCar) { reverseCar = true; MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1); } if (darray[parkCount] == 0) { if (!crossCtrlLineSw) { // 倒车前,前轮未驶过控制线 AddExamFault(20104, rtkTime); DEBUG("倒车前,前轮未驶过控制线"); } darray[parkCount] = 1; firstReverseTimepoint = tp; // 首次倒车,才意味着此项目开始 } } else { // 切换为前进 DEBUG("切换为前进"); if (tp - stopTimepoint >= CHECK_PARK_DELAY) { if (crossCtrlLineSw) { if (parkStatus[parkCount] != 1) { // 倒库不入,不合格 reportParkFail = true; AddExamFault(20103, rtkTime); DEBUG("倒库不入"); } } crossCtrlLineSw = false; if (parkCount < 2) parkCount++; } } } prevMoveDirect = moveDirect; } else if (moveDirect == -1) { if (darray[parkCount] == 0) { // 切换为倒车 if (!crossCtrlLineSw) { // 倒车前,前轮未驶过控制线 AddExamFault(20104, rtkTime); DEBUG("倒车前,前轮未驶过控制线"); } darray[parkCount] = 1; firstReverseTimepoint = tp; } } else if (moveDirect == 0 && crossCtrlLineSw) { if (tp - stopTimepoint >= CHECK_PARK_DELAY && checkPartStatus) { if (EnterParking(map, car)) { parkStatus[parkCount] = 1; } checkPartStatus = false; } } TEST_END: if (!testing && reverseCar) { MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0); } return testing ? 1 : 0; } // 检测2前轮是否正向越过左右控制线 static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car) { // 过右控制线 if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) && (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) && (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) { return 'R'; } // 过左控制线 if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) && (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) && (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) { return 'L'; } return 0; } static bool EnterParking(const Polygon *map, const car_model *car) { bool succ = false; Polygon parking; Polygon car_body; 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->carXY[car->body[i]]; } MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]}); if (IntersectionOf(&car_body, &parking) == GM_Containment) { succ = true; } CleanPolygon(&parking); free(car_body.point); DEBUG("检查倒库状态 %s", succ ? "成功" : "失败"); return succ; } static bool CrashRedLine(const Polygon *map, const car_model *car, int &who) { bool ret = false; Line red_line; const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}}; Polygon car_body; 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->carXY[car->body[i]]; } for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]])); if (IntersectionOf(red_line, &car_body) != GM_None) { if (i == 2 || i == 1) { who = 1; } else if (i == 4 || i == 5) { who = 2; } ret = true; break; } } free(car_body.point); return ret; } static bool ExitParkArea(const Polygon *map, const car_model *car) { // 全车都需不在地图中 bool ret = false; Polygon carBody; 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, map) == GM_None) { ret = true; } free(carBody.point); return ret; }