From 59abff0d03403344619420aa0bcd9c2b28ff3522 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期三, 08 四月 2020 11:06:58 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/driver_test.cpp | 451 ++----------------------- lib/src/main/cpp/native-lib.h | 2 lib/src/main/cpp/test_items/park_edge.h | 2 lib/src/main/cpp/test_items/park_bottom.h | 2 app/src/main/assets/map.json | 73 ++-- lib/src/main/cpp/test_items/area_exam.cpp | 336 +++++++++++++++++++ lib/src/main/cpp/CMakeLists.txt | 2 lib/src/main/cpp/driver_test.h | 21 lib/src/main/cpp/test_common/Geometry.cpp | 15 lib/src/main/cpp/test_items/park_bottom.cpp | 2 lib/src/main/cpp/test_items/turn_a90.cpp | 2 lib/src/main/cpp/test_items/park_edge.cpp | 2 lib/src/main/cpp/master/comm_if.cpp | 101 +++-- lib/src/main/cpp/native-lib.cpp | 2 lib/src/main/cpp/test_items/area_exam.h | 5 lib/src/main/cpp/test_items2/through_something.cpp | 2 lib/src/main/cpp/test_common/Geometry.h | 0 17 files changed, 506 insertions(+), 514 deletions(-) diff --git a/app/src/main/assets/map.json b/app/src/main/assets/map.json index 0c6078c..e9be599 100644 --- a/app/src/main/assets/map.json +++ b/app/src/main/assets/map.json @@ -1,38 +1,37 @@ -[{ - "id": 876, - "item": 3, - "point": [{ - "x-y": [16589.2108, -7592.2752, 16587.738, -7588.9626, 16581.1917, -7592.3313, 16580.1238, -7590.1814, 16573.4773, -7593.6051, 16574.5939, -7595.7506, 16568.6578, -7598.8277, 16570.2548, -7602.0429] - }] -}, { - "id": 879, - "item": 2, - "point": [{ - "x-y": [16631.9724, -7572.0083, 16640.7097, -7567.8933, 16638.8423, -7563.8859, 16639.3253, -7563.5842, 16641.2092, -7567.6242, 16641.4985, -7567.4958, 16639.6009, -7563.438, 16640.095, -7563.1417, 16642.0225, -7567.2532] - }] -}, { - "id": 877, - "item": 1, - "point": [{ - "x-y": [16606.5718, -7592.7425, 16609.7972, -7598.9155, 16615.6195, -7595.8881, 16617.9794, -7600.5532, 16620.2055, -7599.3555, 16617.8385, -7594.7065, 16623.7326, -7591.5665, 16620.5822, -7585.3858] - }] -}, { - "id": 878, - "item": 5, - "point": [{ - "x-y": [16601.7211, -7627.4499, 16604.7674, -7625.387, 16607.3525, -7630.2736, 16611.6697, -7628.0306, 16613.0442, -7631.3969, 16605.8191, -7635.1681] - }] -}, { - "id": 875, - "item": 4, - "point": [{ - "x-y": [16582.5745, -7625.2715, 16583.3317, -7626.2782, 16584.3733, -7627.0041, 16585.3725, -7627.3071, 16586.6709, -7627.2873, 16587.8216, -7626.8839, 16588.8947, -7626.0781, 16589.5949, -7624.9904, 16590.0621, -7623.8121, 16590.628, -7622.7458, 16591.3904, -7621.7376, 16592.2946, -7620.9083, 16593.2589, -7620.2455, 16594.1722, -7619.8063, 16595.3586, -7619.4565, 16596.6087, -7619.2749, 16598.3161, -7619.4024, 16599.5229, -7619.7126, 16600.5292, -7620.151, 16601.5604, -7620.8035, 16602.481, -7621.6376, 16603.0784, -7622.348, 16603.3629, -7622.755] +{ + "items": [{ + "id": 876, + "item": 3, + "point": [{ + "x-y": [16589.2108, -7592.2752, 16587.738, -7588.9626, 16581.1917, -7592.3313, 16580.1238, -7590.1814, 16573.4773, -7593.6051, 16574.5939, -7595.7506, 16568.6578, -7598.8277, 16570.2548, -7602.0429] + }] }, { - "x-y": [16579.3086, -7626.9764, 16579.8354, -7627.7888, 16580.3393, -7628.4316, 16581.181, -7629.2701, 16582.0667, -7629.9052, - 16583.1605, -7630.4413, 16584.3072, -7630.818, 16585.5416, -7630.993, 16586.918, -7630.9599, 16588.0948, -7630.7205, 16589.1489, -7630.3119, 16590.3826, -7629.6211, 16591.2856, -7628.8977, 16592.0383, -7628.0128, - - 16592.8408, -7626.7007, 16593.3354, -7625.4394, 16593.886, -7624.4265, 16594.9656, -7623.4808, 16596.0498, -7623.0286, 16597.3348, -7622.955, 16598.4564, -7623.2286, - 16599.4254, -7623.7931, 16600.1166, -7624.5026 - ] - }] -}] \ No newline at end of file + "id": 879, + "item": 2, + "point": [{ + "x-y": [16631.9724, -7572.0083, 16640.7097, -7567.8933, 16638.8423, -7563.8859, 16639.3253, -7563.5842, 16641.2092, -7567.6242, 16641.4985, -7567.4958, 16639.6009, -7563.438, 16640.095, -7563.1417, 16642.0225, -7567.2532] + }] + }, { + "id": 877, + "item": 1, + "point": [{ + "x-y": [16606.5718, -7592.7425, 16609.7972, -7598.9155, 16615.6195, -7595.8881, 16617.9794, -7600.5532, 16620.2055, -7599.3555, 16617.8385, -7594.7065, 16623.7326, -7591.5665, 16620.5822, -7585.3858] + }] + }, { + "id": 878, + "item": 5, + "point": [{ + "x-y": [16601.7211, -7627.4499, 16604.7674, -7625.387, 16607.3525, -7630.2736, 16611.6697, -7628.0306, 16613.0442, -7631.3969, 16605.8191, -7635.1681] + }] + }, { + "id": 875, + "item": 4, + "point": [{ + "x-y": [16582.5745, -7625.2715, 16583.3317, -7626.2782, 16584.3733, -7627.0041, 16585.3725, -7627.3071, 16586.6709, -7627.2873, 16587.8216, -7626.8839, 16588.8947, -7626.0781, 16589.5949, -7624.9904, 16590.0621, -7623.8121, 16590.628, -7622.7458, 16591.3904, -7621.7376, 16592.2946, -7620.9083, 16593.2589, -7620.2455, 16594.1722, -7619.8063, 16595.3586, -7619.4565, 16596.6087, -7619.2749, 16598.3161, -7619.4024, 16599.5229, -7619.7126, 16600.5292, -7620.151, 16601.5604, -7620.8035, 16602.481, -7621.6376, 16603.0784, -7622.348, 16603.3629, -7622.755] + }, { + "x-y": [16579.3086, -7626.9764, 16579.8354, -7627.7888, 16580.3393, -7628.4316, 16581.181, -7629.2701, 16582.0667, -7629.9052, 16583.1605, -7630.4413, 16584.3072, -7630.818, 16585.5416, -7630.993, 16586.918, -7630.9599, 16588.0948, -7630.7205, 16589.1489, -7630.3119, 16590.3826, -7629.6211, 16591.2856, -7628.8977, 16592.0383, -7628.0128, 16592.8408, -7626.7007, 16593.3354, -7625.4394, 16593.886, -7624.4265, 16594.9656, -7623.4808, 16596.0498, -7623.0286, 16597.3348, -7622.955, 16598.4564, -7623.2286, 16599.4254, -7623.7931, 16600.1166, -7624.5026] + }] + }], + "name": "绉戜簩鍦鸿�冨湴鍥�", + "type": "yard" +} \ No newline at end of file diff --git a/lib/src/main/cpp/CMakeLists.txt b/lib/src/main/cpp/CMakeLists.txt index 368252d..d6861f1 100644 --- a/lib/src/main/cpp/CMakeLists.txt +++ b/lib/src/main/cpp/CMakeLists.txt @@ -24,7 +24,7 @@ rtk_platform/parse_net.cpp rtk_platform/platform.cpp rtk_module/parse_gps.cpp - Geometry.cpp + test_common/Geometry.cpp driver_test.cpp mcu/mcu_if.cpp diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index b502094..22b8ff2 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -18,7 +18,7 @@ #include "driver_test.h" #include "defs.h" -#include "Geometry.h" +#include "test_common/Geometry.h" #include "common/apptimer.h" #include "jni_log.h" #include "test_items/park_edge.h" @@ -35,6 +35,7 @@ #include "mcu/mcu_if.h" #include "test_common/car_sensor.h" #include "test_items2/road_exam.h" +#include "test_items/area_exam.h" #define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args) @@ -59,29 +60,16 @@ static int ExamType; static bool reportSeatbeltEject; -static int CarInArea = 0; -int errs = 0; - -vector<int> ErrorList; vector<ExamFault> ExamFaultList; static int examFaultIndex = 0; -static struct map_list { - int id; - int type; - Polygon map; - Polygon map2; -} MapList[MAP_LIST_SIZE]; +static LIST_AREA_MAP AreaMapList; static Polygon RoadMapPoints; static LIST_ROAD_MAP RoadMapList; -static int MapNum = 0; -static int CurrExamMapIndex = -1; -static int CurrEnterMapIndex = -1; -static int CurrExamStatus = EXAM_AREA_NONE; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑� static int exam_dummy_light; static car_model *CarModel = NULL; @@ -102,7 +90,6 @@ static rtk_info *RtkBuffer = NULL; static int RtkBufferNum = 0, RtkBufferIn = 0; -static void DetectEnterOrExitMap(void); static void EngineStartHold(union sigval sig); static void ExecuteExam(const struct RtkTime* rtkTime); static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime); @@ -111,19 +98,15 @@ static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel); static bool UpdateCarCoord(double &spd, int &mov, int &idx); -static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList); -static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum); -static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum); -static int GetMapId(int index, const struct map_list *mapList, int mapNum); -static int GetMapType(int index, const struct map_list *mapList, int mapNum); void DriverTestInit(void) { ExamStart = false; - memset(&MapList, 0, sizeof(MapList)); - MapNum = 0; + CarModel = NULL; CarModelList.clear(); + + AreaMapList.clear(); RoadMapPoints.num = 0; RoadMapPoints.point = NULL; @@ -137,8 +120,6 @@ RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info)); RtkBufferNum = RtkBufferIn = 0; - - CurrExamStatus = EXAM_AREA_NONE; } static void ReadDriverExamPrimerTimeout(union sigval sig) @@ -155,54 +136,54 @@ MA_ReadSensor(); } -void ClearMap(void) +void ClearAreaMap(void) { - if (ExamStart) return; - - for (int i = 0; i < MapNum; ++i) { - if (MapList[i].map.point != NULL) - free(MapList[i].map.point); - if (MapList[i].map2.point != NULL) - free(MapList[i].map2.point); - } - memset(&MapList, 0, sizeof(MapList)); - MapNum = 0; -} - -void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2) -{ - DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2); - - AppTimer_delete(ReadDriverExamPrimerTimeout); - - if (map == NULL || pointNum == 0 || ExamStart) + if (ExamStart) return; - MapList[MapNum].id = id; + for (int i = 0; i < AreaMapList.size(); ++i) { + if (AreaMapList[i].map.point != NULL) + free(AreaMapList[i].map.point); + if (AreaMapList[i].map2.point != NULL) + free(AreaMapList[i].map2.point); + } - MapList[MapNum].type = type; + AreaMapList.clear(); +} - MapList[MapNum].map.num = pointNum; +void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2) +{ + if (map == NULL || pointNum == 0 || ExamStart) + return; + DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2); + + struct area_exam_map newMap; + + newMap.id = id; + newMap.type = type; + newMap.map.num = pointNum; + newMap.map2.num = 0; + newMap.map.point = NULL; + newMap.map2.point = NULL; + if (pointNum > 0) { - MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum); + newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF)); for (int i = 0; i < pointNum; ++i) { - MapList[MapNum].map.point[i].X = map[i][0]; - MapList[MapNum].map.point[i].Y = map[i][1]; + newMap.map.point[i].X = map[i][0]; + newMap.map.point[i].Y = map[i][1]; } } - MapList[MapNum].map2.num = pointNum2; if (pointNum2 > 0 && map2 != NULL) { - MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2); + newMap.map2.num = pointNum2; + newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF)); for (int i = 0; i < pointNum2; ++i) { - MapList[MapNum].map2.point[i].X = map2[i][0]; - MapList[MapNum].map2.point[i].Y = map2[i][1]; + newMap.map2.point[i].X = map2[i][0]; + newMap.map2.point[i].Y = map2[i][1]; } } - MapNum++; - - DEBUG("AddMap num %d", MapNum); + AreaMapList.push_back(newMap); } void CleanRoadMap(void) @@ -550,14 +531,15 @@ DEBUG("缁撴潫鑰冭瘯"); TerminateRoadExam(); + TerminateAreaExam(); - CurrExamMapIndex = -1; + ExamStart = false; MA_SendExamStatus(0, 0); return; } - if (MapNum == 0 && type == TEST_TYPE_AREA) { + if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) { DEBUG("娌℃湁鍦鸿�冨湴鍥�"); err = true; MA_SendExamStatus(0, -1); @@ -590,21 +572,12 @@ if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) { exam_dummy_light = 0; - -// InitRoadExam(); //////////////////// + } + if (type == TEST_TYPE_AREA) { + InitAreaExam(); } } MA_SendExamStatus(1, 0); - } -} - -void StartMapExam(int map_id, int exam) -{ - DEBUG("娴嬭瘯璇ュ満鍦� %d: %d", map_id, exam); - - if (map_id >= 0 && exam == 0) { - CurrExamMapIndex = map_id; - CurrExamStatus = EXAM_AREA_START; } } @@ -692,35 +665,8 @@ rtkTime.ss = RtkBuffer[index].ss; rtkTime.mss = RtkBuffer[index].dss; - DetectEnterOrExitMap(); - if (ExamStart) { ExecuteExam(speed, move, azimuth, &rtkTime); - } - } -} - -static void DetectEnterOrExitMap(void) -{ - if (ExamType == TEST_TYPE_AREA) { - - } - if (CurrExamMapIndex < 0) { - if (CurrEnterMapIndex < 0) { - CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum); - if (CurrEnterMapIndex >= 0) { -// DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); -// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1); - - CurrExamMapIndex = CurrEnterMapIndex; - CurrExamStatus = EXAM_AREA_START; - } - } else { - if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) { -// DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); -// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0); - CurrEnterMapIndex = -1; - } } } } @@ -875,87 +821,8 @@ if (exam_dummy_light == 2) { TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime); } - return; - } - - if (CurrExamMapIndex >= 0) { - int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum); - - if (CurrExamStatus == EXAM_AREA_START) { - DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype); - - switch (mtype) { - case MAP_TYPE_PARK_BUTTOM: - DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴"); - MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - - StartParkBottom(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_STOP_START: - DEBUG("杩涘叆涓婂潯璧锋鍦哄湴"); - MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartSAS(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_PART_EDGE: - DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�"); - MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartParkEdge(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_CURVE: - DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴"); - MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartDrivingCurve(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_TURN_90: - DEBUG("杩涘叆鐩磋杞集鍦哄湴"); - MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartTurnA90(GetMapId(CurrExamMapIndex, MapList, MapNum), move, azimuth, rtkTime); - CurrExamStatus = EXAM_AREA_RUN; - break; - default:break; - } - } else if (CurrExamStatus == EXAM_AREA_RUN) { - int testing = 0; - switch (mtype) { - case MAP_TYPE_PARK_BUTTOM: - testing = TestParkBottom(&MapList[CurrExamMapIndex].map, - CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_STOP_START: - testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_PART_EDGE: - testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_CURVE: - testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_TURN_90: - testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, 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) { - // 鏌愰」缁撴潫 - //DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - //MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - CurrExamStatus = EXAM_AREA_NONE; - CurrExamMapIndex = -1; - } + } else { + TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime); } } @@ -1140,225 +1007,6 @@ return true; } -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<car_model *>::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; -} - -static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum) -{ - for (int i = 0; i < mapNum && 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, const struct map_list *mapList, int mapNum) -{ - bool ret = false; - if (index < 0 || mapList == NULL || mapNum == 0) 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 int GetMapId(int index, const struct map_list *mapList, int mapNum) -{ - if (index < 0 || mapList == NULL || mapNum == 0) - return -1; - - return mapList[index].id; -} - -static int GetMapType(int index, const struct map_list *mapList, int mapNum) -{ - if (index < 0 || mapList == NULL || mapNum == 0) - return -1; - - return mapList[index].type; -} - void AddExamFault(int wrong, const struct RtkTime *rtkTime) { struct ExamFault fault; @@ -1383,11 +1031,6 @@ MA_SendExamWrong(ExamFaultList); ExamFaultList.clear(); -} - -car_model_cache_t *GetCarModelCache(int node) -{ - return NULL; } /******************************************************************* diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h index 0cc5aa0..acdb6c2 100644 --- a/lib/src/main/cpp/driver_test.h +++ b/lib/src/main/cpp/driver_test.h @@ -5,7 +5,7 @@ #ifndef RTKDRIVERTEST_DRIVER_TEST_H #define RTKDRIVERTEST_DRIVER_TEST_H -#include "Geometry.h" +#include "test_common/Geometry.h" #include "rtk_module/rtk.h" #include "test_items2/dummy_light.h" @@ -125,14 +125,26 @@ typedef vector<struct road_exam_map> LIST_ROAD_MAP; +struct area_exam_map { + int id; + int type; + Polygon map; + Polygon map2; +}; + +typedef vector<struct area_exam_map> LIST_AREA_MAP; + typedef list<car_model *> LIST_CAR_MODEL; //vector<ExamFault> ExamFaultList; void DriverTestInit(void); void ReadDriverExamPrimer(void); -void ClearMap(void); -void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2); + +void ClearAreaMap(void); +void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2); + + void CleanRoadMap(void); void SetRoadMapPoints(vector<double> &mapPoints); void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines, @@ -145,11 +157,10 @@ int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight); void StartDriverExam(int start, int type); -void StartMapExam(int map_id, int exam); void UpdateRTKInfo(const rtk_info *s); void AddExamFault(int wrong, const struct RtkTime *rtkTime); -car_model_cache_t *GetCarModelCache(int node); + void SystemShutdown(int event, int timeout); void SetDummyLightExam(int n, struct dummy_light_exam *cfg);; diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp index 90b4b1a..2dd5537 100644 --- a/lib/src/main/cpp/master/comm_if.cpp +++ b/lib/src/main/cpp/master/comm_if.cpp @@ -749,69 +749,73 @@ Document doc; doc.Parse(value); if (!doc.HasParseError()) { - ClearMap(); + ClearAreaMap(); - const Value &a = doc.GetArray(); + if (doc.HasMember("items")) { - if (a.IsArray()) { - for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) { - // a Map - int id, type, pointNum = 0, point2Num = 0; - double (*map)[2] = NULL, (*map2)[2] = NULL; + const Value &a = doc["items"]; - if (itr->IsObject()) { - if (itr->HasMember("id")) { - const Value &s = (*itr)["id"]; - id = s.GetInt(); - } - if (itr->HasMember("item")) { - const Value &s = (*itr)["item"]; - type = s.GetInt(); - } - if (itr->HasMember("point")) { - const Value &s = (*itr)["point"]; - int map_index = 0; + if (a.IsArray()) { + for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) { + // a Map + int id, type, pointNum = 0, point2Num = 0; + double (*map)[2] = NULL, (*map2)[2] = NULL; - for (Value::ConstValueIterator itr2 = s.Begin(); - itr2 != s.End(); ++itr2, ++map_index) { - // 鏇茬嚎椹鹃┒鏈�2缁� - const Value &s2 = (*itr2)["x-y"]; + if (itr->IsObject()) { + if (itr->HasMember("id")) { + const Value &s = (*itr)["id"]; + id = s.GetInt(); + } + if (itr->HasMember("item")) { + const Value &s = (*itr)["item"]; + type = s.GetInt(); + } + if (itr->HasMember("point")) { + const Value &s = (*itr)["point"]; + int map_index = 0; - if (map_index == 0) { - int i = 0, j = 0; - pointNum = s2.Size()/2; - map = (double (*)[2]) new double[pointNum][2]; + for (Value::ConstValueIterator itr2 = s.Begin(); + itr2 != s.End(); ++itr2, ++map_index) { + // 鏇茬嚎椹鹃┒鏈�2缁� + const Value &s2 = (*itr2)["x-y"]; + + if (map_index == 0) { + int i = 0, j = 0; + pointNum = s2.Size() / 2; + map = (double (*)[2]) new double[pointNum][2]; // map = (double (*)[2]) malloc(pointNum * 2 * sizeof(double)); - for (Value::ConstValueIterator itr3 = s2.Begin(); - itr3 != s2.End(); ++itr3) { - map[i][j] = (*itr3).GetDouble(); - if (++j == 2) { - j = 0; - i++; + for (Value::ConstValueIterator itr3 = s2.Begin(); + itr3 != s2.End(); ++itr3) { + map[i][j] = (*itr3).GetDouble(); + if (++j == 2) { + j = 0; + i++; + } } - } - } else if (map_index == 1) { - int i = 0, j = 0; - point2Num = s2.Size()/2; - map2 = (double (*)[2]) new double[s2.Size()][2]; + } else if (map_index == 1) { + int i = 0, j = 0; + point2Num = s2.Size() / 2; + map2 = (double (*)[2]) new double[s2.Size()][2]; // map2 = (double (*)[2]) malloc(point2Num * 2 * sizeof(double)); - for (Value::ConstValueIterator itr3 = s2.Begin(); - itr3 != s2.End(); ++itr3) { - map2[i][j] = (*itr3).GetDouble(); - if (++j == 2) { - j = 0; - i++; + for (Value::ConstValueIterator itr3 = s2.Begin(); + itr3 != s2.End(); ++itr3) { + map2[i][j] = (*itr3).GetDouble(); + if (++j == 2) { + j = 0; + i++; + } } } } } - } - AddMap(id, type, map, pointNum, map2, point2Num); - if (map) delete []map; - if (map2) delete []map2; + AddAreaMap(id, type, map, pointNum, map2, point2Num); + + if (map) delete[]map; + if (map2) delete[]map2; + } } } } @@ -1006,7 +1010,6 @@ Value& s = doc["map_id"]; Value& s2 = doc["exam"]; - StartMapExam(s.GetInt(), s2.GetInt()); } } break; diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp index cdc854e..206f071 100644 --- a/lib/src/main/cpp/native-lib.cpp +++ b/lib/src/main/cpp/native-lib.cpp @@ -9,7 +9,7 @@ #include "common/apptimer.h" #include "rtk_platform/parse_net.h" #include "native-lib.h" -#include "Geometry.h" +#include "test_common/Geometry.h" #include "rtk_platform/platform.h" #include "rtk_module/rtk.h" #include "mcu/mcu_if.h" diff --git a/lib/src/main/cpp/native-lib.h b/lib/src/main/cpp/native-lib.h index 60b05cd..d109ac4 100644 --- a/lib/src/main/cpp/native-lib.h +++ b/lib/src/main/cpp/native-lib.h @@ -6,7 +6,7 @@ #define RTKBASESTATION_NATIVE_LIB_H #include <cstdint> -#include "Geometry.h" +#include "test_common/Geometry.h" char * GetImei(void); diff --git a/lib/src/main/cpp/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp similarity index 95% rename from lib/src/main/cpp/Geometry.cpp rename to lib/src/main/cpp/test_common/Geometry.cpp index 0a43ae1..db6deb5 100644 --- a/lib/src/main/cpp/Geometry.cpp +++ b/lib/src/main/cpp/test_common/Geometry.cpp @@ -2,7 +2,7 @@ // Created by YY on 2019/4/30. // -#include "defs.h" +#include "../defs.h" #include "Geometry.h" #include <stdbool.h> #include <stdint.h> @@ -12,7 +12,7 @@ #include <initializer_list> #include <cctype> -#include "jni_log.h" +#include "../jni_log.h" using namespace std; @@ -245,7 +245,6 @@ Relation IntersectionOf(Line line1, Line line2) { - // Fail if either line segment is zero-length. if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2))) return GM_None; @@ -255,15 +254,14 @@ if ((isEqual(line1.X1, line2.X2) && isEqual(line1.Y1, line2.Y2)) || (isEqual(line1.X2, line2.X2) && isEqual(line1.Y2, line2.Y2))) return GM_Intersection; - // (1) Translate the system so that point A is on the origin. + // 鐩寸嚎鍧愭爣鍙樻崲閲嶅悎 line1.X2 -= line1.X1; line1.Y2 -= line1.Y1; line2.X1 -= line1.X1; line2.Y1 -= line1.Y1; line2.X2 -= line1.X1; line2.Y2 -= line1.Y1; - // Discover the length of segment A-B. double distAB = sqrt(line1.X2 * line1.X2 + line1.Y2 * line1.Y2); - // (2) Rotate the system so that point B is on the positive X axis. + // 鏃嬭浆鍒癤杞� double theCos = line1.X2 / distAB; double theSin = line1.Y2 / distAB; double newX = line2.X1 * theCos + line2.Y1 * theSin; @@ -274,19 +272,16 @@ line2.Y2 = line2.Y2 * theCos - line2.X2 * theSin; line2.X2 = newX; - // Fail if segment C-D doesn't cross line A-B. if ((line2.Y1 < 0 && line2.Y2 < 0) || (line2.Y1 >= 0 && line2.Y2 >= 0)) { return GM_None; } - // (3) Discover the position of the intersection point along line A-B. double posAB = line2.X2 + (line2.X1 - line2.X2) * line2.Y2 / (line2.Y2 - line2.Y1); - // Fail if segment C-D crosses line A-B outside of segment A-B. if (posAB < 0 || posAB > distAB) { return GM_None; } - // (4) Apply the discovered position to line A-B in the original coordinate system. + return GM_Intersection; } diff --git a/lib/src/main/cpp/Geometry.h b/lib/src/main/cpp/test_common/Geometry.h similarity index 100% rename from lib/src/main/cpp/Geometry.h rename to lib/src/main/cpp/test_common/Geometry.h diff --git a/lib/src/main/cpp/test_items/area_exam.cpp b/lib/src/main/cpp/test_items/area_exam.cpp index 8516dd1..e9bee0c 100644 --- a/lib/src/main/cpp/test_items/area_exam.cpp +++ b/lib/src/main/cpp/test_items/area_exam.cpp @@ -3,3 +3,339 @@ // #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("<area_exam> <%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<car_model *>::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; +} diff --git a/lib/src/main/cpp/test_items/area_exam.h b/lib/src/main/cpp/test_items/area_exam.h index a562acf..fe00467 100644 --- a/lib/src/main/cpp/test_items/area_exam.h +++ b/lib/src/main/cpp/test_items/area_exam.h @@ -5,4 +5,9 @@ #ifndef MYAPPLICATION2_AREA_EXAM_H #define MYAPPLICATION2_AREA_EXAM_H +#include "../driver_test.h" + +void InitAreaExam(void); +void TerminateAreaExam(void); +void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime); #endif //MYAPPLICATION2_AREA_EXAM_H diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp index e200125..b4546f6 100644 --- a/lib/src/main/cpp/test_items/park_bottom.cpp +++ b/lib/src/main/cpp/test_items/park_bottom.cpp @@ -4,7 +4,7 @@ #include "park_bottom.h" #include "../common/apptimer.h" -#include "../Geometry.h" +#include "../test_common/Geometry.h" #include "../native-lib.h" #include "../jni_log.h" #include "../driver_test.h" diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h index 69cd14e..695ba04 100644 --- a/lib/src/main/cpp/test_items/park_bottom.h +++ b/lib/src/main/cpp/test_items/park_bottom.h @@ -5,7 +5,7 @@ #ifndef RTKDRIVERTEST_PARK_BOTTOM_H #define RTKDRIVERTEST_PARK_BOTTOM_H -#include "../Geometry.h" +#include "../test_common/Geometry.h" #include "../driver_test.h" #include <vector> diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp index 6836600..9387896 100644 --- a/lib/src/main/cpp/test_items/park_edge.cpp +++ b/lib/src/main/cpp/test_items/park_edge.cpp @@ -4,7 +4,7 @@ #include "park_edge.h" #include "../jni_log.h" -#include "../Geometry.h" +#include "../test_common/Geometry.h" #include "../driver_test.h" #include "../common/apptimer.h" #include "../native-lib.h" diff --git a/lib/src/main/cpp/test_items/park_edge.h b/lib/src/main/cpp/test_items/park_edge.h index 8916ff8..e918c33 100644 --- a/lib/src/main/cpp/test_items/park_edge.h +++ b/lib/src/main/cpp/test_items/park_edge.h @@ -5,7 +5,7 @@ #ifndef RTKDRIVERTEST_PARK_EDGE_H #define RTKDRIVERTEST_PARK_EDGE_H -#include "../Geometry.h" +#include "../test_common/Geometry.h" #include "../driver_test.h" #include <vector> diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp index 4b3ba68..5f19758 100644 --- a/lib/src/main/cpp/test_items/turn_a90.cpp +++ b/lib/src/main/cpp/test_items/turn_a90.cpp @@ -3,7 +3,7 @@ // #include "turn_a90.h" -#include "../Geometry.h" +#include "../test_common/Geometry.h" #include "../driver_test.h" #include "../common/apptimer.h" #include "../jni_log.h" diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp index 613a362..b67e989 100644 --- a/lib/src/main/cpp/test_items2/through_something.cpp +++ b/lib/src/main/cpp/test_items2/through_something.cpp @@ -4,7 +4,7 @@ #include "through_something.h" #include "../driver_test.h" -#include "../Geometry.h" +#include "../test_common/Geometry.h" #include "../native-lib.h" #include "../jni_log.h" #include "../test_common/car_sensor.h" -- Gitblit v1.8.0