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/drive_straight.cpp | 137 ++++++---- lib/src/main/cpp/test_items2/drive_straight.h | 4 lib/src/main/cpp/test_items2/stop_car.h | 6 lib/src/main/cpp/driver_test.cpp | 97 -------- lib/src/main/cpp/test_items2/through_something.cpp | 62 ++++ lib/src/main/cpp/test_items2/operate_gear.h | 5 lib/src/main/cpp/test_items2/road_exam.cpp | 276 +++++++++++++++++++--- lib/src/main/cpp/test_items2/stop_car.cpp | 78 +++-- lib/src/main/cpp/test_items2/operate_gear.cpp | 9 lib/src/main/cpp/driver_test.h | 3 lib/src/main/cpp/test_common/Geometry.cpp | 2 11 files changed, 434 insertions(+), 245 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index b0811d2..233b416 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -196,106 +196,9 @@ DEBUG("娓呴櫎鏃х殑璺�冨湴鍥�"); - if (RoadMapPoints.point != NULL) { - free(RoadMapPoints.point); - } - RoadMapPoints.num = 0; - - for (int i = 0; i < RoadMap.roads.size(); ++i) { - for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) { - RoadMap.roads[i].leftEdge[j].points.clear(); - vector<PointF>().swap(RoadMap.roads[i].leftEdge[j].points); - } - RoadMap.roads[i].leftEdge.clear(); - vector<edge_t>().swap(RoadMap.roads[i].leftEdge); - - for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) { - RoadMap.roads[i].rightEdge[j].points.clear(); - vector<PointF>().swap(RoadMap.roads[i].rightEdge[j].points); - } - RoadMap.roads[i].rightEdge.clear(); - vector<edge_t>().swap(RoadMap.roads[i].rightEdge); - - for (int j = 0; j < RoadMap.roads[i].separate.size(); ++j) { - for (int k = 0; k < RoadMap.roads[i].separate[j].lines.size(); ++k) { - for (int m = 0; m < RoadMap.roads[i].separate[j].lines[k].size(); ++m) { - RoadMap.roads[i].separate[j].lines[k][m].points.clear(); - vector<PointF>().swap(RoadMap.roads[i].separate[j].lines[k][m].points); - } - - RoadMap.roads[i].separate[j].lines[k].clear(); - vector<segment_t>().swap(RoadMap.roads[i].separate[j].lines[k]); - } - RoadMap.roads[i].separate[j].lines.clear(); - vector<vector<segment_t>>().swap(RoadMap.roads[i].separate[j].lines); - } - RoadMap.roads[i].separate.clear(); - vector<separate_t>().swap(RoadMap.roads[i].separate); - } - - RoadMap.roads.clear(); vector<road_t>().swap(RoadMap.roads); - - for (int i = 0; i < RoadMap.specialAreas.size(); ++i) { - RoadMap.specialAreas[i].area.clear(); - vector<PointF>().swap(RoadMap.specialAreas[i].area); - } - RoadMap.specialAreas.clear(); vector<special_area_t>().swap(RoadMap.specialAreas); - - RoadMap.triggerLines.clear(); vector<trigger_line_t>().swap(RoadMap.triggerLines); -/* - for (int i = 0; i < RoadMapList.size(); ++i) { - struct road_exam_map map = RoadMapList[i]; - - if (map.redLine != NULL) { - for (int j = 0; j < map.redLineNum; ++j) { - if (map.redLine[j].point != NULL) - free(map.redLine[j].point); - } - free(map.redLine); - } - - if (map.greenLine != NULL) { - for (int j = 0; j < map.greenLineNum; ++j) { - if (map.greenLine[j].point != NULL) - free(map.greenLine[j].point); - } - free(map.greenLine); - } - - if (map.redArea != NULL) { - for (int j = 0; j < map.redAreaNum; ++j) { - if (map.redArea[j].point != NULL) - free(map.redArea[j].point); - } - free(map.redArea); - } - - if (map.triggerLine != NULL) { - for (int j = 0; j < map.triggerLineNum; ++j) { - if (map.triggerLine[j].line.point != NULL) - free(map.triggerLine[j].line.point); - } - free(map.triggerLine); - } - - if (map.area.point != NULL) { - free(map.area.point); - } - - if (map.roadEdgeLine != NULL) { - for (int j = 0; j < map.roadEdgeLineNum; ++j) { - if (map.roadEdgeLine[j].point != NULL) - free(map.roadEdgeLine[j].point); - } - - free(map.roadEdgeLine); - } - } - - RoadMapList.clear();*/ } void SetRoadMap(road_exam_map &map) diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h index 46a6abc..81d7b1d 100644 --- a/lib/src/main/cpp/driver_test.h +++ b/lib/src/main/cpp/driver_test.h @@ -168,6 +168,7 @@ int targetRoad; int stopFlag; string tts; + bool arrivedTail; Polygon area; // 鏁翠釜閬撹矾鍖哄煙 std::vector<edge_t> leftEdge; std::vector<edge_t> rightEdge; @@ -186,6 +187,8 @@ int id; int road; int active; + int time; // 椤圭洰鏈�澶у畬鎴愭椂闂� + int distance; // 椤圭洰鏈�澶у畬鎴愯窛绂� string tts; Line line; } trigger_line_t; diff --git a/lib/src/main/cpp/test_common/Geometry.cpp b/lib/src/main/cpp/test_common/Geometry.cpp index 7f8c02c..03b5434 100644 --- a/lib/src/main/cpp/test_common/Geometry.cpp +++ b/lib/src/main/cpp/test_common/Geometry.cpp @@ -536,10 +536,10 @@ p2.Y = line.Y2; PointF pv = GetVerticalPoint(p1, p2, point); + vp = pv; if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) || (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) { - vp = pv; return true; } return false; diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp index 5e2b12f..3b9f8d3 100644 --- a/lib/src/main/cpp/test_items2/drive_straight.cpp +++ b/lib/src/main/cpp/test_items2/drive_straight.cpp @@ -15,88 +15,117 @@ static const double CHECK_STAGE_DISTANCE = 100.0; static const double MAX_OFFSET_DISTANCE = 0.3; +static int ttsPlayEnd; static bool crossStartLine; static bool reportOffsetOver; static double edgeDistance; +static double distanceToStartSum; -void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) { - if (index == -1) - return; +static double CalcDistance2Edge(road_t &road, const car_model *car); - DEBUG("杩涘叆璺�冪洿绾胯椹跺湴鍥� index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); +void StartDriveStraightExam(std::string tts) { + DEBUG("寮�濮嬬洿绾胯椹�"); - if (!RoadMapList[index].tts.empty()) { - DEBUG("鎾斁TTS"); - PlayTTS(RoadMapList[index].tts.c_str()); + if (!tts.empty()) { + PlayTTS(tts.c_str()); } else { DEBUG("娌℃湁TTS"); } - crossStartLine = false; + distanceToStartSum = 0; reportOffsetOver = false; } -int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, +int ExecuteDriveStraightExam(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { - Line road_edge; - static PointF startPoint; - MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]); + double dis2roadEdge = 0; - { - double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge); + if (ttsPlayEnd == 1) { + ttsPlayEnd = 2; + startPoint = car->basePoint; + edgeDistance = CalcDistance2Edge(road, car); // 鍩哄噯杈硅窛 - double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge); - - MA_SendDistance(6 - (l1+l2)/2.0, (l1+l2)/2.0); + DEBUG("褰撳墠鍩哄噯璺竟闂磋窛 %f", edgeDistance); } - if (!crossStartLine) { - PointF p1, p2; + if (ttsPlayEnd != 2) + return 1; - p1.X = RoadMapList[index].stopLine.X1; - p1.Y = RoadMapList[index].stopLine.Y1; - p2.X = RoadMapList[index].stopLine.X2; - p2.Y = RoadMapList[index].stopLine.Y2; + double distanceToStart = DistanceOf(car->basePoint, startPoint); + dis2roadEdge = CalcDistance2Edge(road, car); - if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) { - DEBUG("璺ㄨ繃鏍囪绾匡紝寮�濮嬬洿绾挎祴璇�"); - crossStartLine = true; - startPoint = car->basePoint; - edgeDistance = DistanceOf(car->basePoint, road_edge); + DEBUG("璺竟闂磋窛 %f --- %f", dis2roadEdge, edgeDistance); - DEBUG("褰撳墠鍩哄噯璺竟闂磋窛 %f", edgeDistance); - } - } else { - double distanceToStart = DistanceOf(car->basePoint, startPoint); + if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) { + DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背"); + // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸 + AddExamFault(30, rtkTime); + reportOffsetOver = true; - DEBUG("璺竟闂磋窛 %f --- %f", DistanceOf(car->basePoint, road_edge), edgeDistance); - - if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) { - DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背"); - // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸 - AddExamFault(30, rtkTime); - reportOffsetOver = true; - - ////////////////////////////////////////////// - startPoint = car->basePoint; - edgeDistance = DistanceOf(car->basePoint, road_edge); - reportOffsetOver = false; - } - - if (distanceToStart > CHECK_STAGE_DISTANCE) { - DEBUG("澶嶄綅杈硅窛鍋忕Щ閲�"); - startPoint = car->basePoint; - edgeDistance = DistanceOf(car->basePoint, road_edge); - reportOffsetOver = false; - } + ////////////////////////////////////////////// + startPoint = car->basePoint; + edgeDistance = dis2roadEdge; + reportOffsetOver = false; } - if (ExitSonArea(index, RoadMapList, car)) { + if (distanceToStart > CHECK_STAGE_DISTANCE) { + DEBUG("澶嶄綅杈硅窛鍋忕Щ閲�"); + startPoint = car->basePoint; + edgeDistance = dis2roadEdge; + reportOffsetOver = false; + distanceToStartSum += distanceToStart; + distanceToStart = 0; + } + + if (distanceToStart + distanceToStartSum > 150) { DEBUG("绂诲紑鐩寸嚎琛岄┒鍖哄煙"); return -1; } + return 1; +} - return index; +static double CalcDistance2Edge(road_t &road, const car_model *car) +{ + PointF vp; + bool get_vp = false; + double distance = 0; + + // 妫�娴嬮亾璺竟缂樼嚎 + for (int i = 0; i < road.leftEdge.size(); ++i) { + PointF p1, p2; + Line edge; + + p1 = road.leftEdge[i].points[0]; + for (int j = 1; j < road.leftEdge[i].points.size(); ++j) { + p2 = road.leftEdge[i].points[j]; + MakeLine(&edge, &p1, &p2); + + if (VerticalPointOnLine(car->basePoint, edge, vp)) { + get_vp = true; + goto FIND_VP_END; + } + + p1 = p2; + } + } + +FIND_VP_END: + if (get_vp) { + distance = DistanceOf(car->basePoint, vp); + } else { + // 娌℃湁鎵惧埌鍖归厤绾跨锛屾寜鏈�灏忚窛绂婚《鐐硅绠� + distance = 100; + for (int i = 0; i < road.leftEdge.size(); ++i) { + for (int j = 0; j < road.leftEdge[i].points.size(); ++j) { + double x; + if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) { + distance = x; + } + } + } + } + + return distance; } diff --git a/lib/src/main/cpp/test_items2/drive_straight.h b/lib/src/main/cpp/test_items2/drive_straight.h index fa6e3b3..d4fd2ba 100644 --- a/lib/src/main/cpp/test_items2/drive_straight.h +++ b/lib/src/main/cpp/test_items2/drive_straight.h @@ -7,8 +7,8 @@ #include "../driver_test.h" -void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList); -int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, +void StartDriveStraightExam(std::string tts); +int ExecuteDriveStraightExam(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); #endif //MYAPPLICATION2_DRIVE_STRAIGHT_H diff --git a/lib/src/main/cpp/test_items2/operate_gear.cpp b/lib/src/main/cpp/test_items2/operate_gear.cpp index 1785dfb..f40b9db 100644 --- a/lib/src/main/cpp/test_items2/operate_gear.cpp +++ b/lib/src/main/cpp/test_items2/operate_gear.cpp @@ -30,9 +30,7 @@ static void CheckGear(union sigval sig); -void StartOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const struct RtkTime *rtkTime) { - if (index == -1) - return; +void StartOperateGearExam(const struct RtkTime *rtkTime) { DEBUG("寮�濮嬪姞鍑忔尅鎿嶄綔"); testing = true; @@ -148,10 +146,9 @@ } } -int ExecuteOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, - LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { +int ExecuteOperateGearExam(const struct RtkTime *rtkTime) { currRtkTime = *rtkTime; - return testing ? index : -1; + return testing ? 1 : -1; } void OperateGearTTSDone(int id) diff --git a/lib/src/main/cpp/test_items2/operate_gear.h b/lib/src/main/cpp/test_items2/operate_gear.h index a254549..258a573 100644 --- a/lib/src/main/cpp/test_items2/operate_gear.h +++ b/lib/src/main/cpp/test_items2/operate_gear.h @@ -7,9 +7,8 @@ #include "../driver_test.h" -void StartOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const struct RtkTime *rtkTime); -int ExecuteOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, - LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); +void StartOperateGearExam(const struct RtkTime *rtkTime); +int ExecuteOperateGearExam(const struct RtkTime *rtkTime); void OperateGearTTSDone(int id); void TerminateOperateGearExam(void); 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 { diff --git a/lib/src/main/cpp/test_items2/stop_car.cpp b/lib/src/main/cpp/test_items2/stop_car.cpp index 4e93662..514b2e4 100644 --- a/lib/src/main/cpp/test_items2/stop_car.cpp +++ b/lib/src/main/cpp/test_items2/stop_car.cpp @@ -33,12 +33,8 @@ static void PlayTTSTimeout(union sigval sig); -void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList) { - if (index == -1) - return; - - DEBUG("杩涘叆闈犺竟鍋滆溅鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); - +void StartStopCarExam(std::string tts) { + DEBUG("闈犺竟鍋滆溅"); ttsPlayEnd = 0; moveDistance = 0; prevMoveDirect = 0; @@ -49,8 +45,8 @@ doorNotClose = false; checkRoadDistance = false; - if (!RoadMapList[index].tts.empty()) { - examTtsSeq = PlayTTS(RoadMapList[index].tts.c_str()); + if (!tts.empty()) { + examTtsSeq = PlayTTS(tts.c_str()); } else { examTtsSeq = PlayTTS("璇烽潬杈瑰仠杞�"); } @@ -73,7 +69,7 @@ AppTimer_delete(PlayTTSTimeout); } -int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, +int ExecuteStopCarExam(int index, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { if (ttsPlayEnd == 1) { ttsPlayEnd = 2; @@ -114,42 +110,62 @@ if (tp - stopTimepoint >= STOP_CAR_TIME && !checkRoadDistance) { // 鍋滆溅瓒�2绉掞紝寮�濮嬪垽鏂� DEBUG("妫�娴嬪拰璺竟鐨勮窛绂�"); - PointF pc; + PointF pc, vp; + bool get_vp = false; + double dis2roadEdge = 0; checkRoadDistance = true; - + // 鍓嶅悗杞殑涓偣 pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2; pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2; // 妫�娴嬮亾璺竟缂樼嚎 - for (int i = 0; i < RoadMapList[index].roadEdgeLineNum; ++i) { - PointF p1, p2, pv; + for (int i = 0; i < road.rightEdge.size(); ++i) { + PointF p1, p2; + Line edge; - p1 = RoadMapList[index].roadEdgeLine[i].point[0]; - for (int j = 1; j < RoadMapList[index].roadEdgeLine[i].num; ++j) { - p2 = RoadMapList[index].roadEdgeLine[i].point[j]; - pv = GetVerticalPoint(p1, p2, pc); + p1 = road.rightEdge[i].points[0]; + for (int j = 1; j < road.rightEdge[i].points.size(); ++j) { + p2 = road.rightEdge[i].points[j]; + MakeLine(&edge, &p1, &p2); - if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) || - (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) { - double dis2roadEdge = DistanceOf(pc, pv); - DEBUG("鍋滆溅璺濊矾杈硅窛绂� %f", dis2roadEdge); - - if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) { - DEBUG("鍋滆溅瓒呭嚭璺竟0.5绫�"); - // 鍋滆溅璺濈瓒呰繃50鍘樼背锛屼笉鍚堟牸 - AddExamFault(36, rtkTime); - } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) { - DEBUG("鍋滆溅瓒呭嚭璺竟0.3绫�"); - // 鍋滆溅璺濈瓒呰繃30鍘樼背锛屾墸10鍒� - AddExamFault(37, rtkTime); - } + if (VerticalPointOnLine(pc, edge, vp)) { + get_vp = true; + goto FIND_VP_END; } p1 = p2; } } + +FIND_VP_END: + if (get_vp) { + dis2roadEdge = DistanceOf(pc, vp); + } else { + // 娌℃湁鎵惧埌鍖归厤绾跨锛屾寜鏈�灏忚窛绂婚《鐐硅绠� + dis2roadEdge = 100; + for (int i = 0; i < road.rightEdge.size(); ++i) { + for (int j = 0; j < road.rightEdge[i].points.size(); ++j) { + double dis; + if (dis2roadEdge > (dis = DistanceOf(pc, road.rightEdge[i].points[j]))) { + dis2roadEdge = dis; + } + } + } + } + + DEBUG("鍋滆溅璺濊矾杈硅窛绂� %f", dis2roadEdge); + + if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) { + DEBUG("鍋滆溅瓒呭嚭璺竟0.5绫�"); + // 鍋滆溅璺濈瓒呰繃50鍘樼背锛屼笉鍚堟牸 + AddExamFault(36, rtkTime); + } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) { + DEBUG("鍋滆溅瓒呭嚭璺竟0.3绫�"); + // 鍋滆溅璺濈瓒呰繃30鍘樼背锛屾墸10鍒� + AddExamFault(37, rtkTime); + } } } diff --git a/lib/src/main/cpp/test_items2/stop_car.h b/lib/src/main/cpp/test_items2/stop_car.h index afaf176..4b3f622 100644 --- a/lib/src/main/cpp/test_items2/stop_car.h +++ b/lib/src/main/cpp/test_items2/stop_car.h @@ -7,10 +7,10 @@ #include "../driver_test.h" -void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList); -void StopCarTTSDone(int id); -int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, +void StartStopCarExam(std::string tts); +int ExecuteStopCarExam(int index, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); void TerminateStopCarExam(void); +void StopCarTTSDone(int id); #endif //MYAPPLICATION2_STOP_CAR_H diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp index d9349e9..5a87311 100644 --- a/lib/src/main/cpp/test_items2/through_something.cpp +++ b/lib/src/main/cpp/test_items2/through_something.cpp @@ -18,6 +18,7 @@ using namespace std; static const double LASTEST_BREAK_POINT = 30.0; +static const double NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧� static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0; static const double PASS_SCHOOL_MAX_SPEED = 30.0; // kmh @@ -155,17 +156,52 @@ return index; } + + void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList) { int BreakDone = ReadCarStatus(BREAK); - for (int i = 0; i < map.specialAreas.size(); i++) { - if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) { - double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]); - PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw); - Line extLine; - MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint); + // 璁$畻杞﹀墠杩涜建杩瑰欢闀跨嚎 + double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]); + PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw); + PointF extPoint2 = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw); + Line extLine, extLine2; + MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint); + MakeLine(&extLine2, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint2); + // 璺彛鍒硅溅鐐� + for (int i = 0; i < map.roads.size(); ++i) { + // 杞﹀ご鍜岃矾鍙h窛绂讳笉瓒�30绫� + if (IntersectionOf(extLine, map.roads[i].stopLine) == GM_Intersection && + IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[i].stopLine) == 1 ) { + DEBUG("杩涘叆鍑忛�熷尯"); + if (BreakDone == BREAK_ACTIVE) { + auto itx = breakRecord.find(map.roads[i].id); + if (itx != breakRecord.end()) { + itx->second = true; + } + } + } + // 璺ㄧ嚎鍚庯紝妫�鏌ュ埞杞﹀姩浣� + if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) { + auto itx = breakRecord.find(map.roads[i].id); + if (itx != breakRecord.end()) { + if (itx->second == false) { + // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍� + DEBUG("涓嶆寜瑙勫畾鍑忛��"); + + } + itx->second = false; + } + } + } + // 浜鸿閬撱�佸叕浜ょ珯鍒硅溅鐐癸紱瀛︽牎闄愰�熷尯 + for (int i = 0; i < map.specialAreas.size(); i++) { + if (map.specialAreas[i].type == GRID_AREA) + continue; + + if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) { // 璁$畻鐐瑰埌宸︿晶璺竟绾跨殑鍨傜偣 int road = 0; for (road = 0; road < map.roads.size(); ++road) { @@ -174,11 +210,19 @@ } PointF vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]); + DEBUG("璁$畻鍨傜偣1 (%f, %f)", vPoint.X, vPoint.Y); + + map.specialAreas[i].leftPoints.push_back(vPoint); + + vPoint = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]); + DEBUG("璁$畻鍨傜偣2 (%f, %f)", vPoint.X, vPoint.Y); + map.specialAreas[i].leftPoints.push_back(vPoint); + } + + if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) { Line startLine; - DEBUG("璁$畻鍨傜偣 (%f, %f)", vPoint.X, vPoint.Y); - - MakeLine(&startLine, &map.specialAreas[i].area[0], &vPoint); + MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]); // 杞﹀ご鍜屾枒椹嚎璺濈涓嶈冻30绫� if (IntersectionOf(extLine, startLine) == GM_Intersection && -- Gitblit v1.8.0