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/drive_straight.cpp | 11 + lib/src/main/cpp/driver_test.cpp | 4 lib/src/main/cpp/test_items2/through_something.h | 2 lib/src/main/cpp/master/comm_if.cpp | 31 ++++++ lib/src/main/cpp/test_items2/through_something.cpp | 40 ++++++- lib/src/main/cpp/test_items2/road_exam.cpp | 147 +++++++++++++++++++++-------- lib/src/main/cpp/driver_test.h | 9 + 7 files changed, 192 insertions(+), 52 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index 70cec73..d293ee9 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -779,6 +779,10 @@ // 璁$畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫� double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 / (double)(tmDiff); + +// DEBUG("浣嶇Щ %f 鏃堕棿 %ld 閫熷害 %f", sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)), tmDiff, speed); +// DEBUG("%d %d %f, %f - %d %d %f, %f", RtkBuffer[p1].ss, RtkBuffer[p1].dss, RtkBuffer[p1].x, RtkBuffer[p1].y, RtkBuffer[p2].ss, RtkBuffer[p2].dss, RtkBuffer[p2].x, RtkBuffer[p2].y); + int move = 0; double deg = 0.0; diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h index 58359b1..524fccc 100644 --- a/lib/src/main/cpp/driver_test.h +++ b/lib/src/main/cpp/driver_test.h @@ -96,6 +96,7 @@ #define LINE_SOLID 1 #define LINE_HALF_SOLID_LEFT 2 #define LINE_HALF_SOLID_RIGHT 3 +#define LINE_BOUNDARY 4 //杞﹂亾鏂瑰悜锛堟寜浣嶇粍鍚堬級,濡傛灉涓�0锛屽垯琛ㄦ棤杞﹂亾鏂瑰悜璇存槑锛� #define LANE_FORWARD 0x01 @@ -140,6 +141,7 @@ string tts; bool stopFlag; Line line; + PointF centrePoint; } stop_line_t; typedef struct { @@ -184,10 +186,17 @@ std::vector<PointF> leftPoints; // 瀵瑰簲鍒伴亾璺乏渚х殑鐐� } trigger_line_t; +typedef struct { + int id; + int type; + std::vector<PointF> points; +} forbid_line_t; + struct road_exam_map { std::vector<road_t> roads; std::vector<special_area_t> specialAreas; std::vector<trigger_line_t> triggerLines; + std::vector<forbid_line_t> forbidLines; }; struct area_exam_map { diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp index e6c8db2..cb82dff 100644 --- a/lib/src/main/cpp/master/comm_if.cpp +++ b/lib/src/main/cpp/master/comm_if.cpp @@ -918,7 +918,11 @@ MakeLine(&temp.line, &p1, &p2); } } + if (itr2->HasMember("center_point")) { + const Value &s = (*itr2)["center_point"]; + temp.centrePoint = mapPoints[s.GetInt()]; + } crossing.push_back(temp); } @@ -1136,6 +1140,33 @@ map.triggerLines.push_back(trigger); } } + if (doc.HasMember("red_line")) { + const Value &a = doc["red_line"]; + + for (Value::ConstValueIterator itr = a.Begin(); + itr != a.End(); ++itr) { + forbid_line_t forbid; + + if (itr->HasMember("type")) { + const Value &s = (*itr)["type"]; + forbid.type = s.GetInt(); + } + if (itr->HasMember("id")) { + const Value &s = (*itr)["id"]; + forbid.id = s.GetInt(); + } + if (itr->HasMember("points")) { + const Value &a2 = (*itr)["points"]; + + for (Value::ConstValueIterator itr2 = a2.Begin(); + itr2 != a2.End(); ++itr2) { + forbid.points.push_back(mapPoints[(*itr2).GetInt()]); + } + } + map.forbidLines.push_back(forbid); + } + } + DEBUG("鍦板浘瑙f瀽瀹屾瘯"); CleanRoadMap(); diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp index 8c69204..94ba625 100644 --- a/lib/src/main/cpp/test_items2/drive_straight.cpp +++ b/lib/src/main/cpp/test_items2/drive_straight.cpp @@ -45,17 +45,22 @@ yaw += car->yaw; } yaw_stat++; + + DEBUG("瑙掑害鐭 car %f yaw %f", car->yaw, yaw); + if (yaw_stat == 5) { yaw = fmod(yaw, 360) / 5; - + DEBUG("鍒涘缓鍩虹嚎 yaw %f", yaw); PointF extPoint = PointExtend(car->basePoint, 100, yaw); MakeLine(&baseLine, &car->basePoint, &extPoint); beginOdo = ReadOdo(); setup = 2; } } else if (setup == 2) { - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine) > MAX_OFFSET_DISTANCE) { - DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背"); + double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine); + + if (offset > MAX_OFFSET_DISTANCE) { + DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset = %f", offset); // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸 AddExamFault(30, rtkTime); return false; 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) { diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp index 7372634..f75d5db 100644 --- a/lib/src/main/cpp/test_items2/through_something.cpp +++ b/lib/src/main/cpp/test_items2/through_something.cpp @@ -66,10 +66,12 @@ TargetReduceRec2.clear(); } -void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime) +double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime) { + vector<double> nearbyTarget; // 瀛︽牎銆佸叕浜ょ珯銆佷汉琛岄亾鍖哄煙鐨勮窛绂� + if (roadIndex < 0 || roadIndex >= RoadMap.roads.size()) - return; + return 100000; // 璺彛 for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) { PointF point; @@ -106,12 +108,19 @@ continue; if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) { - double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge); + double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge); + double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge); int key = i; int rec = GetTargetReduceRec(TargetReduceRec2, key); - if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) { + if (distance1 < -1e-3 && distance2 > 1e-3) { + nearbyTarget.push_back(0); + } else if (distance1 > 1e-3 && distance2 > 1e-3) { + nearbyTarget.push_back(distance1); + } + + if (distance1 > 1e-3 && distance1 < LASTEST_BREAK_POINT) { if (rec == NOT_ENTER) { SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z); } @@ -119,30 +128,45 @@ if (dobreak && !(rec & REDUCE_SPEED)) { SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED); } - } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) { + } else if (distance1 > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) { RemoveTargetReduceRec(TargetReduceRec2, key); } } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) { double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge); double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge); + int key = i; int rec = GetTargetReduceRec(TargetReduceRec2, key); if (distance1 < -1e-3 && distance2 > 1e-3) { + nearbyTarget.push_back(0); if (rec == NOT_ENTER) { SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z); } if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED && !(rec & OVER_SPEED)) { SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED); - DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴��"); + DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴�� %f kmh", ConvertMs2KMh(speed)); AddExamFault(49, rtkTime); } - } else if (rec != NOT_ENTER) { - RemoveTargetReduceRec(TargetReduceRec2, key); + } else if (distance1 < -1e-3 && distance2 < -1e-3) { + if (rec != NOT_ENTER) { + RemoveTargetReduceRec(TargetReduceRec2, key); + } + } else { + nearbyTarget.push_back(distance1); + if (rec != NOT_ENTER) { + RemoveTargetReduceRec(TargetReduceRec2, key); + } } } } + + if (nearbyTarget.size() > 0) { + sort(nearbyTarget.begin(), nearbyTarget.end()); + return nearbyTarget[0]; + } + return 100000; } void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime) diff --git a/lib/src/main/cpp/test_items2/through_something.h b/lib/src/main/cpp/test_items2/through_something.h index 9977762..eaa37ff 100644 --- a/lib/src/main/cpp/test_items2/through_something.h +++ b/lib/src/main/cpp/test_items2/through_something.h @@ -8,7 +8,7 @@ #include "../driver_test.h" void ResetTarget(road_exam_map &RoadMap); -void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime); +double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime); void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime); -- Gitblit v1.8.0