| | |
| | | #include "../test_common/car_sensor.h" |
| | | #include "../native-lib.h" |
| | | #include "through_something.h" |
| | | #include "../master/comm_if.h" |
| | | |
| | | #include <vector> |
| | | #include <list> |
| | |
| | | |
| | | using namespace std; |
| | | |
| | | #define TURN_CHECK_CNT 5 |
| | | #define TURN_CHECK_CNT 4 |
| | | |
| | | static const int TURN_THRESHOLD = 10; |
| | | static const int TURN_CHECK_INTERVAL = D_SEC(1); |
| | | static const int TURN_THRESHOLD = 5; |
| | | static const int TURN_CHECK_INTERVAL = 500; |
| | | const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; |
| | | const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; |
| | | |
| | |
| | | |
| | | void InitRoadExam(void) |
| | | { |
| | | DEBUG("Start road_exam"); |
| | | |
| | | crashGreenCmpTime.hour = -1; |
| | | occurCrashRedLine = false; |
| | | occurCrashGreenLine = false; |
| | |
| | | uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); |
| | | |
| | | if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) { |
| | | // 停车超2秒,停在红区 |
| | | // 停车超2秒,停在红区,不合格 |
| | | AddExamFault(16, rtkTime); |
| | | DEBUG("中途停车"); |
| | | DEBUG("禁停区停车"); |
| | | reportStopCarOnRedArea = true; |
| | | } |
| | | } else if (moveDirect == -1) { |
| | |
| | | // 检查是否持续转向 |
| | | char turnDirect = CheckCarTurn(CarModelList); |
| | | if (turnDirect == 'L') { |
| | | PlayTTS("左1", 5692); |
| | | if (currTurnSignalStatus != LEFT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打左转灯"); |
| | |
| | | } |
| | | } |
| | | } else if (turnDirect == 'R') { |
| | | PlayTTS("右1", 5692); |
| | | if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打右转灯"); |
| | |
| | | if (CrashRedLine(RoadMapList, car)) { |
| | | if (!occurCrashRedLine) { |
| | | // 车辆行驶中骑轧车道中心实线或者车道边缘实线,不合格 |
| | | DEBUG("撞道路边缘线"); |
| | | AddExamFault(11, rtkTime); |
| | | occurCrashRedLine = true; |
| | | } |
| | |
| | | crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10); |
| | | |
| | | if (diff >= CRASH_DOTTED_LINE_TIMEOUT) { |
| | | DEBUG("长时间压虚线"); |
| | | checkCrashGreenTimeout = 2; |
| | | // 长时间骑轧车道分界线行驶,不合格 |
| | | AddExamFault(12, rtkTime); |
| | |
| | | crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10); |
| | | |
| | | if (diff < CHANGE_ROAD_MIN_INTERVAL) { |
| | | DEBUG("===================== 连续变道 ============!!"); |
| | | // 连续变道,不合格 |
| | | AddExamFault(15, rtkTime); |
| | | } |
| | |
| | | |
| | | // 检查变道前,是否提前转向灯 |
| | | if (inter == 1) { |
| | | PlayTTS("左2", 5698); |
| | | // 向左侧变道 |
| | | DEBUG("向左侧变道"); |
| | | if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | AddExamFault(13, rtkTime); |
| | | } |
| | | } else { |
| | | PlayTTS("右2", 5698); |
| | | // 向右侧变道 |
| | | DEBUG("向右侧变道"); |
| | | if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | AddExamFault(13, rtkTime); |
| | | } |
| | |
| | | if (currExamMapIndex == -1) { |
| | | currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList); |
| | | if (currExamMapIndex != -1) { |
| | | DEBUG("碰撞触发线"); |
| | | |
| | | MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1); |
| | | StartThroughExam(currExamMapIndex, RoadMapList); |
| | | } |
| | | } else { |
| | | int prevIdx = currExamMapIndex; |
| | | |
| | | currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car, |
| | | CarModelList, speed, moveDirect, rtkTime); |
| | | |
| | | if (currExamMapIndex == -1) { |
| | | MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1); |
| | | } |
| | | } |
| | | } |
| | | |
| | |
| | | return ret; |
| | | } |
| | | |
| | | bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | { |
| | | bool ret = false; |
| | | |
| | | if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size()) |
| | | return ret; |
| | | |
| | | Polygon trace; |
| | | |
| | | trace.num = 5; // 最近5个轮轨迹 |
| | | trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); |
| | | |
| | | list<car_model *>::iterator iter = CarModelList.begin(); |
| | | |
| | | int pn = 0; |
| | | while (iter != CarModelList.end() && pn < trace.num) { |
| | | trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]]; |
| | | ++iter; |
| | | } |
| | | |
| | | // 每条线都检测 |
| | | for (int j = 0; j < RoadMapList[index].redLineNum; ++j) { |
| | | Line red_line; |
| | | |
| | | int kp = 0; |
| | | |
| | | // 触发线一般应该只有首尾2点(id, p1, p2) |
| | | for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) { |
| | | MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp], |
| | | &RoadMapList[index].redLine[j].point[k]); |
| | | |
| | | int pp = 0; |
| | | for (int p = 1; p < pn; ++p) { |
| | | Line trace_line; |
| | | MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); |
| | | |
| | | if (IntersectionOf(trace_line, red_line) == GM_Intersection) { |
| | | // 碰到触发线 |
| | | ret = true; |
| | | goto SEARCH_SON_RED_LINE_END; |
| | | } |
| | | |
| | | pp = p; |
| | | } |
| | | |
| | | kp = k; |
| | | } |
| | | } |
| | | |
| | | SEARCH_SON_RED_LINE_END: |
| | | free(trace.point); |
| | | |
| | | return ret; |
| | | } |
| | | |
| | | static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) |
| | | { |
| | | tm.hour = rtkTime->hh; |
| | |
| | | |
| | | static char isTurn(int currYaw, int prevYaw) |
| | | { |
| | | // DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw); |
| | | |
| | | int deltaAng = 0; |
| | | |
| | | if (ABS(currYaw - prevYaw) > 180) { |
| | | currYaw = 360 - ABS(currYaw-prevYaw); |
| | | deltaAng = 360 - ABS(currYaw-prevYaw); |
| | | } else { |
| | | currYaw = ABS(currYaw - prevYaw); |
| | | deltaAng = ABS(currYaw - prevYaw); |
| | | } |
| | | |
| | | if (currYaw >= TURN_THRESHOLD) { |
| | | // DEBUG("角度差值 %d", deltaAng); |
| | | |
| | | if (deltaAng >= TURN_THRESHOLD) { |
| | | if((( currYaw + 360 - prevYaw) % 360) < 180) { |
| | | DEBUG("右转"); |
| | | // DEBUG("右转"); |
| | | return 'R'; |
| | | } else { |
| | | DEBUG("左转"); |
| | | // DEBUG("左转"); |
| | | return 'L'; |
| | | } |
| | | } |
| | |
| | | char turn[TURN_CHECK_CNT] = {0}; |
| | | int checkCnt = 0; |
| | | |
| | | while (iter != CarModelList.end()) { |
| | | // 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) { |
| | | turn[checkCnt++] = isTurn((int)c1->yaw, (int)c2->yaw); |
| | | c1 = c2; |
| | | |
| | | if (checkCnt == TURN_CHECK_CNT) |
| | | turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw); |
| | | // 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] == 0) { |
| | | break; |
| | | } |
| | | |
| | | c1 = c2; |
| | | checkCnt++; |
| | | } |
| | | |
| | | ++iter; |
| | |
| | | break; |
| | | } |
| | | |
| | | if (i == TURN_CHECK_CNT-1) |
| | | if (i == TURN_CHECK_CNT-1) { |
| | | DEBUG("左右转确认 %c", turn[0]); |
| | | return turn[0]; |
| | | } |
| | | |
| | | return 0; |
| | | } |
| | |
| | | for (int k = 1; k < RoadMapList[i].triggerLine[j].line.num; ++k) { |
| | | MakeLine(&trigger_line, &RoadMapList[i].triggerLine[j].line.point[kp], &RoadMapList[i].triggerLine[j].line.point[k]); |
| | | |
| | | int pp = 1; |
| | | for (int p = 2; p < pn; ++p) { |
| | | int pp = 0; |
| | | for (int p = 1; p < pn; ++p) { |
| | | Line trace_line; |
| | | MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); |
| | | |
| | | if (IntersectionOf(trace_line, trigger_line) == GM_Intersection && |
| | | IntersectionOfLine(trace.point[pp], trace.point[p], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && |
| | | DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trace_line) > 0.1) { |
| | | IntersectionOfLine(RoadMapList[i].triggerLine[j].line.point[kp], |
| | | RoadMapList[i].triggerLine[j].line.point[k], |
| | | car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && |
| | | DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trigger_line) > 0.1) { |
| | | // 碰到触发线 |
| | | DEBUG("碰撞触发线 引发地图 id = %d", RoadMapList[i].triggerLine[j].triggerMapId); |
| | | map_idx = FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList); |
| | | goto SEARCH_TRIGGER_LINE_END; |
| | | } |