From 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期五, 15 五月 2020 15:53:20 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/test_items2/road_exam.cpp | 276 +++++++++++++++++++++++++++++++++++++++++++++++------- 1 files changed, 237 insertions(+), 39 deletions(-) diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp index f712b10..a682012 100644 --- a/lib/src/main/cpp/test_items2/road_exam.cpp +++ b/lib/src/main/cpp/test_items2/road_exam.cpp @@ -89,7 +89,8 @@ int road; int separate; int lane; -} CarOnLane; +} CurrentLane; +static bool laneChanging; static const int MAX_ENGINE_RPM = 2500; static const double START_CAR_MOVE_DISTANCE = 10.0; @@ -100,6 +101,8 @@ static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10); static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10); static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3); + +static const double NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧� static const int CRL_NONE = 0; static const int CRL_SEP_DOTTED = 1; @@ -127,6 +130,7 @@ static int CrashRoadLine(road_t &road, const car_model *car); static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2); static bool LaneIsValid(struct car_on_lane lane); +static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList); void InitRoadExam(road_exam_map &RoadMap) { @@ -167,7 +171,8 @@ currCarOnRoadIndex = FIND_POSITION; - CarOnLane.road = CarOnLane.separate = CarOnLane.lane = -1; + CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1; + laneChanging = false; InitThroughSomething(RoadMap); } @@ -395,6 +400,16 @@ reportStopCarOnRedArea = false; DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); + + if (slideNormalDistance) { + // 鍚庢粦锛屾墸10鍒� + AddExamFault(18, rtkTime); + DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背"); + } + + slideNormalDistance = false; + slideLongDistance = false; + occurSlide = false; } else if (moveDirect == -1 && prevMoveDirect == 0) { DEBUG("寮�濮嬪悗婊�"); stopPoint = car->basePoint; @@ -402,6 +417,7 @@ } 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)) { @@ -411,6 +427,7 @@ reportStopCarOnRedArea = true; }*/ } else if (moveDirect == -1) { + // 鎸佺画鍚庢粦 if (occurSlide) { double slideDistance = DistanceOf(stopPoint, car->basePoint); @@ -423,18 +440,32 @@ AddExamFault(5, rtkTime); DEBUG("鍚庢粦瓒呰繃30鍘樼背"); slideLongDistance = true; + slideNormalDistance = false; + occurSlide = false; } } } else { - if (slideNormalDistance) { - // 鍚庢粦锛屾墸10鍒� - AddExamFault(18, rtkTime); - DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背"); - } + // 鍓嶈繘 - slideNormalDistance = false; - slideLongDistance = false; - occurSlide = false; + } + + // 杞悜鐏鍙� + switch (ReadCarStatus(TURN_SIGNAL_LAMP)) { + case LEFT_TURN_LIGHT: + if (currTurnSignalStatus != LEFT_TURN_LIGHT) { + currTurnSignalStatus = LEFT_TURN_LIGHT; + Rtk2DriveTimer(turnSignalChangeTime, rtkTime); + } + break; + case RIGHT_TURN_LIGHT: + if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { + currTurnSignalStatus = RIGHT_TURN_LIGHT; + Rtk2DriveTimer(turnSignalChangeTime, rtkTime); + } + break; + default: + currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP); + break; } // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛�� @@ -563,32 +594,174 @@ } } + // 妫�娴嬪帇绾跨姸鎬� + bool crashRedLineNow = false; + bool crashGreenLineNow = false; + if (currExamMapIndex >= 0) { int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car); if (crl == CRL_NONE) { - DEBUG("浠�涔堥兘娌″帇"); +// DEBUG("浠�涔堥兘娌″帇"); } else if (crl == CRL_SEP_DOTTED) { - DEBUG("鍘嬪垎閬撹櫄绾�"); +// DEBUG("鍘嬪垎閬撹櫄绾�"); + crashGreenLineNow = true; } else if (crl == CRL_SEP_SOLID) { - DEBUG("鍘嬪垎閬撳疄绾�"); +// DEBUG("鍘嬪垎閬撳疄绾�"); + crashRedLineNow = true; } else if (crl == CRL_EDGE_DOTTED) { - DEBUG("鍘嬭竟娌胯櫄绾�"); +// DEBUG("鍘嬭竟娌胯櫄绾�"); + crashGreenLineNow = true; } else if (crl == CRL_EDGE_SOLID) { - DEBUG("鍘嬭竟娌垮疄绾�"); +// DEBUG("鍘嬭竟娌垮疄绾�"); + crashRedLineNow = true; } - if (crl != CRL_SEP_DOTTED || crl != CRL_SEP_SOLID) { - struct car_on_lane lane; - UpdateLane(lane, RoadMap.roads[currExamMapIndex], car); - if (!LaneIsSame(lane, CarOnLane)) { - if (LaneIsValid(CarOnLane)) { - // 杞﹂亾鍙樻崲 - DEBUG("鍙樻洿杞﹂亾"); + struct car_on_lane lane; + if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) { + + if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate && + lane.lane == CurrentLane.lane) { + laneChanging = false; + } + + if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate && + lane.lane != CurrentLane.lane) { + if (crl == CRL_SEP_DOTTED) { + // 鍙橀亾杩涜涓� + if (!laneChanging) { + laneChanging = true; + DEBUG("鍙戠敓鍙橀亾"); + // 姣旇緝涓婃璺ㄧ嚎鏃堕棿 + if (crashGreenCmpTime.hour >= 0) { + uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, + rtkTime->mss * 10, + crashGreenCmpTime.hour, + crashGreenCmpTime.min, + crashGreenCmpTime.sec, + crashGreenCmpTime.msec * 10); + + if (diff < CHANGE_ROAD_MIN_INTERVAL) { + DEBUG("===================== 杩炵画鍙橀亾 ============!!"); + // 杩炵画鍙橀亾锛屼笉鍚堟牸 + AddExamFault(15, rtkTime); + } + } + + // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅 + if (lane.lane < CurrentLane.lane) { + // 鍚戝乏渚у彉閬� + DEBUG("鍚戝乏渚у彉閬�"); + if (currTurnSignalStatus != LEFT_TURN_LIGHT) { + DEBUG("鍙樿皟鏈墦鐏�!!"); + // 娌℃墦鐏紝涓嶅悎鏍� + ReportTurnSignalError(13, rtkTime); + } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, + rtkTime->mss * 10, + turnSignalChangeTime.hour, + turnSignalChangeTime.min, + turnSignalChangeTime.sec, + turnSignalChangeTime.msec * 10) < + TURN_SIGNAL_LAMP_ADVANCE) { + if (!reportTurnSignalError) { + DEBUG("杞悜鐏椂闂翠笉瓒�"); + // 涓嶈冻3绉掞紝涓嶅悎鏍� + reportTurnSignalError = true; + ReportTurnSignalError(14, rtkTime); + } + } + } else { + // 鍚戝彸渚у彉閬� + DEBUG("鍚戝彸渚у彉閬�"); + if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { + DEBUG("鍙樿皟鏈墦鐏�!!"); + // 娌℃墦鐏紝涓嶅悎鏍� + ReportTurnSignalError(13, rtkTime); + } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, + rtkTime->mss * 10, + turnSignalChangeTime.hour, + turnSignalChangeTime.min, + turnSignalChangeTime.sec, + turnSignalChangeTime.msec * 10) < + TURN_SIGNAL_LAMP_ADVANCE) { + if (!reportTurnSignalError) { + DEBUG("杞悜鐏椂闂翠笉瓒�"); + // 涓嶈冻3绉掞紝涓嶅悎鏍� + reportTurnSignalError = true; + ReportTurnSignalError(14, rtkTime); + } + } + } + } + } else { + // 鍙橀亾瀹屾垚 + DEBUG("鍙橀亾瀹屾垚"); + CurrentLane = lane; + laneChanging = false; + + // 璁板綍鏈鍙橀亾鏃堕棿鐐� + Rtk2DriveTimer(crashGreenCmpTime, rtkTime); } - CarOnLane = lane; + } + + if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) { + // 璺垨娈靛彉鏇达紝鎾ら攢鍙橀亾璺熻釜 + DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d NEW %d, %d", CurrentLane.road, + CurrentLane.separate, + lane.road, lane.separate); + CurrentLane = lane; + laneChanging = false; } } + } + + // 鎾炵孩绾� + if (crashRedLineNow) { + if (!occurCrashRedLine) { + // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍� + DEBUG("鎾為亾璺竟缂樼嚎"); + AddExamFault(11, rtkTime); + occurCrashRedLine = true; + } + } else { + occurCrashRedLine = false; + } + + // 鍘嬭櫄绾� + if (crashGreenLineNow) { + // 鍘嬭櫄绾� + if (moveDirect != 0) { + if (checkCrashGreenTimeout == 0) { + checkCrashGreenTimeout = 1; + Rtk2DriveTimer(crashGreenRunTime, rtkTime); // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣 + } else if (checkCrashGreenTimeout == 1) { + uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, + crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10); + + if (diff >= CRASH_DOTTED_LINE_TIMEOUT) { + DEBUG("闀挎椂闂村帇铏氱嚎"); + checkCrashGreenTimeout = 2; + // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸 + AddExamFault(12, rtkTime); + } + } + } else { + // 鍋滆溅灏变笉璁℃椂浜� + checkCrashGreenTimeout = 0; + } + + // 妫�娴嬪綋鍓嶈溅杈嗕簬铏氱嚎鐨勪綅缃紝鍋氬彉閬撴娴�; + // 妫�娴嬫槸鍚�3绉掑墠鏈夊紑鍚搴斾箣杞悜鐏� + if (!occurCrashGreenLine) { + occurCrashGreenLine = true; + // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣 + Rtk2DriveTimer(crashGreenStartTime, rtkTime); + turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus; + } + } else { + // 涓嶅啀鍘嬭櫄绾� + occurCrashGreenLine = false; + checkCrashGreenTimeout = 0; } } @@ -636,6 +809,8 @@ rightExt = true; MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp); goto RIGHT_EXT_CMP; + } else { + DEBUG("鍙充晶涓嶅瀭鐐� %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y); } p1 = p2; } @@ -643,6 +818,7 @@ RIGHT_EXT_CMP: if (!leftExt || !rightExt) { + DEBUG("宸﹀彸杈圭晫涓嶅尮閰�"); return false; } @@ -669,20 +845,20 @@ orthogonal.insert(pair<int, int>(j, 1)); orthogonalInSegment = true; intersection = true; - DEBUG("鍒嗛亾绾� %d 宸︽浜�", j); +// DEBUG("鍒嗛亾绾� %d 宸︽浜�", j); break; } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) { orthogonal.insert(pair<int, int>(j, 2)); orthogonalInSegment = true; intersection = true; - DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j); +// DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j); break; } p1 = p2; } } } - DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size()); +// DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size()); if (orthogonal.size() > 0) { if (orthogonal.size() == road.separate[i].lines.size()) { @@ -696,7 +872,7 @@ lane.separate = i; lane.lane = itx->first; - DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane); +// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane); break; } } @@ -706,16 +882,18 @@ lane.separate = i; lane.lane = orthogonal.size(); - DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane); +// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane); } out = lane; return true; } else { // 涓嶅畬鍏ㄦ浜わ紝鐩存帴閫�鍑� } + DEBUG("鍒嗛亾绾块暱鐭笉鍚岋紝閫�鍑�"); return false; } } + DEBUG("娈靛尮閰嶅け璐�"); return false; } @@ -800,12 +978,12 @@ // 鍘嬪疄绾� return CRL_SEP_SOLID; } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) { - if (LaneIsValid(CarOnLane) && CarOnLane.lane <= j) { + if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) { return CRL_SEP_SOLID; } return CRL_SEP_DOTTED; } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) { - if (LaneIsValid(CarOnLane) && CarOnLane.lane > j) { + if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) { return CRL_SEP_SOLID; } return CRL_SEP_DOTTED; @@ -1164,11 +1342,11 @@ RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) { StartThroughExam(currExamMapIndex, RoadMapList); } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) { - StartDriveStraightExam(currExamMapIndex, RoadMapList); + } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) { - StartStopCarExam(currExamMapIndex, RoadMapList); + } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) { - StartOperateGearExam(currExamMapIndex, RoadMapList, rtkTime); + } } } else if (startCar == START_CAR_DONE) { @@ -1179,14 +1357,11 @@ CarModelList, speed, moveDirect, rtkTime); } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) { - currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car, - CarModelList, speed, moveDirect, rtkTime); + } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) { - currExamMapIndex = ExecuteStopCarExam(currExamMapIndex, RoadMapList, car, - CarModelList, speed, moveDirect, rtkTime); + } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) { - currExamMapIndex = ExecuteOperateGearExam(currExamMapIndex, RoadMapList, car, - CarModelList, speed, moveDirect, rtkTime); + } if (currExamMapIndex == -1) { @@ -1636,6 +1811,29 @@ return px; } +static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList) +{ + // 璁$畻杞﹀墠杩涜建杩瑰欢闀跨嚎 + double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]); + + PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw); + Line extLine; + + MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint); + + if (IntersectionOf(extLine, road.stopLine) == GM_Intersection && + IntersectionOfLine(extPoint, road.stopLine) == -1) { + if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) { + // 鎺ヨ繎璺彛鍚庯紝瑕佹鏌ヨ溅杈嗘槸鍚﹁繘鍏ラ敊璇溅閬� + road.arrivedTail = true; + if (!road.tts.empty()) + PlayTTS(road.tts.c_str()); + } + } else if (road.arrivedTail) { + road.arrivedTail = false; + } +} + #if 0 typedef struct { -- Gitblit v1.8.0