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 +++++++++++++++++++++++++++------------------ 1 files changed, 83 insertions(+), 54 deletions(-) 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; } -- Gitblit v1.8.0