From 7fdc4fa80d5d04b5936fc1bdd617b64c6ae9ef37 Mon Sep 17 00:00:00 2001
From: lizhanwei <Dana_Lee1016@126.com>
Date: 星期一, 26 四月 2021 13:17:00 +0800
Subject: [PATCH] Merge branch 'master' of https://gitee.com/endian11/DriveJudge
---
lib/src/main/cpp/test_items/park_edge.cpp | 134 +++++++++++++++++++++++++++++++++++---------
1 files changed, 107 insertions(+), 27 deletions(-)
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index e7b9366..cf155ed 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -20,6 +20,12 @@
using namespace std;
+enum {
+ TESTING,
+ TEST_FAIL, // 鍥犺Е鍙戞煇浜涜鍒欙紝鍦ㄨ溅韬湭瀹屽叏绔嬪満鎯呭喌涓嬶紝鎻愬墠缁堟閮ㄥ垎娴嬭瘯
+ TEST_FINISH
+};
+
const uint32_t CHECK_PARK_DELAY = 400;
static int mapIndex = 0;
@@ -30,9 +36,10 @@
static bool occurCrashRedLine1, occurCrashRedLine2;
static int prevMoveStatus, storeMoveStatusBeforeStop;
static int parkStatus;
-static bool occurMoveBack, parkSuccess;
+static int gearAtStop;
+static bool occurMoveBack, checkPark, parkSuccess, checkLight;
static uint32_t moveBackTimePoint;
-static bool testing = false;
+static int testStatus;
static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
@@ -44,7 +51,7 @@
{
DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
- testing = true;
+ testStatus = TESTING;
mapIndex = index;
occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
@@ -54,12 +61,16 @@
parkSuccess = false;
parkStatus = 0;
occurMoveBack = false;
+ checkPark = false;
+ checkLight = false;
+ gearAtStop = -1;
+ PlayTTS("鎮ㄥ凡杩涘叆渚ф柟鍋滆溅鍖哄煙", NULL);
// 浠呭綋鍙戠敓鍊掕溅锛屾墠鎰忓懗鐫�椤圭洰寮�濮�
- if (moveStatus == -1) {
+ /*if (moveStatus == -1) {
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- }
+ }*/
}
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
@@ -67,9 +78,39 @@
vector<double> dtox;
vector<Line> line_set;
Line distance_line;
+ bool is_gear_r = false;
+
+ // 棣栨鎸傚�掓尅锛� 鎵嶆剰鍛崇潃椤圭洰寮�濮�
+ if (testStatus == TESTING) {
+ if (ReadCarStatus(GEAR) == GEAR_R) {
+ is_gear_r = true;
+ if (!occurMoveBack) {
+ occurMoveBack = true;
+ moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
+ rtkTime->mss * 10); // 寮�濮嬭鏃�
+ MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
+ }
+ } else {
+ is_gear_r = false;
+ if (occurMoveBack && !checkPark) {
+ // 妫�鏌ヨ溅韬叆搴撴儏鍐�
+ DEBUG("妫�鏌ヨ溅韬叆搴撴儏鍐�");
+ checkPark = true;
+ if (EnterParking(map, car)) {
+ parkStatus = 1;
+ parkSuccess = true;
+ } else {
+ // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
+ AddExamFault(20401, rtkTime);
+ parkSuccess = false;
+ DEBUG("绉诲簱涓嶅叆");
+ }
+ }
+ }
+ }
if (CrashRedLine1(map, car)) {
- if (!occurCrashRedLine1 && occurMoveBack) {
+ if (!occurCrashRedLine1 /*&& occurMoveBack*/) {
// 杞﹁疆鍘嬭竟绾匡紝姣忔鎵�10鍒�
AddExamFault(20403, rtkTime);
DEBUG("杞﹁疆鍘嬭竟绾�");
@@ -80,7 +121,7 @@
}
if (CrashRedLine2(map, car)) {
- if (!occurCrashRedLine2 && occurMoveBack) {
+ if (!occurCrashRedLine2 /*&& occurMoveBack*/) {
// 杞﹁韩鍘嬪簱浣嶇嚎锛屾瘡娆℃墸10鍒�
AddExamFault(20404, rtkTime);
DEBUG("杞﹁韩鍘嬪簱浣嶇嚎");
@@ -90,14 +131,20 @@
occurCrashRedLine2 = false;
}
- if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
- if (!parkSuccess && occurMoveBack && !reportParkFail) {
+ if (ExitParkArea2(map, car)) {
+ /*if (!parkSuccess && occurMoveBack && !reportParkFail) {
// 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
AddExamFault(10103, rtkTime);
reportParkFail = true;
DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ }*/
+ if (occurMoveBack && !checkPark) {
+ // 鍊掕溅鐩存帴椹剁娴嬭瘯鍖�
+ AddExamFault(10103, rtkTime);
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
}
- testing = false;
+
+ testStatus = TEST_FINISH;
goto TEST_END;
}
@@ -117,6 +164,13 @@
DistanceOfTire2X(dtox, car, line_set);
MA_SendDistance(dtox[0], dtox[1]);
+ if (testStatus == TESTING && !occurMoveBack && ExitParkArea(map, car)) {
+ // 鍏ュ簱鍚庝竴鐩村墠杩涳紝杞﹀ご绉诲嚭椹剁绾�
+ AddExamFault(10103, rtkTime);
+ DEBUG("鐩存帴椹剁娴嬭瘯鍖�,涓嶆寜鑰冭瘯鍛樻寚浠ら┚椹�");
+ testStatus = TEST_FAIL;
+ }
+
if (occurMoveBack) {
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
@@ -133,15 +187,33 @@
parkStatus = 0;
stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
storeMoveStatusBeforeStop = prevMoveStatus;
-
+ gearAtStop = is_gear_r ? 1 : 0;
DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
- } else {
+ } else if (prevMoveStatus == 0) {
DEBUG("缁х画琛岄┒ %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
DEBUG("鍋滆溅鏃堕棿 %ld", tp - stopTimepoint);
- if (moveStatus == storeMoveStatusBeforeStop) {
+ if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria)
+ && occurMoveBack
+ && gearAtStop == (is_gear_r ? 1 : 0)) {
+ // 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
+ AddExamFault(20406, rtkTime);
+ DEBUG("鍋滆溅瓒呮椂");
+ }
+
+ if (moveStatus == 1 && checkPark && !checkLight) {
+ // 鍦ㄨ繖閲屾鏌ヨ浆鍚戠伅鐘舵��
+ checkLight = true;
+ if (ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT) {
+ // 涓嶅紑杞悜鐏紝鎵�10鍒�
+ AddExamFault(20405, rtkTime);
+ DEBUG("鏈紑鍚浆鍚戠伅");
+ }
+ }
+
+ /*if (moveStatus == storeMoveStatusBeforeStop) {
// 鍚屾柟鍚戝啀鍚姩锛岀户缁垽鏂槸鍚﹀仠杞﹁秴鏃�
if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_edge_pause_criteria) && occurMoveBack) {
// 鍋滆溅瓒�2绉掞紝姣忔鎵�5鍒�
@@ -172,18 +244,18 @@
DEBUG("鏈紑鍚浆鍚戠伅");
}
}
- }
+ }*/
- if (moveStatus == -1 && !occurMoveBack) {
+ /* if (moveStatus == -1 && !occurMoveBack) {
DEBUG("寮�濮嬪�掕溅");
occurMoveBack = true;
moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
- }
+ }*/
}
prevMoveStatus = moveStatus;
- } else if (moveStatus == 0 && parkStatus == 0) {
+ } /*else if (moveStatus == 0 && parkStatus == 0) {
uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
@@ -194,13 +266,15 @@
parkStatus = -1;
}
}
- }
+ }*/
TEST_END:
- if (!testing && occurMoveBack) {
+ if (testStatus == TEST_FINISH) {
+ DEBUG("渚ф柟鍋滆溅缁撴潫");
MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
+ return 0;
}
- return testing ? 1 : 0;
+ return 1;
}
// 杞﹁疆鏄惁鍘嬮亾璺竟绾�
@@ -234,7 +308,7 @@
bool ret = false;
Line red_line;
- const int red_lines[][2] = {{2, 3}, {3, 4}, {4, 5}};
+ const int red_lines[][2] = {{0, 7}, {2, 3}, {3, 4}, {4, 5}};
Polygon car_body;
@@ -249,6 +323,14 @@
if (IntersectionOf(red_line, &car_body) != GM_None) {
ret = true;
break;
+ }
+ }
+
+ if (!occurMoveBack) {
+ // 鍊掕溅鍓嶏紝杞﹁韩涓嶅緱鍘嬪簱浣嶈櫄绾�
+ MakeLine(&red_line, &map->point[2], &map->point[5]);
+ if (IntersectionOf(red_line, &car_body) != GM_None) {
+ ret = true;
}
}
@@ -282,14 +364,12 @@
return succ;
}
-// 鏁翠釜杞﹁締閮借椹惰繃鍓嶅簱浣嶇嚎
+// 杞﹀ご瑕侀┒杩囧墠搴撲綅绾�
static bool ExitParkArea(const Polygon *map, const car_model *car)
{
- for (int i = 0; i < car->bodyNum; ++i) {
- if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->body[i]]) != -1)
- return false;
- }
- return true;
+ if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[ car->axial[AXIAL_FRONT] ]) == -1)
+ return true;
+ return false;
}
static bool ExitParkArea2(const Polygon *map, const car_model *car)
--
Gitblit v1.8.0