From fb7b0660a319a9c54ff35c3944548348fae11b60 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 24 八月 2020 18:53:23 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 147 +++++++++++++++++++++++++++++++++++-------------
1 files changed, 107 insertions(+), 40 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index e4e8c9a..5913dc5 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -79,8 +79,8 @@
static int prevMoveDirect;
-static uint32_t stopTimepoint = 0;
-static bool reportStopCarOnRedArea;
+
+static bool StopCarOnRedArea;
static PointF stopPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;
@@ -89,7 +89,7 @@
static bool slideNormalDistance;
static bool occurSlide;
-static struct RtkTime crashGreenRunTime, crashGreenStartTime;
+static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
@@ -150,6 +150,7 @@
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
+static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);
static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
@@ -169,7 +170,7 @@
prevMoveDirect = 0;
- reportStopCarOnRedArea = false;
+ StopCarOnRedArea = false;
occurSlide = false;
slideLongDistance = false;
slideNormalDistance = false;
@@ -611,6 +612,47 @@
}
}
+ for (int n = 0; n < RoadMap.forbidLines.size(); ++n) {
+ // 鍦板浘鍥存爮
+ if (RoadMap.forbidLines[n].type == LINE_BOUNDARY && track && RoadMap.forbidLines[n].points.size() >= 2) {
+ p1 = RoadMap.forbidLines[n].points[0];
+ for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
+ p2 = RoadMap.forbidLines[n].points[m];
+ MakeLine(&redLine, &p1, &p2);
+ if (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection) {
+ lineType = LINE_BOUNDARY;
+ goto CRASH_LINE_CONFIRM;
+ }
+ p1 = p2;
+ }
+ }
+ // 鍏跺畠绾㈢嚎
+ if (RoadMap.forbidLines[n].type == LINE_SOLID && RoadMap.forbidLines[n].points.size() >= 2) {
+ p1 = RoadMap.forbidLines[n].points[0];
+ for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
+ p2 = RoadMap.forbidLines[n].points[m];
+ MakeLine(&redLine, &p1, &p2);
+
+ if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
+ IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
+ }
+
+ if (track &&
+ (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
+ IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
+ lineType = LINE_SOLID;
+ goto CRASH_LINE_CONFIRM;
+ }
+ p1 = p2;
+ }
+ }
+ }
+
CRASH_LINE_CONFIRM:
if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) {
if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) {
@@ -826,6 +868,12 @@
DEBUG("鎾為亾璺竟缂樼嚎");
AddExamFault(11, rtkTime);
}
+ if (newLineType == LINE_BOUNDARY && CrashLineType != LINE_BOUNDARY) {
+ // 杞﹁締瓒婄晫锛岄�冭窇浜嗭紝涓嶅悎鏍�
+ DEBUG("杞﹁締瓒婄晫");
+ AddExamFault(3, rtkTime);
+ }
+
if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
if (!crashDottedLine) {
checkCrashGreenTimeout = 0;
@@ -1163,7 +1211,7 @@
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
- double BigStraightRoadFree = 0, RoadCrossingFree = 0;
+ double BigStraightRoadFree = 0, RoadCrossingFree = 0, TargetFree = 0;
UpdateCarSensor(rtkTime);
@@ -1269,8 +1317,8 @@
// 璧锋鍚庢粦
if (moveDirect != prevMoveDirect) {
if (moveDirect == 0) {
- stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
- reportStopCarOnRedArea = false;
+ stopTimepoint = *rtkTime;
+ StopCarOnRedArea = false;
DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
@@ -1291,14 +1339,12 @@
prevMoveDirect = moveDirect;
} else if (moveDirect == 0) {
// 鎸佺画鍋滆溅
- uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
-
- /*if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
+ if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea && StopOnRedArea(RoadMap, car)) {
// 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
AddExamFault(16, rtkTime);
DEBUG("绂佸仠鍖哄仠杞�");
- reportStopCarOnRedArea = true;
- }*/
+ StopCarOnRedArea = true;
+ }
} else if (moveDirect == -1) {
// 鎸佺画鍚庢粦
if (occurSlide) {
@@ -1336,12 +1382,12 @@
if (currExamMapIndex >= 0) {
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
// 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
- ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
+ TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
}
ExitTarget(RoadMap, car, CarModelList, rtkTime);
oldid = CrashLineType;
-
+ // 妫�娴嬪帇绾跨姸鎬�
DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
if (oldid != CrashLineType) {
@@ -1385,15 +1431,13 @@
RoadCrossingFree = freeSepDis;
-// DEBUG("绱琛岄┒璺濈 %f, 鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f", ReadOdo(), BigStraightRoadFree, RoadCrossingFree);
+// DEBUG("杞﹂�� %f, 绱琛岄┒璺濈 %f, 鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f 鐗规畩鍖哄煙璺濈 %f", ConvertMs2KMh(speed), ReadOdo(), BigStraightRoadFree, RoadCrossingFree, TargetFree);
}
-
- // 妫�娴嬪帇绾跨姸鎬�
// 棰濆鐨勮浆鍚戞娴�
DetectTurn(car, moveDirect, rtkTime);
- ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);
+ ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
}
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
@@ -1643,39 +1687,62 @@
/**********************************************************
* 鎸夋暣涓溅韬槸鍚﹁鐩栬绠�
- * @param RoadMapList
+ * @param RoadMap
* @param car
* @return
*/
-/*static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
+static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car)
{
- bool ret = false;
+ for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
+ Polygon redArea;
+ redArea.num = 0;
- Polygon carBody;
+ if (RoadMap.specialAreas[i].type == GRID_AREA && RoadMap.specialAreas[i].area.size() == 4) {
+ redArea.num = 4;
+ redArea.point = (PointF *)malloc(4 * sizeof(PointF));
- carBody.num = car->bodyNum;
- carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
- for (int i = 0; i < carBody.num; ++i) {
- carBody.point[i] = car->carXY[car->body[i]];
- }
-
- for (int i = 0; i < RoadMapList.size(); ++i) {
- if (RoadMapList[i].type == GENERAL_MAP) {
-
- // 姣忔潯绾㈠尯閮芥娴�
- for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
- if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
- ret = true;
+ redArea.point[0] = RoadMap.specialAreas[i].area[0];
+ redArea.point[1] = RoadMap.specialAreas[i].area[1];
+ redArea.point[2] = RoadMap.specialAreas[i].area[2];
+ redArea.point[3] = RoadMap.specialAreas[i].area[3];
+ } else if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING) {
+ int j = 0;
+ for (; j < RoadMap.roads.size(); ++j) {
+ if (RoadMap.specialAreas[i].road == RoadMap.roads[j].id) {
+ break;
}
}
- break;
+
+ if (j < RoadMap.roads.size()) {
+
+ PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
+ RoadMap.specialAreas[i].area[0]);
+ PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
+ RoadMap.specialAreas[i].area[1]);
+
+ redArea.num = 4;
+ redArea.point = (PointF *) malloc(4 * sizeof(PointF));
+
+ redArea.point[0] = RoadMap.specialAreas[i].area[0];
+ redArea.point[1] = RoadMap.specialAreas[i].area[1];
+ redArea.point[2] = p2;
+ redArea.point[3] = p1;
+ }
+ }
+
+ if (redArea.num > 0) {
+ for (int i = 0; i < car->bodyNum; ++i) {
+ if (IntersectionOf(car->carXY[car->body[i]], &redArea) == GM_Containment) {
+ free(redArea.point);
+ return true;
+ }
+ }
+ free(redArea.point);
}
}
- free(carBody.point);
-
- return ret;
-}*/
+ return false;
+}
bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
--
Gitblit v1.8.0