// // Created by YY on 2019/10/23. // // // 4 ___________________________ 3 // | | // | | // | | // ________________|9 8|___________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 #include #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) using namespace std; static bool overpass_parkingspace; static bool check_parking_space; const uint32_t CHECK_PARK_DELAY = 400; static bool occurCrashRedLine; static bool firstMoveBack, checkLight; static void ParkTimeout(apptimer_var_t val); static bool CrashRedLine(prime_t &prime); static bool CheckParkspace(prime_t &prime); static bool OverpassParkspace(prime_t &prime); static void MotionChange(move_status_t mv); enum { PE_PREPARE_PARK, PE_PARKING, PE_EXIT }; void StartParkEdge(prime_t &prime) { DEBUG("进入侧方停车场地"); occurCrashRedLine = false; // 这个科目规定特殊点,发生一次扣10分,而不直接淘汰 firstMoveBack = false; checkLight = false; check_parking_space = false; prime.examing_area.stage = PE_PREPARE_PARK; PlayTTS("您已进入侧方停车区域", NULL); } void StopParkEdge(prime_t &prime) { if (prime.examing_area.type != MAP_TYPE_PARK_EDGE) return; DEBUG("离开侧方停车场地"); if (!check_parking_space) { DEBUG("直接驶离测试区,不按考试员指令驾驶"); AddExamFault(10103); } prime.examing_area.type = MAP_TYPE_NONE; AppTimer_delete(ParkTimeout); } static void ParkTimeout(apptimer_var_t val) { // 超时90秒,不合格 DEBUG("移库90秒超时"); AddExamFault(20402); } void TestParkEdge(prime_t &prime) { if (prime.examing_area.type != MAP_TYPE_PARK_EDGE) return; // 检测后轮是否驶过库位前段 if (!overpass_parkingspace) { overpass_parkingspace = OverpassParkspace(prime); } if (prime.pMotion->move == BACKWARD && prime.sensor.gear == GEAR_R) { if (!firstMoveBack) { // 开始挂倒挡倒车 firstMoveBack = true; if (!overpass_parkingspace) { DEBUG("后轮没有开过库位线就开始倒车"); AddExamFault(10103); } prime.examing_area.stage = PE_PARKING; AppTimer_delete(ParkTimeout); AppTimer_add(ParkTimeout, prime.examParam.park_edge_limit_time); } } //从倒车状态,看是否从倒挡退出(如果只是看停车状态,无法和中途停车有效区分) if (firstMoveBack && prime.sensor.gear != GEAR_R) { if (!check_parking_space) { check_parking_space = true; if (!CheckParkspace(prime)) { // 停止后,车身出线,不合格 DEBUG("车身出线"); AddExamFault(20401); } prime.examing_area.stage = PE_EXIT; } } // 检测左转向灯 if (check_parking_space && prime.pMotion->move == FORWARD && !checkLight) { checkLight = true; if (prime.sensor.turn_signal_lamp != LEFT_TURN_LIGHT) { // 不开转向灯,扣10分 DEBUG("未开启转向灯"); AddExamFault(20405); } } if (CrashRedLine(prime)) { if (!occurCrashRedLine) { // 车身压库位线,每次扣10分 DEBUG("车身压库位线"); AddExamFault(20404); occurCrashRedLine = true; } } else { occurCrashRedLine = false; } } // 车轮或车身是否压道路边线 static bool CrashRedLine(prime_t &prime) { const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}}; Line frontAxle, rearAxle; MAKE_LINE(frontAxle, 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]]); MAKE_LINE(rearAxle, 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]]); MakePolygon body(prime.pModel->body.size()); for (int i = 0; i < prime.pModel->body.size(); ++i) { body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]); } for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { Line red_line; MAKE_LINE(red_line, std::get(prime.maps)[prime.examing_area.idx].points[red_lines[i][0]], std::get(prime.maps)[prime.examing_area.idx].points[red_lines[i][1]]); if (IntersectionOf(red_line, frontAxle) == GM_Intersection || IntersectionOf(red_line, rearAxle) == GM_Intersection || IntersectionOf(red_line, body.GetPolygon()) != GM_None) { return true; } } return false; } static bool CheckParkspace(prime_t &prime) { DEBUG("检查停车到位..."); MakePolygon area({std::get(prime.maps)[prime.examing_area.idx].points[8], std::get(prime.maps)[prime.examing_area.idx].points[3], std::get(prime.maps)[prime.examing_area.idx].points[4], std::get(prime.maps)[prime.examing_area.idx].points[9]}); MakePolygon car_body(prime.pModel->body.size()); for (int i = 0; i < prime.pModel->body.size(); ++i) { car_body.AddPoint(prime.pModeling->points[prime.pModel->body[i]]); } return (IntersectionOf(car_body.GetPolygon(), area.GetPolygon()) == GM_Containment)? true : false; } static bool OverpassParkspace(prime_t &prime) { Line parkspace_top; MAKE_LINE(parkspace_top, std::get(prime.maps)[prime.examing_area.idx].points[4], std::get(prime.maps)[prime.examing_area.idx].points[5]); if (IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT && IntersectionOfLine(prime.pModeling->points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]], parkspace_top) == REL_POS_RIGHT) { return true; } return false; }