// // Created by YY on 2020/3/20. // #include #include #include "through_something.h" #include "../driver_test.h" #include "../test_common/Geometry.h" #include "../native-lib.h" #include "../jni_log.h" #include "../test_common/car_sensor.h" #include "road_exam.h" #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) 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 static int breakActive; static int stopActive; static bool crashRedLine; static map breakRecord; // 记录车辆驶过各个停止线前刹车动作 void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList) { if (index == -1) 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; } void InitThroughSomething(road_exam_map &RoadMap) { breakRecord.clear(); // 为每一个刹车检测点 for (int i = 0; i < RoadMap.specialAreas.size(); ++i) { if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) { breakRecord.insert(pair(RoadMap.specialAreas[i].id, false)); } } for (int i = 0; i < RoadMap.roads.size(); ++i) { breakRecord.insert(pair(RoadMap.roads[i].id, false)); } } 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; 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 (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: AddExamFault(49, rtkTime); break; case THROUGH_BUS_STATION_MAP: AddExamFault(50, rtkTime); break; default: break; } } } // 有停止标记的,是否停车(停止线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; } void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList) { int BreakDone = ReadCarStatus(BREAK); // 计算车前进轨迹延长线 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) { // 车头和路口距离不足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) { if (map.roads[road].id == map.specialAreas[i].road) break; } 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; MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]); // 车头和斑马线距离不足30米 if (IntersectionOf(extLine, startLine) == GM_Intersection && IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) { DEBUG("进入减速区"); if (BreakDone == BREAK_ACTIVE) { auto itx = breakRecord.find(map.specialAreas[i].id); if (itx != breakRecord.end()) { itx->second = true; } } } // 跨线后,检查刹车动作 if (CrashTheLine(startLine, car, CarModelList)) { auto itx = breakRecord.find(map.specialAreas[i].id); if (itx != breakRecord.end()) { if (itx->second == false) { // 不按规定减速,不合格 DEBUG("不按规定减速"); } itx->second = false; } } } else if (map.specialAreas[i].type == SCHOOL_AREA) { } } }