// // Created by YY on 2019/10/23. // // // 4 ___________________________ 3 // | | // | | // | | // ________________| |___________1 // 6 5 2 // // // 7______________________________________________________0 // #include "park_edge.h" #include "../jni_log.h" #include "../test_common/Geometry.h" #include "../driver_test.h" #include "../common/apptimer.h" #include "../native-lib.h" #include "../utils/xconvert.h" #include "../test_common/car_sensor.h" #include "../master/comm_if.h" #include "area_exam.h" #include "../test_common/odo_graph.h" #include #include #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) using namespace std; enum { TESTING, TEST_FAIL, // 因触发某些规则,在车身未完全立场情况下,提前终止部分测试 TEST_FINISH }; const uint32_t CHECK_PARK_DELAY = 400; static bool reportExamTimeout; static bool reportParkFail; static uint32_t stopTimepoint = 0; static bool occurCrashRedLine1, occurCrashRedLine2, occurCrashRedLine3; static int prevMoveStatus, storeMoveStatusBeforeStop; static int parkStatus; static int gearAtStop; static bool occurMoveBack, checkPark, parkSuccess, checkLight; static uint32_t moveBackTimePoint; static int testStatus; static int exitAreaCfm; static int currGear; static double odo; static bool CrashRedLine1(const Polygon *map, const car_model_t *car); static bool CrashRedLine2(const Polygon *map, const car_model_t *car); static bool CrashRedLine3(const Polygon *map, const car_model_t *car); static bool EnterParking(const Polygon *map, const car_model_t *car); static bool ExitParkArea(const Polygon *map, const car_model_t *car); static bool ExitParkArea2(const Polygon *map, const car_model_t *car); void StartParkEdge(prime_t &prime) { DEBUG("进入侧方停车场地"); testStatus = TESTING; occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false; // 这个科目规定特殊点,发生一次扣10分,而不直接淘汰 reportExamTimeout = false; reportParkFail = false; prevMoveStatus = prime.pMotion->move; parkSuccess = false; parkStatus = 0; occurMoveBack = false; checkPark = false; checkLight = false; gearAtStop = -1; stopTimepoint = 0; odo = ReadOdo(); currGear = ReadCarStatus(GEAR); exitAreaCfm = 0; PlayTTS("您已进入侧方停车区域", NULL); // 仅当发生倒车,才意味着项目开始 /*if (moveStatus == -1) { occurMoveBack = true; moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); }*/ } int TestParkEdge(prime_t &prime) { vector dtox; vector line_set; Line distance_line; bool gear_change = false; int gear = ReadCarStatus(GEAR); if (gear == GEAR_R) { if (currGear != GEAR_R) { gear_change = true; currGear = GEAR_R; } } else if (currGear == GEAR_R) { gear_change = true; currGear = gear; } if (gear_change) { // 检查上一次挡位的行驶距离,过小就放弃,避开学员原地挂挡重试 double run_distance = ReadOdo() - odo; DEBUG("2次挡位运行距离 %f", run_distance); if (run_distance < 1) { gear_change = false; DEBUG("2次挡位运行距离过小,忽略"); } odo = ReadOdo(); } // 首次挂倒挡, 才意味着项目开始 if (testStatus == TESTING && gear_change) { if (currGear == GEAR_R) { if (!occurMoveBack) { DEBUG("首次侧方停车"); } else { DEBUG("再次侧方停车"); } { checkPark = false; occurMoveBack = true; moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss * 10); // 开始计时 MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1); } } else { if (occurMoveBack && !checkPark) { // 检查车身入库情况 DEBUG("检查车身入库情况"); checkPark = true; checkLight = false; if (EnterParking(map, car)) { parkStatus = 1; parkSuccess = true; } else { // 停止后,车身出线,不合格 AddExamFault(20401, rtkTime); parkSuccess = false; DEBUG("移库不入"); } } } } if (CrashRedLine1(map, car)) { if (!occurCrashRedLine1 /*&& occurMoveBack*/) { // 车轮压边线,每次扣10分 AddExamFault(20403, rtkTime); DEBUG("车轮压边线"); occurCrashRedLine1 = true; } } else { occurCrashRedLine1 = false; } if (CrashRedLine2(map, car)) { if (!occurCrashRedLine2 /*&& occurMoveBack*/) { // 车身压库位线,每次扣10分 AddExamFault(20404, rtkTime); DEBUG("车身压库位线"); occurCrashRedLine2 = true; } } else { occurCrashRedLine2 = false; } if (CrashRedLine3(map, car)) { if (!occurCrashRedLine3 && !occurMoveBack && moveStatus == 1) { // 车身压库位线,每次扣10分 AddExamFault(20407, rtkTime); DEBUG("车身压库位线"); occurCrashRedLine3 = true; } } else { occurCrashRedLine3 = false; } if (ExitParkArea2(map, car)) { /*if (!parkSuccess && occurMoveBack && !reportParkFail) { // 直接驶离测试区,认为移库不入 AddExamFault(10103, rtkTime); reportParkFail = true; DEBUG("直接驶离测试区,不按考试员指令驾驶"); }*/ if (occurMoveBack && !checkPark) { // 倒车直接驶离测试区 AddExamFault(10103, rtkTime); DEBUG("直接驶离测试区,不按考试员指令驾驶"); } testStatus = TEST_FINISH; 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 (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) { // 入库后一直前进,车头移出驶离线 if (++exitAreaCfm >= 4) { // 避免信号漂移造成的误判 AddExamFault(10103, rtkTime); DEBUG("直接驶离测试区,不按考试员指令驾驶"); testStatus = TEST_FAIL; } } else { exitAreaCfm = 0; } if (occurMoveBack) { uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); if (!reportExamTimeout && tp - moveBackTimePoint >= examParam.park_edge_limit_time) { // 超时90秒,不合格 AddExamFault(20402, rtkTime); reportExamTimeout = true; DEBUG("移库90秒超时"); } } if (moveStatus != prevMoveStatus) { if (moveStatus == 0) { parkStatus = 0; stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); storeMoveStatusBeforeStop = prevMoveStatus; gearAtStop = (currGear == GEAR_R ? 1 : 0); DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); DEBUG("停车时挡位 = %d", gearAtStop); } else if (prevMoveStatus == 0 && stopTimepoint > 0) { DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); DEBUG("停车时间 %ld", tp - stopTimepoint); DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0); if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack && gearAtStop == (currGear == GEAR_R ? 1 : 0)) { // 停车超2秒,每次扣5分 AddExamFault(20406, rtkTime); DEBUG("停车超时"); } if (moveStatus == 1 && checkPark && !checkLight) { // 在这里检查转向灯状态 checkLight = true; if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) { // 不开转向灯,扣10分 AddExamFault(20405, rtkTime); DEBUG("未开启转向灯"); } } /*if (moveStatus == storeMoveStatusBeforeStop) { // 同方向再启动,继续判断是否停车超时 if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) { // 停车超2秒,每次扣5分 AddExamFault(20406, rtkTime); DEBUG("停车超时"); } } else { // 倒车切换到前进 if (moveStatus == 1 && tp - stopTimepoint >= CHECK_PARK_DELAY) { if (parkStatus == 0) { if (EnterParking(map, car)) { parkStatus = 1; parkSuccess = true; } } if (!parkSuccess && !reportParkFail) { // 停止后,车身出线,不合格 AddExamFault(20401, rtkTime); reportParkFail = true; DEBUG("移库不入"); } // 在这里检查转向灯状态 if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) { // 不开转向灯,扣10分 AddExamFault(20405, rtkTime); DEBUG("未开启转向灯"); } } }*/ /* if (moveStatus == -1 && !occurMoveBack) { DEBUG("开始倒车"); occurMoveBack = true; moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1); }*/ } prevMoveStatus = moveStatus; } /*else if (moveStatus == 0 && parkStatus == 0) { uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); if (tp - stopTimepoint >= CHECK_PARK_DELAY) { if (EnterParking(map, car)) { parkStatus = 1; parkSuccess = true; } else { parkStatus = -1; } } }*/ TEST_END: if (testStatus == TEST_FINISH) { DEBUG("侧方停车结束"); MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0); return 0; } return 1; } int EnterParkEdge(prime_t &prime) { if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) { return -1; } PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]]; PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]]; PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]; PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]; Line left_trace, right_trace; MAKE_LINE(left_trace, lp1, lp2); MAKE_LINE(right_trace, rp1, rp2); // 不同地区左右倒库次序可能不同,所以两个方向都可以进入 for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) { Line left_entrance_line; MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]); if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection && IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) { return i; } } return -1; } // 车轮是否压道路边线 static bool CrashRedLine1(const Polygon *map, const car_model_t *car) { bool ret = false; Line red_line; const int red_lines[][2] = {{0, 7}, {1, 2}, {5, 6}}; Line frontAxle, rearAxle; MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]); MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]); 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, frontAxle) == GM_Intersection || IntersectionOf(red_line, rearAxle) == GM_Intersection) { ret = true; break; } } return ret; } // 车身是否压库位线 static bool CrashRedLine2(const Polygon *map, const car_model_t *car) { bool ret = false; Line red_line; const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}}; 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) { ret = true; break; } } free(car_body.point); return ret; } static bool CrashRedLine3(const Polygon *map, const car_model_t *car) { bool ret = false; if (!occurMoveBack) { // 倒车前,车身不得压库位虚线 Polygon car_body; Line red_line; 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]]; } MakeLine(&red_line, &map->point[2], &map->point[5]); if (IntersectionOf(red_line, &car_body) != GM_None) { ret = true; } free(car_body.point); } return ret; }