From 1101dc614acb3cd794c13cd68c9a24d7c353cb28 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期六, 21 三月 2020 15:56:03 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/test_items2/road_exam.cpp | 133 +++++++++++++++++++++++++++++++++++++------ 1 files changed, 113 insertions(+), 20 deletions(-) diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp index 51fdf57..ae82561 100644 --- a/lib/src/main/cpp/test_items2/road_exam.cpp +++ b/lib/src/main/cpp/test_items2/road_exam.cpp @@ -11,6 +11,7 @@ #include "../test_common/car_sensor.h" #include "../native-lib.h" #include "through_something.h" +#include "../master/comm_if.h" #include <vector> #include <list> @@ -21,10 +22,10 @@ using namespace std; -#define TURN_CHECK_CNT 5 +#define TURN_CHECK_CNT 4 -static const int TURN_THRESHOLD = 10; -static const int TURN_CHECK_INTERVAL = D_SEC(1); +static const int TURN_THRESHOLD = 5; +static const int TURN_CHECK_INTERVAL = 500; const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; @@ -83,6 +84,8 @@ void InitRoadExam(void) { + DEBUG("Start road_exam"); + crashGreenCmpTime.hour = -1; occurCrashRedLine = false; occurCrashGreenLine = false; @@ -209,9 +212,9 @@ uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) { - // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯 + // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸 AddExamFault(16, rtkTime); - DEBUG("涓�斿仠杞�"); + DEBUG("绂佸仠鍖哄仠杞�"); reportStopCarOnRedArea = true; } } else if (moveDirect == -1) { @@ -263,6 +266,7 @@ // 妫�鏌ユ槸鍚︽寔缁浆鍚� char turnDirect = CheckCarTurn(CarModelList); if (turnDirect == 'L') { + PlayTTS("宸�1", 5692); if (currTurnSignalStatus != LEFT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("娌℃墦宸﹁浆鐏�"); @@ -280,6 +284,7 @@ } } } else if (turnDirect == 'R') { + PlayTTS("鍙�1", 5692); if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("娌℃墦鍙宠浆鐏�"); @@ -304,6 +309,7 @@ if (CrashRedLine(RoadMapList, car)) { if (!occurCrashRedLine) { // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍� + DEBUG("鎾為亾璺竟缂樼嚎"); AddExamFault(11, rtkTime); occurCrashRedLine = true; } @@ -324,6 +330,7 @@ crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10); if (diff >= CRASH_DOTTED_LINE_TIMEOUT) { + DEBUG("闀挎椂闂村帇铏氱嚎"); checkCrashGreenTimeout = 2; // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸 AddExamFault(12, rtkTime); @@ -368,6 +375,7 @@ crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10); if (diff < CHANGE_ROAD_MIN_INTERVAL) { + DEBUG("===================== 杩炵画鍙橀亾 ============!!"); // 杩炵画鍙橀亾锛屼笉鍚堟牸 AddExamFault(15, rtkTime); } @@ -378,16 +386,20 @@ // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅 if (inter == 1) { + PlayTTS("宸�2", 5698); // 鍚戝乏渚у彉閬� DEBUG("鍚戝乏渚у彉閬�"); if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) { + DEBUG("鍙樿皟鏈墦鐏�!!"); // 娌℃墦鐏紝涓嶅悎鏍� AddExamFault(13, rtkTime); } } else { + PlayTTS("鍙�2", 5698); // 鍚戝彸渚у彉閬� DEBUG("鍚戝彸渚у彉閬�"); if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) { + DEBUG("鍙樿皟鏈墦鐏�!!"); // 娌℃墦鐏紝涓嶅悎鏍� AddExamFault(13, rtkTime); } @@ -402,10 +414,20 @@ if (currExamMapIndex == -1) { currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList); if (currExamMapIndex != -1) { + DEBUG("纰版挒瑙﹀彂绾�"); + + MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1); StartThroughExam(currExamMapIndex, RoadMapList); } } else { + int prevIdx = currExamMapIndex; + currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car, + CarModelList, speed, moveDirect, rtkTime); + + if (currExamMapIndex == -1) { + MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1); + } } } @@ -436,6 +458,61 @@ return ret; } +bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList) +{ + bool ret = false; + + if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size()) + return ret; + + Polygon trace; + + trace.num = 5; // 鏈�杩�5涓疆杞ㄨ抗 + trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); + + list<car_model *>::iterator iter = CarModelList.begin(); + + int pn = 0; + while (iter != CarModelList.end() && pn < trace.num) { + trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]]; + ++iter; + } + + // 姣忔潯绾块兘妫�娴� + for (int j = 0; j < RoadMapList[index].redLineNum; ++j) { + Line red_line; + + int kp = 0; + + // 瑙﹀彂绾夸竴鑸簲璇ュ彧鏈夐灏�2鐐癸紙id, p1, p2锛� + for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) { + MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp], + &RoadMapList[index].redLine[j].point[k]); + + int pp = 0; + for (int p = 1; p < pn; ++p) { + Line trace_line; + MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); + + if (IntersectionOf(trace_line, red_line) == GM_Intersection) { + // 纰板埌瑙﹀彂绾� + ret = true; + goto SEARCH_SON_RED_LINE_END; + } + + pp = p; + } + + kp = k; + } + } + + SEARCH_SON_RED_LINE_END: + free(trace.point); + + return ret; +} + static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) { tm.hour = rtkTime->hh; @@ -446,18 +523,24 @@ static char isTurn(int currYaw, int prevYaw) { +// DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw); + + int deltaAng = 0; + if (ABS(currYaw - prevYaw) > 180) { - currYaw = 360 - ABS(currYaw-prevYaw); + deltaAng = 360 - ABS(currYaw-prevYaw); } else { - currYaw = ABS(currYaw - prevYaw); + deltaAng = ABS(currYaw - prevYaw); } - if (currYaw >= TURN_THRESHOLD) { +// DEBUG("瑙掑害宸�� %d", deltaAng); + + if (deltaAng >= TURN_THRESHOLD) { if((( currYaw + 360 - prevYaw) % 360) < 180) { - DEBUG("鍙宠浆"); +// DEBUG("鍙宠浆"); return 'R'; } else { - DEBUG("宸﹁浆"); +// DEBUG("宸﹁浆"); return 'L'; } } @@ -480,17 +563,22 @@ char turn[TURN_CHECK_CNT] = {0}; int checkCnt = 0; - while (iter != CarModelList.end()) { +// DEBUG("CheckCarTurn........."); + + while (iter != CarModelList.end() && checkCnt < TURN_CHECK_CNT) { 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 >= TURN_CHECK_INTERVAL) { - turn[checkCnt++] = isTurn((int)c1->yaw, (int)c2->yaw); - c1 = c2; - - if (checkCnt == TURN_CHECK_CNT) + turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw); +// DEBUG("%c 瑙掑害姣旇緝 %02d:%02d:%02d.%03d %02d:%02d:%02d.%03d", turn[checkCnt], 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 (turn[checkCnt] == 0) { break; + } + + c1 = c2; + checkCnt++; } ++iter; @@ -502,8 +590,10 @@ break; } - if (i == TURN_CHECK_CNT-1) + if (i == TURN_CHECK_CNT-1) { + DEBUG("宸﹀彸杞‘璁� %c", turn[0]); return turn[0]; + } return 0; } @@ -659,15 +749,18 @@ for (int k = 1; k < RoadMapList[i].triggerLine[j].line.num; ++k) { MakeLine(&trigger_line, &RoadMapList[i].triggerLine[j].line.point[kp], &RoadMapList[i].triggerLine[j].line.point[k]); - int pp = 1; - for (int p = 2; p < pn; ++p) { + int pp = 0; + for (int p = 1; p < pn; ++p) { Line trace_line; MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); if (IntersectionOf(trace_line, trigger_line) == GM_Intersection && - IntersectionOfLine(trace.point[pp], trace.point[p], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && - DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trace_line) > 0.1) { + IntersectionOfLine(RoadMapList[i].triggerLine[j].line.point[kp], + RoadMapList[i].triggerLine[j].line.point[k], + car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && + DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trigger_line) > 0.1) { // 纰板埌瑙﹀彂绾� + DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘 id = %d", RoadMapList[i].triggerLine[j].triggerMapId); map_idx = FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList); goto SEARCH_TRIGGER_LINE_END; } -- Gitblit v1.8.0