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.h | 1 lib/src/main/cpp/driver_test.cpp | 27 +++++- lib/src/main/cpp/master/comm_if.cpp | 2 lib/src/main/cpp/native-lib.cpp | 2 lib/src/main/cpp/test_items2/through_something.cpp | 24 +++++ lib/src/main/cpp/test_items2/road_exam.cpp | 133 ++++++++++++++++++++++++++++----- lib/src/main/cpp/driver_test.h | 2 7 files changed, 159 insertions(+), 32 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index 8601f17..fae8d3b 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -277,7 +277,7 @@ } } -void AddRoadMapParent(int id, int type, string tts, +void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines, vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, @@ -290,8 +290,13 @@ newMap.id = id; newMap.type = type; newMap.tts = tts; + newMap.flagStop = stopFlag; DEBUG("鍔犲叆璺�冨湴鍥句俊鎭� id = %d type = %d", id, type); + + if (!tts.empty()) { + DEBUG("TTS 淇℃伅 = %s", tts.c_str()); + } if ((newMap.redLineNum = redLines.size()) > 0) { newMap.redLine = (Polygon *) malloc(sizeof(Polygon) * newMap.redLineNum); @@ -357,7 +362,7 @@ newMap.triggerLine[i].line.num = triggerLines[i].size() - 1; newMap.triggerLine[i].line.point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].line.num); - DEBUG(" 缁撶偣 %d 涓�", newMap.triggerLine[i].line.num); + DEBUG(" 缁撶偣 %d 涓�, 瑙﹀彂 id = %d", newMap.triggerLine[i].line.num, triggerLines[i][0]); newMap.triggerLine[i].triggerMapId = triggerLines[i][0]; for (int j = 0; j < newMap.triggerLine[i].line.num; ++j) { @@ -497,7 +502,7 @@ { DEBUG("鑾峰彇妯℃嫙璺�冪伅鍏夋祴璇曢」鐩� N = %d %d", n, ExamStart); -// if (ExamStart) return; + if (ExamStart) return; if (DummyLightContent != NULL) { free(DummyLightContent); @@ -538,8 +543,8 @@ } if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) { DEBUG("娌℃湁妯℃嫙鐏厜"); - err = true; - MA_SendExamStatus(0, -3); +// err = true; +// MA_SendExamStatus(0, -3); } if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) { DEBUG("娌℃湁璺�冨湴鍥�"); @@ -559,6 +564,8 @@ if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) { exam_dummy_light = 0; + +// InitRoadExam(); //////////////////// } } MA_SendExamStatus(1, 0); @@ -733,16 +740,18 @@ if (ExamType == TEST_TYPE_AREA) AddExamFault(4, rtkTime); } -DEBUG("22222222222222222222"); + if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) { if (exam_dummy_light == 0) { StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime); exam_dummy_light = 1; // 姹囨姤鐏厜鑰冭瘯寮�濮� + DEBUG("鐏厜鑰冭瘯寮�濮�"); } else if (exam_dummy_light == 1) { exam_dummy_light = ExecuteDummyLightExam(rtkTime); // 姹囨姤鐏厜鑰冭瘯缁撴潫 if (exam_dummy_light == 2) { + DEBUG("鐏厜鑰冭瘯缁撴潫"); InitRoadExam(); } } @@ -912,6 +921,7 @@ tm.DD = RtkBuffer[p1].DD; tm.hh = RtkBuffer[p1].hh; tm.mm = RtkBuffer[p1].mm; + tm.ss = RtkBuffer[p1].ss; tm.mss = RtkBuffer[p1].dss; UpdateCarBodyCoord(&tm, RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel); @@ -1210,6 +1220,11 @@ fault.sn = examFaultIndex++; sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); + + if (ExamType != TEST_TYPE_AREA) { + wrong += 1000; + } + fault.wrong_id = wrong; DEBUG("鑰冭瘯鍙戠敓閿欒 %d %s", wrong, fault.utc); diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h index ca11dee..b590054 100644 --- a/lib/src/main/cpp/driver_test.h +++ b/lib/src/main/cpp/driver_test.h @@ -124,7 +124,7 @@ void AddMap(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, vector<vector<int>> &redLines, +void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines, vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines, vector<int> area, vector<int> stopLine); diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp index cf9abe8..fddeb5c 100644 --- a/lib/src/main/cpp/master/comm_if.cpp +++ b/lib/src/main/cpp/master/comm_if.cpp @@ -633,7 +633,7 @@ tts = s.GetString(); } - AddRoadMapParent(id, type, tts, redLines, redAreas, greenLines, triggerLines, area, stopLine); + AddRoadMapParent(id, type, tts, stop_flag, redLines, redAreas, greenLines, triggerLines, area, stopLine); } } } diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp index 9cc7f3c..e19a097 100644 --- a/lib/src/main/cpp/native-lib.cpp +++ b/lib/src/main/cpp/native-lib.cpp @@ -26,7 +26,7 @@ const int RTK_PLATFORM_PORT = 12125; const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01}; -const char *VIRTUAL_RTK_IP = "192.168.3.52"; +const char *VIRTUAL_RTK_IP = "192.168.16.100"; const int VIRTUAL_RTK_PORT = 9001; static void SendBootIndicate(union sigval sig); 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; } diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h index 16d0fd2..2b9d29c 100644 --- a/lib/src/main/cpp/test_items2/road_exam.h +++ b/lib/src/main/cpp/test_items2/road_exam.h @@ -22,5 +22,6 @@ void InitRoadExam(void); void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car); +bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList); #endif //MYAPPLICATION2_ROAD_EXAM_H diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp index 33fe453..f0fa865 100644 --- a/lib/src/main/cpp/test_items2/through_something.cpp +++ b/lib/src/main/cpp/test_items2/through_something.cpp @@ -17,17 +17,22 @@ static int breakActive; static int stopActive; +static bool crashRedLine; void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList) { if (index == -1) return; - DEBUG("杩涘叆璺�冨瓙鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); + DEBUG("杩涘叆璺�冮�氳繃something鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); if (!RoadMapList[index].tts.empty()) { + DEBUG("鎾斁TTS"); PlayTTS(RoadMapList[index].tts.c_str(), 0); + } else { + DEBUG("娌℃湁TTS"); } breakActive = 0; stopActive = 0; + crashRedLine = false; } int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, @@ -99,6 +104,7 @@ default: break; } + stopActive = -1; } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) { if (moveDirect == 0) { stopActive = 1; @@ -106,9 +112,21 @@ } } - if (ExitSonArea(index, RoadMapList, car)) + if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) { + if (!crashRedLine) { + DEBUG("涓嶆寜鑰冭瘯鍛樻寚浠よ椹�"); + crashRedLine = true; + // 涓嶆寜鑰冭瘯鍛樻寚浠よ椹� + AddExamFault(3, rtkTime); + } + } else { + crashRedLine = false; + } + + if (ExitSonArea(index, RoadMapList, car)) { + DEBUG("绂诲紑閫氳繃something鍖哄煙"); return -1; + } return index; } - -- Gitblit v1.8.0