| | |
| | | STOP_CAR_DONE |
| | | }; |
| | | |
| | | typedef struct { |
| | | int road; |
| | | int sep; |
| | | int no; |
| | | int guide; |
| | | } lane_t; |
| | | |
| | | typedef struct { |
| | | int edgeIndex; |
| | | int pointIndex; |
| | | PointF point; |
| | | } projection_t; |
| | | |
| | | typedef struct |
| | | { |
| | | int name; |
| | | int value; |
| | | struct RtkTime time; |
| | | } car_sensor_value_t; |
| | | |
| | | static const int INVALID_ROAD = -1; |
| | | |
| | | static const int TURN_THRESHOLD = 1; |
| | |
| | | static bool occurCrashGreenLine; |
| | | static bool occurOverSpeed; |
| | | static bool occurSecondBreak; |
| | | static int checkCrashGreenTimeout; |
| | | |
| | | static char carIntersectionOfGreenLine; |
| | | static int currTurnSignalStatus; |
| | | static int turnSignalStatusWhenCrashGreenLine; |
| | |
| | | static bool occurSlide; |
| | | static bool startCarLeftTurnSignal, checkStartCarSignal; |
| | | |
| | | static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime; |
| | | static struct RtkTime crashGreenRunTime, crashGreenStartTime; |
| | | static struct drive_timer gearErrorTimePoint; |
| | | static struct drive_timer gearNSlideTimePoint; |
| | | static struct drive_timer startCarLeftTurnSignalTime; |
| | |
| | | static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10); |
| | | |
| | | static bool turnError13Cold, turnError14Cold; |
| | | static struct car_on_lane { |
| | | int road; |
| | | int separate; |
| | | int lane; |
| | | int direct; // 车道方向限制 |
| | | int type; // 实线,虚线 |
| | | } CurrentLane; |
| | | |
| | | static lane_t Lane; |
| | | |
| | | static bool laneChanging; |
| | | static int changeLaneDirect; |
| | | static int CrashLineType; |
| | | |
| | | |
| | | |
| | | typedef struct { |
| | | int road; |
| | | int sep; |
| | | int lane; |
| | | int guide; |
| | | } lane_t; |
| | | |
| | | typedef struct { |
| | | int edgeIndex; |
| | | int pointIndex; |
| | | PointF point; |
| | | } projection_t; |
| | | static map<int, car_sensor_value_t> CarSensorValue; |
| | | |
| | | static const int MAX_ENGINE_RPM = 2500; |
| | | static const double START_CAR_MOVE_DISTANCE = 10.0; |
| | |
| | | static const int CRL_EDGE_DOTTED = 3; |
| | | static const int CRL_EDGE_SOLID = 4; |
| | | |
| | | static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0; |
| | | static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0; |
| | | static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}}; |
| | | |
| | | static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); |
| | |
| | | static void TurnSignalError13ColdTimer(union sigval sig); |
| | | static void TurnSignalError14ColdTimer(union sigval sig); |
| | | static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime); |
| | | static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car); |
| | | static int CrashRoadLine(road_t &road, const car_model *car); |
| | | static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList); |
| | | |
| | | |
| | | static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList); |
| | | static void ClearAll(road_exam_map &map); |
| | | static bool AllCmp(road_exam_map &map); |
| | |
| | | { |
| | | DEBUG("Start road_exam"); |
| | | |
| | | crashGreenCmpTime.hour = -1; |
| | | occurCrashRedLine = false; |
| | | occurCrashGreenLine = false; |
| | | occurOverSpeed = false; |
| | | |
| | | occurSecondBreak = false; |
| | | checkCrashGreenTimeout = 0; |
| | | |
| | | carIntersectionOfGreenLine = 0; |
| | | |
| | | reportTurnSignalError = false; |
| | |
| | | |
| | | turnError13Cold = turnError14Cold = true; |
| | | |
| | | CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1; |
| | | Lane.road = Lane.sep = Lane.no = -1; |
| | | laneChanging = false; |
| | | changeLaneDirect = 0; |
| | | |
| | | nextRoadId = -1; |
| | | checkTurn = false; |
| | | ClearAll(RoadMap); |
| | | |
| | | CrashLineType = -1; |
| | | |
| | | ResetOdo(); |
| | | |
| | |
| | | |
| | | /************************************************ |
| | | * 车轮压实线,前后轴交叉,前后轮迹交叉 |
| | | * @param mode 1 - 道路实线/分道实线 2 - 分道虚线/半实半虚线 |
| | | * @param currCrashLineType |
| | | * @param RoadMap |
| | | * @param car |
| | | * @param CarModelList |
| | | * @return |
| | | */ |
| | | static bool CrashRedLine(int mode, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | static int CrashRedLine(int currCrashLineType, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | { |
| | | Line frontTireAxle, rearTireAxle; |
| | | Line frontLeftTireTrack, frontRightTireTrack; |
| | | Line rearLeftTireTrack, rearRightTireTrack; |
| | | bool track = false; |
| | | int lineType = -1; |
| | | |
| | | PointF p11, p12, p21, p22, p31, p32, p41, p42; |
| | | |
| | | MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]); |
| | | MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]); |
| | | |
| | | if (CarModelList.size() >= 2) { |
| | | list<car_model *>::iterator iter = CarModelList.begin(); |
| | | |
| | | PointF p11, p12, p21, p22, p31, p32, p41, p42; |
| | | |
| | | p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]]; |
| | | p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]]; |
| | | p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]]; |
| | |
| | | |
| | | struct RtkTime time2 = ((car_model *)(*iter))->tm; |
| | | |
| | | if (TimeGetDiff(time1.hh, time1.mm, time1.ss, time1.mss, time2.hh, time2.mm, time2.ss, time2.mss) <= D_SEC(1)) { |
| | | if (TimeGetDiff(&time1, &time2) <= D_SEC(1)) { |
| | | MakeLine(&frontLeftTireTrack, &p11, &p12); |
| | | MakeLine(&frontRightTireTrack, &p21, &p22); |
| | | MakeLine(&rearLeftTireTrack, &p31, &p32); |
| | |
| | | } |
| | | |
| | | CRASH_LINE_CONFIRM: |
| | | if (lineType == LINE_HALF_SOLID_LEFT) { |
| | | if (currCrashLineType != LINE_HALF_SOLID_LEFT && track) { |
| | | if (IntersectionOfLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], redLine) == -1) { |
| | | if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) { |
| | | if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) { |
| | | // 非法跨线 |
| | | lineType += 100; |
| | | } |
| | | } |
| | | } else if (lineType == LINE_HALF_SOLID_RIGHT) { |
| | | if (currCrashLineType != LINE_HALF_SOLID_RIGHT && track) { |
| | | if (IntersectionOfLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], redLine) == 1) { |
| | | } else if (lineType == LINE_HALF_SOLID_RIGHT && currCrashLineType != lineType && track) { |
| | | if (IntersectionOfLine(p11, redLine) == 1 && IntersectionOfLine(p12, redLine) == -1) { |
| | | // 非法跨线 |
| | | lineType += 100; |
| | | } |
| | | } |
| | | return lineType; |
| | | } |
| | | |
| | | return false; |
| | | } |
| | | |
| | | |
| | | |
| | | static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo) |
| | | { |
| | |
| | | * @param roadIndex |
| | | * @return |
| | | */ |
| | | static bool GetLane(lane_t &theLane, PointF point, road_exam_map &RoadMap, int roadIndex) |
| | | static bool GetLane(lane_t &lane, PointF point, road_exam_map &RoadMap, int roadIndex) |
| | | { |
| | | Line leftProjLine, rightProjLine; |
| | | Line sep; |
| | |
| | | } |
| | | } |
| | | |
| | | theLane.road = roadIndex; |
| | | theLane.sep = i; |
| | | theLane.lane = CLP.size(); |
| | | lane.road = roadIndex; |
| | | lane.sep = i; |
| | | lane.no = CLP.size(); |
| | | |
| | | for (int x = 0; x < CLP.size(); ++x) { |
| | | if (CLP[x].pos == 'L') { |
| | | theLane.lane = x; |
| | | lane.no = x; |
| | | break; |
| | | } |
| | | } |
| | | theLane.guide = GetGuideDirect(RoadMap, p0, theLane.road, theLane.sep, theLane.lane); |
| | | lane.guide = GetGuideDirect(RoadMap, p0, lane.road, lane.sep, lane.no); |
| | | return true; |
| | | } |
| | | } |
| | | |
| | | return false; |
| | | } |
| | | |
| | | static void WriteCarSensorValue(int name, int value, const struct RtkTime *rtkTime) |
| | | { |
| | | car_sensor_value_t nv; |
| | | |
| | | nv.name = name; |
| | | nv.value = value; |
| | | nv.time = *rtkTime; |
| | | |
| | | auto it = CarSensorValue.find(name); |
| | | |
| | | if (it != CarSensorValue.end()) { |
| | | if (it->second.value != value || name == ENGINE_RPM) { |
| | | CarSensorValue[name] = nv; |
| | | } |
| | | } else { |
| | | CarSensorValue.insert(pair<int, car_sensor_value_t>(name, nv)); |
| | | } |
| | | } |
| | | |
| | | static car_sensor_value_t ReadCarSensorValue(int name) |
| | | { |
| | | car_sensor_value_t nv = {.name = -1}; |
| | | |
| | | auto it = CarSensorValue.find(name); |
| | | |
| | | if (it != CarSensorValue.end()) { |
| | | nv = it->second; |
| | | } |
| | | |
| | | return nv; |
| | | } |
| | | |
| | | static void UpdateCarSensor(const struct RtkTime *rtkTime) |
| | | { |
| | | WriteCarSensorValue(TURN_SIGNAL_LAMP, ReadCarStatus(TURN_SIGNAL_LAMP), rtkTime); |
| | | WriteCarSensorValue(HAND_BREAK, ReadCarStatus(HAND_BREAK), rtkTime); |
| | | WriteCarSensorValue(DOOR, ReadCarStatus(DOOR), rtkTime); |
| | | WriteCarSensorValue(ENGINE_RPM, ReadCarStatus(ENGINE_RPM), rtkTime); |
| | | WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime); |
| | | WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime); |
| | | WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime); |
| | | } |
| | | |
| | | static void TurnSignalError13ColdTimer(union sigval sig) |
| | |
| | | return startCar; |
| | | } |
| | | |
| | | static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | static int checkCrashGreenTimeout = 0; |
| | | static bool crashDottedLine = false; |
| | | int newLineType = CrashRedLine(CrashLineType, roadIndex, RoadMap, car, CarModelList); |
| | | |
| | | if (newLineType >= 100) { |
| | | newLineType -= 100; |
| | | DEBUG("非法跨越分道线"); |
| | | AddExamFault(11, rtkTime); |
| | | } |
| | | if (newLineType == LINE_SOLID && CrashLineType != LINE_SOLID) { |
| | | // 车辆行驶中骑轧车道中心实线或者车道边缘实线,不合格 |
| | | DEBUG("撞道路边缘线"); |
| | | AddExamFault(11, rtkTime); |
| | | } |
| | | if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) { |
| | | if (!crashDottedLine) { |
| | | checkCrashGreenTimeout = 0; |
| | | // 记录开始压线的时间,不确定是否有变道意图,待确认变道后再处理之 |
| | | crashGreenStartTime = *rtkTime; |
| | | } |
| | | crashDottedLine = true; |
| | | } else { |
| | | crashDottedLine = false; |
| | | } |
| | | |
| | | if (crashDottedLine) { |
| | | // 压虚线 |
| | | if (moveDirect != 0) { |
| | | if (checkCrashGreenTimeout == 0) { |
| | | checkCrashGreenTimeout = 1; |
| | | crashGreenRunTime = *rtkTime; // 运动中压虚线的开始时间点 |
| | | } else if (checkCrashGreenTimeout == 1) { |
| | | if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= CRASH_DOTTED_LINE_TIMEOUT) { |
| | | DEBUG("长时间压虚线"); |
| | | checkCrashGreenTimeout = 2; |
| | | // 长时间骑轧车道分界线行驶,不合格 |
| | | AddExamFault(12, rtkTime); |
| | | } |
| | | } |
| | | } else { |
| | | // 停车就不计时了 |
| | | checkCrashGreenTimeout = 0; |
| | | } |
| | | } else { |
| | | // 不再压虚线 |
| | | checkCrashGreenTimeout = 0; |
| | | } |
| | | |
| | | CrashLineType = newLineType; |
| | | } |
| | | |
| | | int DetectLane(road_exam_map &RoadMap, const car_model *car, int roadIndex, const struct RtkTime *rtkTime) |
| | | { |
| | | lane_t newLane; |
| | | int gain = 0; |
| | | |
| | | if (CrashLineType == LINE_DOTTED || CrashLineType == LINE_HALF_SOLID_LEFT || CrashLineType == LINE_HALF_SOLID_RIGHT) { |
| | | // 排除跨线中 |
| | | return 0; |
| | | } |
| | | if (GetLane(newLane, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, roadIndex)) { |
| | | if (newLane.road == Lane.road && newLane.sep == Lane.sep) { |
| | | gain = newLane.no - Lane.no; |
| | | } |
| | | |
| | | // 检查转向灯 |
| | | if (gain != 0) { |
| | | car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP); |
| | | if (lamp.name == TURN_SIGNAL_LAMP) { |
| | | if (gain < 0) { |
| | | if (lamp.value != LEFT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } else { |
| | | if (lamp.value != RIGHT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } else { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | Lane = newLane; |
| | | } |
| | | return gain; |
| | | } |
| | | |
| | | void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | uint32_t cts = AppTimer_GetTickCount(); |
| | | int ri = CalcRoadIndex(-1, RoadMap, car); |
| | | bool crash = CrashRedLine(0, 0, RoadMap, car, CarModelList); |
| | | bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList); |
| | | lane_t laneInfo; |
| | | double redist = -1; |
| | | |
| | | laneInfo.road = -1; |
| | | laneInfo.sep = -1; |
| | | laneInfo.lane = -1; |
| | | laneInfo.no = -1; |
| | | |
| | | if (ri >= 0) { |
| | | GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri); |
| | |
| | | RoadMap.roads[ri].rightEdge); |
| | | } |
| | | |
| | | DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts); |
| | | DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts); |
| | | |
| | | UpdateCarSensor(rtkTime); |
| | | |
| | | UpdataOdo(speed, moveDirect, rtkTime); |
| | | |
| | |
| | | |
| | | } |
| | | |
| | | // 转向灯读取 |
| | | switch (ReadCarStatus(TURN_SIGNAL_LAMP)) { |
| | | case LEFT_TURN_LIGHT: |
| | | if (currTurnSignalStatus != LEFT_TURN_LIGHT) { |
| | | currTurnSignalStatus = LEFT_TURN_LIGHT; |
| | | Rtk2DriveTimer(turnSignalChangeTime, rtkTime); |
| | | } |
| | | break; |
| | | case RIGHT_TURN_LIGHT: |
| | | if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { |
| | | currTurnSignalStatus = RIGHT_TURN_LIGHT; |
| | | Rtk2DriveTimer(turnSignalChangeTime, rtkTime); |
| | | } |
| | | break; |
| | | default: |
| | | currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP); |
| | | break; |
| | | } |
| | | |
| | | // 检测通过路口、人行道等区域时,释放刹车或减速 |
| | | CheckBreakActive(RoadMap, car, CarModelList, |
| | |
| | | // 检测离开此路段,全车需不在范围内 |
| | | currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car); |
| | | |
| | | DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime); |
| | | |
| | | DetectLane(RoadMap, car, currExamMapIndex, rtkTime); |
| | | |
| | | // 检测压线状态 |
| | | bool crashRedLineNow = false; |
| | | bool crashGreenLineNow = false; |
| | | |
| | | if (currExamMapIndex >= 0) { |
| | | int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car); |
| | | |
| | | if (crl == CRL_NONE) { |
| | | // DEBUG("什么都没压"); |
| | | } else if (crl == CRL_SEP_DOTTED) { |
| | | DEBUG("压分道虚线"); |
| | | crashGreenLineNow = true; |
| | | } else if (crl == CRL_SEP_SOLID) { |
| | | DEBUG("压分道实线"); |
| | | crashRedLineNow = true; |
| | | } else if (crl == CRL_EDGE_DOTTED) { |
| | | DEBUG("压边沿虚线"); |
| | | crashGreenLineNow = true; |
| | | } else if (crl == CRL_EDGE_SOLID) { |
| | | DEBUG("压边沿实线"); |
| | | crashRedLineNow = true; |
| | | } |
| | | |
| | | // 检查当前车道 |
| | | struct car_on_lane lane; |
| | | |
| | | if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) { |
| | | |
| | | if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate && |
| | | lane.lane == CurrentLane.lane) { |
| | | laneChanging = false; |
| | | } |
| | | |
| | | if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate && |
| | | lane.lane != CurrentLane.lane) { |
| | | if (crl == CRL_SEP_DOTTED) { |
| | | // 变道进行中 |
| | | if (!laneChanging) { |
| | | laneChanging = true; |
| | | DEBUG("发生变道"); |
| | | // 比较上次跨线时间 |
| | | if (crashGreenCmpTime.hour >= 0) { |
| | | uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, |
| | | rtkTime->mss * 10, |
| | | crashGreenCmpTime.hour, |
| | | crashGreenCmpTime.min, |
| | | crashGreenCmpTime.sec, |
| | | crashGreenCmpTime.msec * 10); |
| | | int laneDirect = 0; |
| | | |
| | | if (CurrentLane.lane > lane.lane) { |
| | | laneDirect = -1; |
| | | } else { |
| | | laneDirect = 1; |
| | | } |
| | | |
| | | if (diff < CHANGE_LANE_MIN_INTERVAL && laneDirect == changeLaneDirect) { |
| | | DEBUG("===================== 连续变道 %d -> %d ============", CurrentLane.lane, lane.lane); |
| | | // 连续变道,不合格 |
| | | AddExamFault(15, rtkTime); |
| | | } |
| | | } |
| | | |
| | | // 检查变道前,是否提前转向灯 |
| | | if (lane.lane < CurrentLane.lane) { |
| | | // 向左侧变道 |
| | | DEBUG("向左侧变道"); |
| | | if (currTurnSignalStatus != LEFT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, |
| | | rtkTime->mss * 10, |
| | | turnSignalChangeTime.hour, |
| | | turnSignalChangeTime.min, |
| | | turnSignalChangeTime.sec, |
| | | turnSignalChangeTime.msec * 10) < |
| | | TURN_SIGNAL_LAMP_ADVANCE) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } else { |
| | | // 向右侧变道 |
| | | DEBUG("向右侧变道"); |
| | | if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { |
| | | DEBUG("变调未打灯!!"); |
| | | // 没打灯,不合格 |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, |
| | | rtkTime->mss * 10, |
| | | turnSignalChangeTime.hour, |
| | | turnSignalChangeTime.min, |
| | | turnSignalChangeTime.sec, |
| | | turnSignalChangeTime.msec * 10) < |
| | | TURN_SIGNAL_LAMP_ADVANCE) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } |
| | | } |
| | | } else { |
| | | // 变道完成 |
| | | DEBUG("变道完成 %d -> %d", CurrentLane.lane, lane.lane); |
| | | |
| | | if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) { |
| | | DEBUG("变更车道项目完成"); |
| | | currRoadItem = NULL; |
| | | PlayTTS("完成变道"); |
| | | } else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) { |
| | | if (CurrentLane.lane > lane.lane) { |
| | | DEBUG("超车变道完成"); |
| | | overtake = true; |
| | | Rtk2DriveTimer(overTakeCmpTime, rtkTime); |
| | | PlayTTS("完成超车"); |
| | | } else { |
| | | DEBUG("右道超车,错误"); |
| | | AddExamFault(3, rtkTime); |
| | | currRoadItem = NULL; |
| | | } |
| | | } |
| | | |
| | | if (CurrentLane.lane > lane.lane) { |
| | | changeLaneDirect = -1; |
| | | } else { |
| | | changeLaneDirect = 1; |
| | | } |
| | | |
| | | CurrentLane = lane; |
| | | laneChanging = false; |
| | | // 记录本次变道时间点 |
| | | Rtk2DriveTimer(crashGreenCmpTime, rtkTime); |
| | | } |
| | | } |
| | | |
| | | if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate || lane.direct != CurrentLane.direct) { |
| | | // 路或段变更,撤销变道跟踪 |
| | | DEBUG("============== 路或段变更 CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road, |
| | | CurrentLane.separate, CurrentLane.direct, |
| | | lane.road, lane.separate, lane.direct); |
| | | |
| | | CurrentLane = lane; |
| | | laneChanging = false; |
| | | } |
| | | |
| | | if (CurrentLane.direct != 0 && !(CurrentLane.direct & RoadMap.roads[currExamMapIndex].active)) { |
| | | if (!RoadMap.roads[currExamMapIndex].errorLane) { |
| | | RoadMap.roads[currExamMapIndex].errorLane = true; |
| | | DEBUG("不按规定车道标线方向行驶 <%d> %d %d", RoadMap.roads[currExamMapIndex].id, CurrentLane.direct, RoadMap.roads[currExamMapIndex].active); |
| | | AddExamFault(9, rtkTime); |
| | | } |
| | | } |
| | | } |
| | | |
| | | ArrivedRoadEnd(RoadMap.roads[currExamMapIndex], car, CarModelList); |
| | | |
| | | // if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) { |
| | | // DEBUG("下一个目标路 id = %d", RoadMap.roads[currExamMapIndex].targetRoad); |
| | | // nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad; |
| | | // checkTurn = true; |
| | | // } |
| | | |
| | | if (checkTurn) { |
| | | // 检查是否持续转向 |
| | | char turnDirect = CheckCarTurn(CarModelList); |
| | | if (turnDirect == 'L') { |
| | | // PlayTTS("左1"); |
| | | if (currTurnSignalStatus != LEFT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打左转灯"); |
| | | // 没打左转灯,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } |
| | | } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } else if (turnDirect == 'R') { |
| | | // PlayTTS("右1"); |
| | | if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("没打右转灯"); |
| | | // 没打右转灯,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(13, rtkTime); |
| | | } |
| | | } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { |
| | | if (!reportTurnSignalError) { |
| | | DEBUG("转向灯时间不足"); |
| | | // 不足3秒,不合格 |
| | | reportTurnSignalError = true; |
| | | ReportTurnSignalError(14, rtkTime); |
| | | } |
| | | } |
| | | } else { |
| | | reportTurnSignalError = false; |
| | | } |
| | | } |
| | | } |
| | | |
| | | // 撞红线 |
| | | if (crashRedLineNow) { |
| | | if (!occurCrashRedLine) { |
| | | // 车辆行驶中骑轧车道中心实线或者车道边缘实线,不合格 |
| | | DEBUG("撞道路边缘线"); |
| | | AddExamFault(11, rtkTime); |
| | | occurCrashRedLine = true; |
| | | } |
| | | } else { |
| | | occurCrashRedLine = false; |
| | | } |
| | | |
| | | // 压虚线 |
| | | if (crashGreenLineNow) { |
| | | // 压虚线 |
| | | if (moveDirect != 0) { |
| | | if (checkCrashGreenTimeout == 0) { |
| | | checkCrashGreenTimeout = 1; |
| | | Rtk2DriveTimer(crashGreenRunTime, rtkTime); // 运动中压虚线的开始时间点 |
| | | } else if (checkCrashGreenTimeout == 1) { |
| | | uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, |
| | | crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10); |
| | | |
| | | if (diff >= CRASH_DOTTED_LINE_TIMEOUT) { |
| | | DEBUG("长时间压虚线"); |
| | | checkCrashGreenTimeout = 2; |
| | | // 长时间骑轧车道分界线行驶,不合格 |
| | | AddExamFault(12, rtkTime); |
| | | } |
| | | } |
| | | } else { |
| | | // 停车就不计时了 |
| | | checkCrashGreenTimeout = 0; |
| | | } |
| | | |
| | | // 检测当前车辆于虚线的位置,做变道检测; |
| | | // 检测是否3秒前有开启对应之转向灯 |
| | | if (!occurCrashGreenLine) { |
| | | occurCrashGreenLine = true; |
| | | // 记录开始压线的时间,不确定是否有变道意图,待确认变道后再处理之 |
| | | Rtk2DriveTimer(crashGreenStartTime, rtkTime); |
| | | turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus; |
| | | } |
| | | } else { |
| | | // 不再压虚线 |
| | | occurCrashGreenLine = false; |
| | | checkCrashGreenTimeout = 0; |
| | | } |
| | | |
| | | TestRoadStartCar(car, speed, moveDirect, rtkTime); |
| | | |
| | |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | /************************************************************* |
| | | * 检测当前车道,以车头中点为基准 |
| | | * @param road |
| | | * @param car |
| | | */ |
| | | static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car) |
| | | { |
| | | bool leftExt = false, rightExt = false; |
| | | Line leftExtLine, rightExtLine; |
| | | struct car_on_lane lane; |
| | | |
| | | for (int i = 0; i < road.leftEdge.size(); ++i) { |
| | | PointF p1, p2; |
| | | Line edge; |
| | | |
| | | p1 = road.leftEdge[i].points[0]; |
| | | for (int j = 1; j < road.leftEdge[i].points.size(); ++j) { |
| | | p2 = road.leftEdge[i].points[j]; |
| | | MakeLine(&edge, &p1, &p2); |
| | | |
| | | PointF vp; |
| | | if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) { |
| | | leftExt = true; |
| | | MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp); |
| | | goto LEFT_EXT_CMP; |
| | | } |
| | | p1 = p2; |
| | | } |
| | | } |
| | | LEFT_EXT_CMP: |
| | | for (int i = 0; i < road.rightEdge.size(); ++i) { |
| | | PointF p1, p2; |
| | | Line edge; |
| | | |
| | | p1 = road.rightEdge[i].points[0]; |
| | | for (int j = 1; j < road.rightEdge[i].points.size(); ++j) { |
| | | p2 = road.rightEdge[i].points[j]; |
| | | MakeLine(&edge, &p1, &p2); |
| | | |
| | | PointF vp; |
| | | if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) { |
| | | rightExt = true; |
| | | MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp); |
| | | goto RIGHT_EXT_CMP; |
| | | } else { |
| | | // DEBUG("右侧不垂点 %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y); |
| | | } |
| | | p1 = p2; |
| | | } |
| | | } |
| | | RIGHT_EXT_CMP: |
| | | |
| | | if (!leftExt || !rightExt) { |
| | | DEBUG("左右边界不匹配"); |
| | | return false; |
| | | } |
| | | |
| | | bool orthogonalInSegment = false; |
| | | |
| | | for (int i = 0; i < road.separate.size(); ++i) { // 段 |
| | | PointF p1, p2; |
| | | Line sep; |
| | | |
| | | map<int, vector<int>> orthogonal; |
| | | |
| | | // 一段分道组内,有一条正交,就必须保证同组的全都正交,否则直接退出 |
| | | for (int j = 0; j < road.separate[i].lines.size(); ++j) { // 线组 |
| | | bool intersection = false; |
| | | |
| | | for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) { // 节 |
| | | |
| | | p1 = road.separate[i].lines[j][k].points[0]; |
| | | for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) { |
| | | p2 = road.separate[i].lines[j][k].points[m]; |
| | | MakeLine(&sep, &p1, &p2); |
| | | |
| | | if (IntersectionOf(leftExtLine, sep) == GM_Intersection) { |
| | | vector<int> stor(4); |
| | | |
| | | stor[0] = 1; |
| | | stor[1] = road.separate[i].lines[j][k].character; |
| | | stor[2] = road.separate[i].lines[j][k].left_lane_direct; |
| | | stor[3] = road.separate[i].lines[j][k].right_lane_direct; |
| | | |
| | | orthogonal.insert(pair<int, vector<int>>(j, stor)); |
| | | orthogonalInSegment = true; |
| | | intersection = true; |
| | | |
| | | break; |
| | | } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) { |
| | | vector<int> stor(4); |
| | | |
| | | stor[0] = 2; |
| | | stor[1] = road.separate[i].lines[j][k].character; |
| | | stor[2] = road.separate[i].lines[j][k].left_lane_direct; |
| | | stor[3] = road.separate[i].lines[j][k].right_lane_direct; |
| | | |
| | | orthogonal.insert(pair<int, vector<int>>(j, stor)); |
| | | orthogonalInSegment = true; |
| | | intersection = true; |
| | | |
| | | break; |
| | | } |
| | | p1 = p2; |
| | | } |
| | | } |
| | | } |
| | | // DEBUG("目标 %d 当前 %d", road.separate[i].lines.size(), orthogonal.size()); |
| | | |
| | | if (orthogonal.size() > 0) { |
| | | if (orthogonal.size() == road.separate[i].lines.size()) { |
| | | // 得到当前在第几个车道 |
| | | int x = 0; |
| | | int left_char = LINE_SOLID; // 道路左边线 |
| | | int right_direct = 0; |
| | | |
| | | for (x = 0; x < orthogonal.size(); ++x) { |
| | | auto itx = orthogonal.find(x); |
| | | if (itx != orthogonal.end()) { |
| | | if (itx->second[0] != 1) { // 在车辆右侧 |
| | | lane.road = road.id; |
| | | lane.separate = i; |
| | | lane.lane = itx->first; |
| | | |
| | | // DEBUG("left_char %d second %d", left_char, itx->second[1]); |
| | | |
| | | if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) && |
| | | (itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT)) // 车道左右均是实线 |
| | | lane.direct = itx->second[2]; |
| | | else |
| | | lane.direct = 0; |
| | | // DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct); |
| | | break; |
| | | } else { |
| | | right_direct = itx->second[3]; |
| | | left_char = itx->second[2]; |
| | | } |
| | | } |
| | | } |
| | | if (x >= orthogonal.size()) { |
| | | lane.road = road.id; |
| | | lane.separate = i; |
| | | lane.lane = orthogonal.size(); |
| | | |
| | | // DEBUG("left_char %d right_direct %d", left_char, right_direct); |
| | | |
| | | // 最后车道的右侧限定 |
| | | if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) |
| | | lane.direct = right_direct; |
| | | else |
| | | lane.direct = 0; |
| | | // DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct); |
| | | } |
| | | out = lane; |
| | | return true; |
| | | } else { |
| | | // 不完全正交,直接退出 |
| | | } |
| | | DEBUG("分道线长短不同,退出"); |
| | | return false; |
| | | } |
| | | } |
| | | DEBUG("段匹配失败"); |
| | | return false; |
| | | } |
| | | |
| | | /************************************************************************* |
| | | * 碰触车道线型 |
| | | * @param road |
| | | * @param car |
| | | */ |
| | | static int CrashRoadLine(road_t &road, const car_model *car) |
| | | { |
| | | Line frontAxle, rearAxle; |
| | | |
| | | MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]); |
| | | MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]); |
| | | |
| | | // 检查道路边缘 |
| | | for (int i = 0; i < road.leftEdge.size(); ++i) { |
| | | PointF p1, p2; |
| | | Line edge; |
| | | |
| | | p1 = road.leftEdge[i].points[0]; |
| | | for (int j = 1; j < road.leftEdge[i].points.size(); ++j) { |
| | | p2 = road.leftEdge[i].points[j]; |
| | | MakeLine(&edge, &p1, &p2); |
| | | |
| | | if (IntersectionOf(edge, frontAxle) == GM_Intersection || |
| | | IntersectionOf(edge, rearAxle) == GM_Intersection) { |
| | | // 压道路左边线 |
| | | if (road.leftEdge[i].character != LINE_SOLID) { |
| | | return CRL_EDGE_DOTTED; |
| | | } |
| | | return CRL_EDGE_SOLID; |
| | | } |
| | | p1 = p2; |
| | | } |
| | | } |
| | | |
| | | for (int i = 0; i < road.rightEdge.size(); ++i) { |
| | | PointF p1, p2; |
| | | Line edge; |
| | | |
| | | p1 = road.rightEdge[i].points[0]; |
| | | for (int j = 1; j < road.rightEdge[i].points.size(); ++j) { |
| | | p2 = road.rightEdge[i].points[j]; |
| | | MakeLine(&edge, &p1, &p2); |
| | | |
| | | if (IntersectionOf(edge, frontAxle) == GM_Intersection || |
| | | IntersectionOf(edge, rearAxle) == GM_Intersection) { |
| | | // 压道路右边线 |
| | | if (road.rightEdge[i].character != LINE_SOLID) { |
| | | return CRL_EDGE_DOTTED; |
| | | } |
| | | return CRL_EDGE_SOLID; |
| | | } |
| | | p1 = p2; |
| | | } |
| | | } |
| | | |
| | | // 检查分道线 |
| | | for (int i = 0; i < road.separate.size(); ++i) { |
| | | // 段 |
| | | PointF p1, p2; |
| | | Line sep; |
| | | |
| | | for (int j = 0; j < road.separate[i].lines.size(); ++j) { |
| | | // 线 |
| | | for (int k = 0; k < road.separate[i].lines[j].size(); ++k) { |
| | | // 节 |
| | | p1 = road.separate[i].lines[j][k].points[0]; |
| | | |
| | | for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) { |
| | | p2 = road.separate[i].lines[j][k].points[m]; |
| | | MakeLine(&sep, &p1, &p2); |
| | | |
| | | if (IntersectionOf(sep, frontAxle) == GM_Intersection || |
| | | IntersectionOf(sep, rearAxle) == GM_Intersection) { |
| | | // 检查道路分隔线类型 |
| | | if (road.separate[i].lines[j][k].character == LINE_DOTTED) { |
| | | // 压虚线 |
| | | return CRL_SEP_DOTTED; |
| | | } else if (road.separate[i].lines[j][k].character == LINE_SOLID) { |
| | | // 压实线 |
| | | return CRL_SEP_SOLID; |
| | | } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) { |
| | | if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) { |
| | | return CRL_SEP_SOLID; |
| | | } |
| | | return CRL_SEP_DOTTED; |
| | | } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) { |
| | | if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) { |
| | | return CRL_SEP_SOLID; |
| | | } |
| | | return CRL_SEP_DOTTED; |
| | | } |
| | | } |
| | | |
| | | p1 = p2; |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | return CRL_NONE; |
| | | } |
| | | |
| | | void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) |
| | |
| | | */ |
| | | static void ClearAll(road_exam_map &map) |
| | | { |
| | | for (int i = 0; i < map.roads.size(); ++i) { |
| | | map.roads[i].activeStop = map.roads[i].activeBreak = false; |
| | | map.roads[i].arrivedTail = false; |
| | | map.roads[i].errorLane = false; |
| | | } |
| | | for (int i = 0; i < map.specialAreas.size(); i++) { |
| | | map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false; |
| | | } |
| | | for (int i = 0; i < map.triggerLines.size(); ++i) { |
| | | map.triggerLines[i].cmp = false; |
| | | } |
| | | // for (int i = 0; i < map.roads.size(); ++i) { |
| | | // map.roads[i].activeStop = map.roads[i].activeBreak = false; |
| | | // map.roads[i].arrivedTail = false; |
| | | // map.roads[i].errorLane = false; |
| | | // } |
| | | // for (int i = 0; i < map.specialAreas.size(); i++) { |
| | | // map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false; |
| | | // } |
| | | // for (int i = 0; i < map.triggerLines.size(); ++i) { |
| | | // map.triggerLines[i].cmp = false; |
| | | // } |
| | | } |
| | | |
| | | static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, |