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