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