From 712eb83d17ffc6e259af6c53ba63bbd58cd1242b Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 19 八月 2020 18:33:42 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 331 +++++++++++++++++++++++++++++++++++-------------------
1 files changed, 212 insertions(+), 119 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 8bb5cea..0f0c587 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -23,6 +23,7 @@
#include <map>
#include <string>
#include <cstdlib>
+#include <algorithm>
#define DEBUG(fmt, args...) LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
@@ -72,6 +73,13 @@
int gain;
struct RtkTime time;
} change_lane_t;
+
+typedef struct {
+ int road_index;
+ int stop_line_index;
+ int sep;
+ double distance;
+} road_end_point_t;
static const int INVALID_ROAD = -1;
@@ -168,6 +176,7 @@
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
+static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -175,7 +184,7 @@
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
-void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);
+static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);
void InitRoadExam(road_exam_map &RoadMap)
{
@@ -1098,33 +1107,118 @@
}
}
-static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
+/*********************************************************
+ * 璁＄畻褰撳墠鎵�鍦ㄥ垎閬撶嚎鐨勫墿浣欓暱搴�
+ * @param RoadMap
+ * @param lane
+ * @param car
+ * @return
+ */
+static double SeparateLength(road_exam_map &RoadMap, lane_t lane, const car_model *car)
{
- for (int i = 0; i < road.stopLine.size(); ++i) {
- PointF point;
+ double distance = 0;
+
+ if (lane.road < 0 || lane.total == 0 || lane.sep < 0)
+ return distance;
+
+ int n = RoadMap.roads[lane.road].separate[lane.sep].lines[0].size();
+ int m = RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points.size();
+
+ return CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]],
+ RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points[m-1],
+ RoadMap.roads[lane.road].rightEdge);
+}
+
+/***********************************************************
+ * 鍓嶅悜鏈�杩戠殑璺彛鍋滄绾挎垨瀵煎悜绾胯捣鐐癸紝涓�瀹氳窛绂诲唴鍋氬嚭璺彛鎻愮ず锛屾垨椤圭洰鑷姩瑙﹀彂
+ * @param roadIndex
+ * @param RoadMap
+ * @param car
+ */
+static road_end_point_t NearbyRoadEndPoint(int roadIndex, road_exam_map &RoadMap, const car_model *car)
+{
+ road_end_point_t rec;
+
+ rec.road_index = roadIndex;
+ rec.stop_line_index = 0;
+ rec.distance = 0;
+
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
+ return rec;
+
+ // 鏈�杩戠殑璺彛璺濈
+ int stop_line_index;
+ PointF nearbyStopPoint;
+
+ NearbyCrossingGuide(stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
+ nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
+ nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;
+
+ double dis1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], nearbyStopPoint, RoadMap.roads[roadIndex].rightEdge);
+
+ struct ep_ {
+ int sep;
double distance;
+ } ep;
- point.X = road.stopLine[i].line.X1;
- point.Y = road.stopLine[i].line.Y1;
+ vector<struct ep_> dset;
- distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, road.rightEdge);
+ ep.sep = -1;
+ ep.distance = dis1;
+ dset.push_back(ep);
- if (distance > 10 && distance < 50) {
- // 鎻愮ず璺彛鎬庝箞璧�
- if (GetCrossingStatus(roadIndex, i) == CROSSING_NOT_HINT) {
- if (!road.stopLine[i].tts.empty()) {
- DEBUG("璺彛鎻愮ず %s", road.stopLine[i].tts.c_str());
- PlayTTS(road.stopLine[i].tts.c_str());
+ // 鏈�杩戠殑鍒嗛亾绾胯窛绂伙紝涓斾笉瓒呰繃鏈�杩戣矾鍙�
+ for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
+ for (int j = 0; j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) {
+ for (int k = 0; k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) {
+ if (RoadMap.roads[roadIndex].separate[i].lines[j][k].character != LINE_DOTTED &&
+ RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size() > 0) {
+ double dis2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0], RoadMap.roads[roadIndex].rightEdge);
+
+ if (dis2 < -1e-3) {
+ continue;
+ }else if (dis2 < dis1) {
+ // 鎵惧嚭鏈�鐭殑鐐�
+ ep.sep = i;
+ ep.distance = dis2;
+ dset.push_back(ep);
+ continue;
+ }
+ goto FIND_END_POINT;
}
- ChangeCrossingStatus(roadIndex, i, CROSSING_HAS_HINT);
}
- } else if (distance > 55 && GetCrossingStatus(roadIndex, i) != CROSSING_NOT_HINT) {
- ChangeCrossingStatus(roadIndex, i, CROSSING_NOT_HINT);
}
+ }
+
+ FIND_END_POINT:
+ sort(dset.begin(), dset.end(), [=](struct ep_ x, struct ep_ y)->bool {return x.distance < y.distance;});
+
+ rec.stop_line_index = stop_line_index;
+ rec.distance = dset[0].distance;
+ rec.sep = dset[0].sep;
+
+ return rec;
+}
+
+static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
+ return;
+
+ if (distance > 5 && distance < 70) {
+ // 鎻愮ず璺彛鎬庝箞璧�
+ if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
+ if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
+ DEBUG("璺彛鎻愮ず %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
+ PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
+ }
+ ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
+ }
+ } else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
+ ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);
}
}
-static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
{
int guide = 0, stopLine = 0;
double distance;
@@ -1173,6 +1267,9 @@
stopLineIndex = stopLine;
return guide;
}
+
+static uint8_t itemExec[4] = {0};
+static double odo;
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -1372,8 +1469,6 @@
if (currExamMapIndex >= 0) {
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
- HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
-
// 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
}
@@ -1393,13 +1488,13 @@
// DEBUG("Lane淇℃伅 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
if (Lane.guide > 0 && currExamMapIndex >= 0) {
- int stop_line;
+ int stop_line_index;
- if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
- if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) {
+ if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
+ if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
AddExamFault(9, rtkTime);
- SetErrorLaneRpt(currExamMapIndex, stop_line, true);
+ SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
}
}
}
@@ -1408,8 +1503,92 @@
DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
}
- if (currExamMapIndex >= 0) {
- AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
+ if (currExamMapIndex >= 0 && Lane.guide == 0) {
+ double BigStraightRoad = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
+
+ road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);
+
+ HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);
+
+ double freeSepDis = SeparateLength(RoadMap, Lane, car);
+
+ // 鍓╀綑璺濈灏辨槸瀵煎悜绾胯捣鐐圭殑璺濈
+ if (Lane.sep >= 0 && Lane.sep == ep.sep) {
+ freeSepDis = ep.distance;
+ }
+
+ DEBUG("鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f", BigStraightRoad, freeSepDis);
+
+ if (startCar == START_CAR_DONE) {
+ if (itemExec[0] == 1 || itemExec[1] == 1 || itemExec[2] == 1 || itemExec[3] == 1) {
+
+ DEBUG("椤圭洰鎵ц璺濈<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);
+
+ if (ReadOdo() - odo > 120) {
+ odo = ReadOdo();
+ if (itemExec[0] == 1) {
+ itemExec[0] = 2;
+ }
+ if (itemExec[1] == 1) {
+ itemExec[1] = 2;
+ }
+ if (itemExec[2] == 1) {
+ itemExec[2] = 2;
+ }
+ if (itemExec[3] == 1) {
+ itemExec[3] = 2;
+ }
+ }
+ goto BIG_DOG;
+ }
+
+ if (itemExec[0] == 2 || itemExec[1] == 2 || itemExec[2] == 2 || itemExec[3] == 2) {
+ DEBUG("椤圭洰浼戞伅璺濈<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);
+
+ if (ReadOdo() - odo > 100) {
+ if (itemExec[0] == 2) {
+ itemExec[0] = 3;
+ }
+ if (itemExec[1] == 2) {
+ itemExec[1] = 3;
+ }
+ if (itemExec[2] == 2) {
+ itemExec[2] = 3;
+ }
+ if (itemExec[3] == 2) {
+ itemExec[3] = 3;
+ }
+ }
+ goto BIG_DOG;
+ }
+
+
+ if (BigStraightRoad > 170 && ep.distance > 170) {
+ if (itemExec[0] == 0) {
+ PlayTTS("浜岀嫍鐩寸嚎琛岄┒");
+ itemExec[0] = 1;
+ odo = ReadOdo();
+ }
+ } else if (BigStraightRoad > 150 && ep.distance > 150) {
+ if (itemExec[3] == 0) {
+ PlayTTS("浜岀嫍鍔犲噺妗�");
+ itemExec[3] = 1;
+ odo = ReadOdo();
+ }
+ } else if (freeSepDis > 150) {
+ if (itemExec[2] == 0) {
+ PlayTTS("浜岀嫍鍙橀亾");
+ itemExec[2] = 1;
+ odo = ReadOdo();
+ } else if (itemExec[1] == 0) {
+ PlayTTS("浜岀嫍瓒呰溅");
+ itemExec[1] = 1;
+ odo = ReadOdo();
+ }
+ }
+
+ BIG_DOG:;
+ }
}
// 妫�娴嬪帇绾跨姸鎬�
@@ -1430,9 +1609,8 @@
stopCar = STOP_CAR_DONE;
}
-
// 鎵ц鏌愪釜椤圭洰
- if (currRoadItem != NULL) {
+ /*if (currRoadItem != NULL) {
if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
DEBUG("鍙橀亾璺濈瓒呮爣");
@@ -1482,7 +1660,7 @@
StartDriveStraightExam(currRoadItem->tts);
}
}
- }
+ }*/
}
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -1625,61 +1803,6 @@
prevTurnWise = angle;
}
-static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
-{
- // 鏈�杩�2绉掑唴锛屾瘡0.5绉掔殑瑙掑害宸ぇ浜�5搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画4娆★紱鎴栫獊鐜拌秴30搴︾殑杞悜锛涜涓鸿浆鍚戙��
- if (CarModelList.size() < 1)
- return false;
-
- list<car_model *>::iterator iter = CarModelList.begin();
-
- car_model *c1 = *iter, *c2;
-
- ++iter;
-
- char turn[TURN_CHECK_CNT] = {0};
- int checkCnt = 0;
-
-// DEBUG("CheckCarTurn.........");
-
- while (iter != CarModelList.end() && checkCnt < TURN_CHECK_CNT) {
- c2 = *iter;
-
- uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
-
- if (tdiff >= TURN_CHECK_INTERVAL) {
- int ang = 0;
- turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
-// DEBUG("%c 瑙掑害姣旇緝 %02d:%02d:%02d.%03d %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
-
- if (turn[checkCnt] == 'N') {
- break;
- } else if (ang >= 30) {
- DEBUG("宸﹀彸杞‘璁� %c", turn[checkCnt]);
- return turn[checkCnt];
- }
-
- c1 = c2;
- checkCnt++;
- }
-
- ++iter;
- }
-
- int i = 0;
- for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
- if (turn[i] != turn[i+1])
- break;
- }
-
- if (i == TURN_CHECK_CNT-1) {
- DEBUG("宸﹀彸杞‘璁� %c", turn[0]);
- return turn[0];
- }
-
- return 0;
-}
-
/**********************************************************
* 鎸夋暣涓溅韬槸鍚﹁鐩栬绠�
* @param RoadMapList
@@ -1738,37 +1861,6 @@
return false;
}
-
-/***************************************************
- * 鎺ヨ繎璺彛鏃讹紝鎻愮ず涓嬩竴姝ユ�庝箞璧�
- * @param road
- * @param car
- * @param CarModelList
- */
-/*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) {
- // 鎺ヨ繎璺彛鍚庯紝瑕佹鏌ヨ溅杈嗘槸鍚﹁繘鍏ラ敊璇溅閬�
- DEBUG("鎺ヨ繎璺彛");
- road.arrivedTail = true;
- if (!road.tts.empty())
- PlayTTS(road.tts.c_str());
- }
- } else if (road.arrivedTail) {
- road.arrivedTail = false;
- }
-}*/
-
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
@@ -1809,21 +1901,22 @@
return true;
}
-void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
+static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
{
double distance = 0;
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
- return;
+ return 0;
for (int i = 0; i < RoadMap.roads[roadIndex].rightEdge.size(); ++i) {
for (int j = 1; j < RoadMap.roads[roadIndex].rightEdge[i].points.size(); ++j) {
PointF point = RoadMap.roads[roadIndex].rightEdge[i].points[j];
- distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
+ distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].leftEdge);
if (distance > 1e-3) {
- DEBUG("鐩撮亾鍓╀綑璺濈 %f", distance);
- return;
+ return distance;
}
}
}
+
+ return 0;
}
--
Gitblit v1.8.0