| | |
| | | #include "../jni_log.h" |
| | | #include "../test_common/car_sensor.h" |
| | | #include "road_exam.h" |
| | | #include "../utils/xconvert.h" |
| | | |
| | | #define DEBUG(fmt, args...) LOGD("<through_something> <%s>: " fmt, __func__, ##args) |
| | | |
| | | #if 0 |
| | | 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 map <int, int> TargetReduceRec; |
| | | static map <int, int> TargetReduceRec2; |
| | | |
| | | #define NOT_ENTER 1 |
| | | #define ENTER_Z 2 |
| | | #define REDUCE_SPEED 4 |
| | | #define STOP_CAR 8 |
| | | #define OVER_SPEED 16 |
| | | |
| | | static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 3.0; |
| | | static const double PASS_SCHOOL_MAX_SPEED = 30.0; // kmh |
| | | static const double LASTEST_BREAK_POINT = 30.0; |
| | | |
| | | static int breakActive; |
| | | static int stopActive; |
| | | static bool crashRedLine; |
| | | |
| | | |
| | | |
| | | void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList) |
| | | static void SetTargetReduceRec(map<int, int> &table, int key, int status) |
| | | { |
| | | if (index == -1) |
| | | auto it = table.find(key); |
| | | |
| | | if (it != table.end()) { |
| | | table.erase(it); |
| | | } |
| | | table.insert(pair<int, int>(key, status)); |
| | | } |
| | | |
| | | static int GetTargetReduceRec(map<int, int> &table, int key) |
| | | { |
| | | auto it = table.find(key); |
| | | |
| | | if (it != table.end()) { |
| | | return it->second; |
| | | } |
| | | return NOT_ENTER; |
| | | } |
| | | |
| | | static void RemoveTargetReduceRec(map<int, int> &table, int key) |
| | | { |
| | | auto it = table.find(key); |
| | | |
| | | if (it != table.end()) { |
| | | table.erase(it); |
| | | } |
| | | } |
| | | |
| | | void ResetTarget(road_exam_map &RoadMap) |
| | | { |
| | | TargetReduceRec.clear(); |
| | | TargetReduceRec2.clear(); |
| | | } |
| | | |
| | | void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | if (roadIndex < 0 || roadIndex >= RoadMap.roads.size()) |
| | | return; |
| | | DEBUG("进入路考通过something地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); |
| | | if (!RoadMapList[index].tts.empty()) { |
| | | DEBUG("播放TTS %s", RoadMapList[index].tts.c_str()); |
| | | PlayTTS(RoadMapList[index].tts.c_str()); |
| | | } else { |
| | | DEBUG("没有TTS"); |
| | | } |
| | | breakActive = 0; |
| | | stopActive = 0; |
| | | crashRedLine = false; |
| | | } |
| | | // 路口 |
| | | for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) { |
| | | PointF point; |
| | | double distance; |
| | | |
| | | void InitThroughSomething(road_exam_map &RoadMap) |
| | | { |
| | | breakRecord.clear(); |
| | | // 为每一个刹车检测点 |
| | | for (int i = 0; i < RoadMap.specialAreas.size(); ++i) { |
| | | point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1; |
| | | point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1; |
| | | |
| | | distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge); |
| | | |
| | | int key = roadIndex * 100 + i; |
| | | int rec = GetTargetReduceRec(TargetReduceRec, key); |
| | | |
| | | if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) { |
| | | if (rec == NOT_ENTER) { |
| | | SetTargetReduceRec(TargetReduceRec, key, ENTER_Z); |
| | | } |
| | | // 记录刹车 |
| | | if (dobreak && !(rec & REDUCE_SPEED)) { |
| | | SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED); |
| | | } |
| | | } else if (distance > 1e-3 && distance < DISTANCE_STOP_CAR_TO_STOP_LINE) { |
| | | // 路口停车观察 |
| | | if (moveDirect == 0 && !(rec & STOP_CAR)) { |
| | | SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR); |
| | | } |
| | | } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) { |
| | | RemoveTargetReduceRec(TargetReduceRec, key); |
| | | } |
| | | } |
| | | // 人行道、公交站、学校 |
| | | for (int i = 0; i < RoadMap.specialAreas.size(); i++) { |
| | | if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id) |
| | | continue; |
| | | |
| | | if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) { |
| | | breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false)); |
| | | } |
| | | } |
| | | for (int i = 0; i < RoadMap.roads.size(); ++i) { |
| | | breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false)); |
| | | } |
| | | } |
| | | double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge); |
| | | |
| | | int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, |
| | | LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | PointF p1, p2; |
| | | int key = i; |
| | | int rec = GetTargetReduceRec(TargetReduceRec2, key); |
| | | |
| | | double distance2StopLine = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], |
| | | RoadMapList[index].stopLine); |
| | | p1.X = RoadMapList[index].stopLine.X1; |
| | | p1.Y = RoadMapList[index].stopLine.Y1; |
| | | p2.X = RoadMapList[index].stopLine.X2; |
| | | p2.Y = RoadMapList[index].stopLine.Y2; |
| | | |
| | | if (IntersectionOfLine(p1, p2, car->carXY[car->axial[AXIAL_FRONT]]) == -1) { |
| | | distance2StopLine = -distance2StopLine; |
| | | } |
| | | |
| | | // 距离停止线30米内是否有刹车动作 |
| | | if (breakActive == 0) { |
| | | if (distance2StopLine > 0 && distance2StopLine <= LASTEST_BREAK_POINT) { |
| | | if (ReadCarStatus(BREAK) == BREAK_ACTIVE) { |
| | | breakActive = 1; |
| | | if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) { |
| | | if (rec == NOT_ENTER) { |
| | | SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z); |
| | | } |
| | | // 记录刹车,停车 |
| | | if (dobreak && !(rec & REDUCE_SPEED)) { |
| | | SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED); |
| | | } |
| | | } else if (distance > 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 (distance2StopLine < 0 && breakActive != 1){ |
| | | breakActive = -1; |
| | | // 不按规定减速,不合格 |
| | | DEBUG("不踩下刹车,不合格"); |
| | | switch (RoadMapList[index].type) { |
| | | case THROUGH_INTERSECTION_MAP: |
| | | AddExamFault(41, rtkTime); |
| | | break; |
| | | case TURN_LEFT_MAP: |
| | | AddExamFault(43, rtkTime); |
| | | break; |
| | | case TURN_RIGHT_MAP: |
| | | AddExamFault(46, rtkTime); |
| | | break; |
| | | case THROUGH_ZEBRA_CROSSING_MAP: |
| | | AddExamFault(48, rtkTime); |
| | | break; |
| | | case THROUGH_SCHOOL_MAP: |
| | | if (distance1 < -1e-3 && distance2 > 1e-3) { |
| | | 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("通过学校区域超速"); |
| | | AddExamFault(49, rtkTime); |
| | | break; |
| | | case THROUGH_BUS_STATION_MAP: |
| | | AddExamFault(50, rtkTime); |
| | | break; |
| | | default: |
| | | break; |
| | | } |
| | | } else if (rec != NOT_ENTER) { |
| | | RemoveTargetReduceRec(TargetReduceRec2, key); |
| | | } |
| | | } |
| | | } |
| | | |
| | | // 有停止标记的,是否停车(停止线5米内有停车动作) |
| | | if (RoadMapList[index].flagStop != 0 && stopActive == 0) { |
| | | if (distance2StopLine < 0.0) { |
| | | // 不停车瞭望,不合格 |
| | | DEBUG("不停车瞭望"); |
| | | switch (RoadMapList[index].type) { |
| | | case THROUGH_INTERSECTION_MAP: |
| | | AddExamFault(42, rtkTime); |
| | | break; |
| | | case TURN_LEFT_MAP: |
| | | AddExamFault(44, rtkTime); |
| | | break; |
| | | case TURN_RIGHT_MAP: |
| | | AddExamFault(47, rtkTime); |
| | | break; |
| | | default: |
| | | break; |
| | | } |
| | | stopActive = -1; |
| | | } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) { |
| | | if (moveDirect == 0) { |
| | | stopActive = 1; |
| | | } |
| | | } |
| | | } |
| | | |
| | | if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) { |
| | | if (!crashRedLine) { |
| | | DEBUG("不按考试员指令行驶"); |
| | | crashRedLine = true; |
| | | // 不按考试员指令行驶 |
| | | AddExamFault(3, rtkTime); |
| | | } |
| | | } else { |
| | | crashRedLine = false; |
| | | } |
| | | |
| | | if (ExitSonArea(index, RoadMapList, car)) { |
| | | DEBUG("离开通过something区域"); |
| | | return -1; |
| | | } |
| | | |
| | | return index; |
| | | } |
| | | #endif |
| | | |
| | | void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime) |
| | | { |
| | | RECHECK: |
| | | for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) { |
| | | int road = it->first / 100; |
| | | int x = it->first % 100; |
| | | |
| | | if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) { |
| | | DEBUG("越过路口 road %d 路口 %d", road, x); |
| | | if (!(it->second & REDUCE_SPEED)) { |
| | | // 不按规定减速,不合格 |
| | | DEBUG("不按规定减速"); |
| | | |
| | | if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) { |
| | | AddExamFault(41, rtkTime); |
| | | } else if (RoadMap.roads[road].stopLine[x].active == |
| | | ROAD_ACTIVE_TURN_LEFT) { |
| | | AddExamFault(43, rtkTime); |
| | | } else if (RoadMap.roads[road].stopLine[x].active == |
| | | ROAD_ACTIVE_TURN_RIGHT) { |
| | | AddExamFault(46, rtkTime); |
| | | } |
| | | } |
| | | if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) { |
| | | // 不停车瞭望,不合格 |
| | | DEBUG("不停车瞭望"); |
| | | if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) { |
| | | AddExamFault(42, rtkTime); |
| | | } else if (RoadMap.roads[road].stopLine[x].active == |
| | | ROAD_ACTIVE_TURN_LEFT) { |
| | | AddExamFault(44, rtkTime); |
| | | } else if (RoadMap.roads[road].stopLine[x].active == |
| | | ROAD_ACTIVE_TURN_RIGHT) { |
| | | AddExamFault(47, rtkTime); |
| | | } |
| | | } |
| | | |
| | | RemoveTargetReduceRec(TargetReduceRec, it->first); |
| | | goto RECHECK; |
| | | } |
| | | } |
| | | |
| | | RECHECK2: |
| | | for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) { |
| | | int x = it->first; |
| | | Line line; |
| | | int roadIndex = 0; |
| | | |
| | | for (; roadIndex < RoadMap.roads.size(); ++roadIndex) { |
| | | if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road) |
| | | break; |
| | | } |
| | | |
| | | PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, |
| | | RoadMap.specialAreas[x].area[1]); |
| | | MakeLine(&line, &RoadMap.specialAreas[x].area[1], &point2); |
| | | |
| | | if (CrashTheLine(line, car, CarModelList)) { |
| | | if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING && |
| | | !(it->second & REDUCE_SPEED)) { |
| | | DEBUG("不按规定减速"); |
| | | AddExamFault(48, rtkTime); |
| | | } |
| | | if (RoadMap.specialAreas[x].type == BUS_STATION_AREA && |
| | | !(it->second & REDUCE_SPEED)) { |
| | | DEBUG("不按规定减速"); |
| | | AddExamFault(50, rtkTime); |
| | | } |
| | | RemoveTargetReduceRec(TargetReduceRec2, it->first); |
| | | goto RECHECK2; |
| | | } |
| | | } |
| | | } |