From 6e0f29b08a040d14576d7053c1206a8439936570 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 24 十一月 2020 14:18:03 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 165 +++++++++++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 155 insertions(+), 10 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index f3164f5..22102d0 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -106,6 +106,12 @@
static map<int, int> CrossingHint;
static map<int, bool> ErrorLaneReport;
+typedef struct {
+ int road_id;
+ double distance;
+} trigger_detect_t;
+static map<int, trigger_detect_t> TriggerDetect; // 璺濈鏈矾娈靛悇瑙﹀彂鐐圭殑璺濈
+
#define ROAD_EXAM_READY_NEXT 0
#define ROAD_EXAM_FREE_RUN 1
#define ROAD_EXAM_ITEM_CAR_START 2
@@ -134,7 +140,7 @@
static int prevTurnWise;
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);
-static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
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);
@@ -142,7 +148,7 @@
static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
-static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
@@ -187,6 +193,8 @@
RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;
RoadExamStatus = ROAD_EXAM_READY_NEXT;
+
+ TriggerDetect.clear();
win = false;
}
@@ -1276,7 +1284,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, TargetFree = 0;
-
+ int forward = 0; // 杞﹁締鐩稿褰撳墠閬撹矾鐨�
UpdateCarSensor(rtkTime);
UpdataOdo(speed, moveDirect, rtkTime);
@@ -1436,7 +1444,7 @@
prevMoveDirect = moveDirect;
} else if (moveDirect == 0) {
// 鎸佺画鍋滆溅
- if (TimeGetDiff(rtkTime, &stopTimepoint) >= examParam.road_pause_criteria && !StopCarOnRedArea &&
+ if (TimeGetDiff(rtkTime, &stopTimepoint) >= CorrectPauseCriteria(examParam.road_pause_criteria) && !StopCarOnRedArea &&
StopOnRedArea(RoadMap, car)) {
// 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
AddExamFault(16, rtkTime);
@@ -1491,6 +1499,34 @@
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
// 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
+ // 妫�鏌�
+ projection_t projection;
+
+ uint32_t tm1 = AppTimer_GetTickCount();
+
+ projection = CalcProjectionWithRoadEdgeEx(RoadMap.roads[currExamMapIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
+
+ if (projection.edgeIndex < RoadMap.roads[currExamMapIndex].rightEdge.size()) {
+ if (projection.pointIndex < RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points.size() - 1) {
+ double roadYaw = YawOf(RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex + 1],
+ RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex]);
+
+ int deltaAng;
+
+ if (ABS((int)roadYaw - (int)car->yaw) > 180) {
+ deltaAng = 360 - ABS((int)roadYaw - (int)car->yaw);
+ } else {
+ deltaAng = ABS((int)roadYaw - (int)car->yaw);
+ }
+
+ if (deltaAng < 90) {
+ forward = 1;
+ } else {
+ forward = -1;
+ }
+// DEBUG("璺鏂瑰悜 %f 杞﹁締鏂瑰悜 %f 宸�� %d 鑰楁椂 %ld", roadYaw, car->yaw, deltaAng, AppTimer_GetTickCount() - tm1);
+ }
+ }
}
ExitTarget(RoadMap, car, CarModelList, rtkTime);
@@ -1554,7 +1590,7 @@
ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime,
BigStraightRoadFree,
TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree);
- ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
+ ItemExam2(RoadMap, currExamMapIndex, car, CarModelList, forward, moveDirect);
}
}
@@ -1621,6 +1657,7 @@
}
} else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
+ DEBUG("瀵绘壘涓嬩竴涓瓙椤圭洰");
RoadExamStatus = ROAD_EXAM_READY_NEXT;
}
} else {
@@ -1649,6 +1686,7 @@
}
if (!testing) {
+ DEBUG("褰撳墠瀛愰」缁撴潫");
RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
freeRunExceptDistance = 60.0;
@@ -1665,9 +1703,13 @@
}
}
-static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
{
- int item = EntryItem(roadIndex, RoadMap, car, CarModelList);
+ int item = 0;
+
+ if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
+ item = EntryItem(roadIndex, RoadMap, car, CarModelList, forward, moveDirect);
+ }
if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
StartDriveStraightExam();
@@ -1919,13 +1961,35 @@
return false;
}
-static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
{
- if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0)
+ if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0 || CarModelList.size() < 2)
return -1;
+
+ list<car_model *>::iterator iter = CarModelList.begin();
+
+ Line trace;
+ PointF p1, p2;
+
+ p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
+ ++iter;
+ p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
+ MakeLine(&trace, &p1, &p2);
+
+ double trace_length = DistanceOf(p1, p2);
+
+ if (TriggerDetect.size() > 0) {
+ auto it = TriggerDetect.begin();
+
+ if (it->second.road_id != RoadMap.roads[index].id) {
+ TriggerDetect.clear();
+ }
+ }
for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {
+
+
/*Line triggerLine;
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
@@ -1937,7 +2001,7 @@
return RoadMap.triggerLines[i].active;
}*/
- vector<double> vec;
+ /* vector<double> vec;
PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
car->carXY[car->axial[AXIAL_FRONT]]);
@@ -1954,6 +2018,87 @@
if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
DEBUG("瑙﹀彂椤圭洰 %d", RoadMap.examScheme[0].triggerLines[j].active);
return RoadMap.examScheme[0].triggerLines[j].active;
+ }*/
+
+// double angleOf = DeltaYaw(YawOf(RoadMap.examScheme[0].triggerLines[j].points[0], car->carXY[car->axial[AXIAL_FRONT]]), car->yaw);
+// auto it = TriggerDetect.find(j);
+//
+// if (it != TriggerDetect.end()) {
+// if (fabs(angleOf - it->second.angleOf) > 2.0) {
+// bool trigger = false;
+//
+// trigger_detect_t up;
+//
+// if (moveDirect == 1 && forward == 1 && it->second.angleOf <= 90 && angleOf > 90) {
+// trigger = true;
+// }
+// DEBUG("瑙傚療瀛�<%d> 鏇存柊 %f -> %f", j, it->second.angleOf, angleOf);
+//
+// up.angleOf = angleOf;
+// up.road_id = RoadMap.roads[index].id;
+//
+// TriggerDetect[j] = up;
+//
+//
+//
+// if (trigger) {
+// DEBUG("pos %d 瑙﹀彂椤圭洰 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
+// return RoadMap.examScheme[0].triggerLines[j].active;
+// }
+// }
+// } else {
+// trigger_detect_t up;
+//
+// up.road_id = RoadMap.roads[index].id;
+// up.angleOf = angleOf;
+//
+// TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
+//
+// DEBUG("瑙傚療瀛�<%d> 鍔犲叆 %f", j, angleOf);
+// }
+
+// double dist = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
+// RoadMap.examScheme[0].triggerLines[j].points[0]);
+ auto it = TriggerDetect.find(j);
+
+ if (it != TriggerDetect.end()) {
+ PointF verticalPoint;
+
+ if (moveDirect == 1 &&
+ forward == 1 &&
+ trace_length > 0.01 &&
+ VerticalPointOnLine(RoadMap.examScheme[0].triggerLines[j].points[0], trace, verticalPoint) &&
+ DistanceOf(verticalPoint, RoadMap.examScheme[0].triggerLines[j].points[0]) < it->second.distance) {
+ DEBUG("pos %d 瑙﹀彂椤圭洰 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
+
+ DEBUG("(%f, %f) ON (%f, %f)-(%f, %f) IS (%f, %f) trace %f",
+ RoadMap.examScheme[0].triggerLines[j].points[0].X,
+ RoadMap.examScheme[0].triggerLines[j].points[0].Y,
+ trace.X1,
+ trace.Y1,
+ trace.X2,
+ trace.Y2,
+ verticalPoint.X,
+ verticalPoint.Y,
+ trace_length);
+
+ return RoadMap.examScheme[0].triggerLines[j].active;
+ }
+ } else {
+ PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
+ PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
+
+ double dist1 = DistanceOf(p1, RoadMap.examScheme[0].triggerLines[j].points[0]);
+ double dist2 = DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]);
+
+ trigger_detect_t up;
+
+ up.road_id = RoadMap.roads[index].id;
+ up.distance = MAX(dist1, dist2);
+
+ TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
+
+ DEBUG("瑙傚療瀛�<%d> 鍔犲叆 %f", j, MAX(dist1, dist2));
}
}
}
--
Gitblit v1.8.0