// // Created by YY on 2020/4/3. // #include "area_exam.h" #include "../test_common/car_sensor.h" #include "../driver_test.h" #include "../jni_log.h" #include "park_bottom.h" #include "stop_and_start.h" #include "park_edge.h" #include "driving_curve.h" #include "turn_a90.h" #include "../utils/xconvert.h" #include "../common/apptimer.h" #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) static int CurrExamStatus = EXAM_AREA_NONE; // 1 测试完成 0 测试中 -1 测试错误退出 static int CurrExamMapIndex = -1; static int CurrEnterMapIndex = -1; static void DetectEnterOrExitMap(const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList); static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList); static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList); static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList); static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime); void TerminateAreaExam(void) { CurrExamMapIndex = -1; } void InitAreaExam(void) { CurrExamMapIndex = -1; } void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime) { DetectEnterOrExitMap(car, CarModelList, AreaMapList); ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime); } static void DetectEnterOrExitMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList) { if (CurrExamMapIndex < 0) { if (CurrEnterMapIndex < 0) { CurrEnterMapIndex = EnterMap(car, CarModelList, mapList); if (CurrEnterMapIndex >= 0) { DEBUG("进入某个子项目 idx = %d", CurrEnterMapIndex); CurrExamMapIndex = CurrEnterMapIndex; CurrExamStatus = EXAM_AREA_START; } } else { if (ExitMap(car, CurrEnterMapIndex, mapList)) { CurrEnterMapIndex = -1; } } } } static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime) { if (index >= 0) { if (CurrExamStatus == EXAM_AREA_START) { DEBUG("CurrExamMapIndex %d mtype %d", AreaMapList[index].id, AreaMapList[index].type); switch (AreaMapList[index].type) { case MAP_TYPE_PARK_BUTTOM: DEBUG("进入倒车入库场地 %d", AreaMapList[index].id); StartParkBottom(AreaMapList[index].id, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_STOP_START: DEBUG("进入上坡起步场地 %d", AreaMapList[index].id); StartSAS(AreaMapList[index].id, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_PART_EDGE: DEBUG("进入侧方位停车场地 %d", AreaMapList[index].id); StartParkEdge(AreaMapList[index].id, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_CURVE: DEBUG("进入曲线行驶场地 %d", AreaMapList[index].id); StartDrivingCurve(AreaMapList[index].id, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_TURN_90: DEBUG("进入直角转弯场地 %d", AreaMapList[index].id); StartTurnA90(AreaMapList[index].id, move, azimuth, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; default:break; } } else if (CurrExamStatus == EXAM_AREA_RUN) { int testing = 0; switch (AreaMapList[index].type) { case MAP_TYPE_PARK_BUTTOM: testing = TestParkBottom(&AreaMapList[index].map, car, NULL, speed, move, rtkTime); break; case MAP_TYPE_STOP_START: testing = TestSAS(&AreaMapList[index].map, car, NULL, speed, move, rtkTime); break; case MAP_TYPE_PART_EDGE: testing = TestParkEdge(&AreaMapList[index].map, car, NULL, speed, move, rtkTime); break; case MAP_TYPE_CURVE: testing = TestDrivingCurve(&AreaMapList[index].map, &AreaMapList[index].map2, car, NULL, speed, move, rtkTime); break; case MAP_TYPE_TURN_90: testing = TestTurnA90(&AreaMapList[index].map, car, NULL, azimuth, speed, move, rtkTime); break; default: break; } if (testing > 0) { CurrExamStatus = EXAM_AREA_RUN; } else { CurrExamStatus = EXAM_AREA_END; } } if (CurrExamStatus != EXAM_AREA_RUN) { // 某项结束 CurrExamStatus = EXAM_AREA_NONE; CurrExamMapIndex = -1; } } } static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList) { for (int i = 0; i < mapList.size() && car != NULL; ++i) { // 车前轮或后轮轨迹越过触发线 if (mapList[i].type == MAP_TYPE_STOP_START) { // 构造虚拟的左上角点 double x9, y9, xo, yo; xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2; yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2; x9 = 2*xo - mapList[i].map.point[8].X; y9 = 2*yo - mapList[i].map.point[8].Y; Line triggerLine; triggerLine.X1 = mapList[i].map.point[0].X; triggerLine.Y1 = mapList[i].map.point[0].Y; triggerLine.X2 = x9; triggerLine.Y2 = y9; if (CrashTriggerLine(triggerLine, car, CarModelList)) return i; } if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) { // 车头顶点在场地内 if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { Line enterLine1, enterLine2; MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7])); if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 && DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1) return i; } } if (mapList[i].type == MAP_TYPE_PART_EDGE) { // 车头顶点在场地内 if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { Line enterLine; MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1) return i; } } if (mapList[i].type == MAP_TYPE_TURN_90) { // 车前轮或后轮轨迹越过触发线 Line triggerLine; MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); if (CrashTriggerLine(triggerLine, car, CarModelList)) return i; } if (mapList[i].type == MAP_TYPE_CURVE) { Line triggerLine; MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]); if (CrashTriggerLine(triggerLine, car, CarModelList)) return i; } } return -1; } static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList) { bool ret = false; if (index < 0 || index >= mapList.size()) return true; if (mapList[index].type == MAP_TYPE_PARK_BUTTOM || mapList[index].type == MAP_TYPE_PART_EDGE || mapList[index].type == MAP_TYPE_TURN_90) { // 全车都需不在地图中 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, &mapList[index].map) == GM_None) { ret = true; } free(carBody.point); } if (mapList[index].type == MAP_TYPE_STOP_START) { // 构造虚拟的左上角点 double x9, y9, xo, yo; bool enter = false; xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2; yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2; x9 = 2*xo - mapList[index].map.point[8].X; y9 = 2*yo - mapList[index].map.point[8].Y; Polygon map; map.num = 4; map.point = (PointF *) malloc(map.num * sizeof(PointF)); map.point[0] = mapList[index].map.point[0]; map.point[1] = mapList[index].map.point[8]; map.point[2] = mapList[index].map.point[7]; map.point[3].X = x9; map.point[3].Y = y9; // 全车都需不在地图中 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); free(map.point); } if (mapList[index].type == MAP_TYPE_CURVE) { ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car); } return ret; } static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList) { bool trigger = false; if (CarModelList.size() < 5) return trigger; Polygon trace, trace2; int pn = 0; trace2.num = trace.num = 5; trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num); list::iterator iter = CarModelList.begin(); car_model *c1 = *iter; trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]]; trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]]; ++iter; while (iter != CarModelList.end() && pn < trace.num) { car_model *c2 = *iter; uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); if (tdiff >= D_SEC(1)) { trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]]; trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]]; c1 = c2; } ++iter; } PointF p1, p2; p1.X = triggerLine.X1; p1.Y = triggerLine.Y1; p2.X = triggerLine.X2; p2.Y = triggerLine.Y2; int pp = 0; for (int p = 1; p < pn; ++p) { Line trace_line, trace2_line; MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]); if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) && IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) { // 碰到触发线 DEBUG("碰撞触发线 引发地图"); trigger = true; goto SEARCH_TRIGGER_LINE_END; } } SEARCH_TRIGGER_LINE_END: free(trace.point); return trigger; }