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