From 24428a1b69afcf113883e9cede1159443201a1b8 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期四, 02 四月 2020 18:57:07 +0800 Subject: [PATCH] 进出科目二区域调整 --- lib/src/main/cpp/driver_test.cpp | 157 +++++++++++++++++++++++++++++++++------------------- 1 files changed, 99 insertions(+), 58 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index 0af9a65..9f47645 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -40,12 +40,6 @@ using namespace std; -#define MAP_TYPE_PARK_BUTTOM 1 -#define MAP_TYPE_STOP_START 2 -#define MAP_TYPE_PART_EDGE 3 -#define MAP_TYPE_CURVE 4 -#define MAP_TYPE_TURN_90 5 - #define RTK_INVALID 0 #define RTK_SINGLE_POINT 1 #define RTK_DIFF 2 @@ -117,7 +111,8 @@ 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 int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum); +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); @@ -650,7 +645,7 @@ RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss); brief.qf = RtkBuffer[index].qf; - brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum); + brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum); brief.move = move; brief.speed = speed * 3.6; brief.heading = RtkBuffer[index].heading; @@ -712,18 +707,18 @@ } if (CurrExamMapIndex < 0) { if (CurrEnterMapIndex < 0) { - CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum); + 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); +// 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); +// DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); +// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0); CurrEnterMapIndex = -1; } } @@ -894,33 +889,33 @@ DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴"); MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartParkBottom(move, rtkTime); + StartParkBottom(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_STOP_START: DEBUG("杩涘叆涓婂潯璧锋鍦哄湴"); MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartSAS(move, rtkTime); + StartSAS(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_PART_EDGE: DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�"); MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartParkEdge(move, rtkTime); + StartParkEdge(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_CURVE: DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴"); MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartDrivingCurve(move, rtkTime); + StartDrivingCurve(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_TURN_90: DEBUG("杩涘叆鐩磋杞集鍦哄湴"); MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartTurnA90(move, azimuth, rtkTime); + StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; default:break; @@ -1145,15 +1140,79 @@ return true; } -static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum) +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; - - bool enter = false; 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; @@ -1161,31 +1220,18 @@ x9 = 2*xo - mapList[i].map.point[8].X; y9 = 2*yo - mapList[i].map.point[8].Y; - Polygon map; + Line triggerLine; - map.num = 4; - map.point = (PointF *) malloc(map.num * sizeof(PointF)); + triggerLine.X1 = mapList[i].map.point[0].X; + triggerLine.Y1 = mapList[i].map.point[0].Y; + triggerLine.X2 = x9; + triggerLine.Y2 = y9; - map.point[0] = mapList[i].map.point[0]; - map.point[1] = mapList[i].map.point[8]; - map.point[2] = mapList[i].map.point[7]; - map.point[3].X = x9; - map.point[3].Y = y9; - - if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) { - Line enterLine1; - - MakeLine(&enterLine1, &(map.point[0]), &(map.point[3])); - - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1) - enter = true; - } - - free(map.point); - - if (enter) return i; + 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; @@ -1198,6 +1244,7 @@ } } if (mapList[i].type == MAP_TYPE_PART_EDGE) { + // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴 if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { Line enterLine; @@ -1208,26 +1255,20 @@ } } if (mapList[i].type == MAP_TYPE_TURN_90) { - if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { - Line enterLine1; + // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾� + Line triggerLine; - MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); + MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1) - return i; - } + if (CrashTriggerLine(triggerLine, car, CarModelList)) + return i; } if (mapList[i].type == MAP_TYPE_CURVE) { - Line startLine; - Line carAxial; + Line triggerLine; - MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]); - MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]); - - if (IntersectionOf(startLine, carAxial) == GM_Intersection) { - if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1) - return i; - } + MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]); + if (CrashTriggerLine(triggerLine, car, CarModelList)) + return i; } } return -1; -- Gitblit v1.8.0