// // Created by YY on 2020/3/17. // #include "road_exam.h" #include "../driver_test.h" #include "../utils/xconvert.h" #include "../common/apptimer.h" #include "../jni_log.h" #include "../defs.h" #include "../test_common/car_sensor.h" #include "../native-lib.h" #include "through_something.h" #include "../master/comm_if.h" #include "drive_straight.h" #include "stop_car.h" #include "operate_gear.h" #include "../test_common/odo_graph.h" #include #include #include #include #include #include #define DEBUG(fmt, args...) LOGD(" <%s>: " fmt, __func__, ##args) using namespace std; #define TURN_CHECK_CNT 6 enum { START_CAR_NOT_DO, START_CAR_DOING, START_CAR_DONE }; enum { STOP_CAR_NOT_DO, STOP_CAR_DOING, 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 const int TURN_CHECK_INTERVAL = 500; const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; const double CHANGE_LANE_RANGE = 100.0; const double OVERTAKE_RANGE = 150.0; const int OVERTAKE_HOLD_TIME = D_SEC(3); // 在超车道行驶的一段时间 const double EXAM_RANGE = 3000.0; // 至少驾驶距离 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 const int FIND_POSITION = -2; static const int INVALID_POSITION = -1; static bool occurCrashRedLine; static bool occurCrashGreenLine; static bool occurOverSpeed; static bool occurSecondBreak; static char carIntersectionOfGreenLine; static int currTurnSignalStatus; static int turnSignalStatusWhenCrashGreenLine; static bool reportTurnSignalError; static int prevMoveDirect; static uint32_t stopTimepoint = 0; static bool reportStopCarOnRedArea; static PointF stopPoint, startPoint; static bool prevGearError = false; static bool prevGearNSlide = false; static bool slideLongDistance; static bool slideNormalDistance; static bool occurSlide; static bool startCarLeftTurnSignal, checkStartCarSignal; static struct RtkTime crashGreenRunTime, crashGreenStartTime; static struct drive_timer gearErrorTimePoint; static struct drive_timer gearNSlideTimePoint; static struct drive_timer startCarLeftTurnSignalTime; static struct drive_timer overTakeCmpTime; static int gearErrorTime; static int gearNSlideTime; static int startCar, stopCar; static int currExamMapIndex; static trigger_line_t *currRoadItem; static int nextRoadId; static PointF roadItemStartPoint; static struct drive_timer roadItemStartTime; static bool overtake = false; static bool checkTurn = false; static bool checkDoor = false; static bool handBreakActive = false; static bool reportRPMOver = false; static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10); static bool turnError13Cold, turnError14Cold; static lane_t Lane; static bool laneChanging; static int changeLaneDirect; static int CrashLineType; static map CarSensorValue; static const int MAX_ENGINE_RPM = 2500; static const double START_CAR_MOVE_DISTANCE = 10.0; static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0; static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5); static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15); static const uint32_t STOP_CAR_TIME = D_SEC(2); static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10); static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10); static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3); static const int CRL_NONE = 0; static const int CRL_SEP_DOTTED = 1; static const int CRL_SEP_SOLID = 2; static const int CRL_EDGE_DOTTED = 3; static const int CRL_EDGE_SOLID = 4; 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 char isTurn(int currYaw, int prevYaw, int &ang); static char CheckCarTurn(LIST_CAR_MODEL &CarModelList); static void TurnSignalError13ColdTimer(union sigval sig); static void TurnSignalError14ColdTimer(union sigval sig); static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime); 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); static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime); static PointF CalcProjectionWithRoadEdge(vector &edge, PointF point); static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car); void InitRoadExam(road_exam_map &RoadMap) { DEBUG("Start road_exam"); occurCrashRedLine = false; occurCrashGreenLine = false; occurOverSpeed = false; occurSecondBreak = false; carIntersectionOfGreenLine = 0; reportTurnSignalError = false; currTurnSignalStatus = OFF_LIGHT; prevMoveDirect = 0; reportStopCarOnRedArea = false; occurSlide = false; slideLongDistance = false; slideNormalDistance = false; prevGearError = false; gearErrorTime = 0; prevGearNSlide = false; gearNSlideTime = 0; currExamMapIndex = -1; startCar = START_CAR_NOT_DO; stopCar = STOP_CAR_NOT_DO; currRoadItem = NULL; checkDoor = false; handBreakActive = false; reportRPMOver = false; checkStartCarSignal = startCarLeftTurnSignal = false; turnError13Cold = turnError14Cold = true; Lane.road = Lane.sep = Lane.no = -1; laneChanging = false; changeLaneDirect = 0; nextRoadId = -1; checkTurn = false; ClearAll(RoadMap); CrashLineType = -1; ResetOdo(); // 初始化考项 } void TerminateRoadExam(void) { TerminateDummyLightExam(); TerminateStopCarExam(); TerminateOperateGearExam(); TerminateDriveStraightExam(); AppTimer_delete(TurnSignalError13ColdTimer); AppTimer_delete(TurnSignalError14ColdTimer); } /********************************************************************* * 计算某点到道路边线的投影点 * @param edge * @param road * @param point * @return */ static PointF CalcProjectionWithRoadEdge(vector &edge, PointF point) { PointF p1, p2; PointF proj; vector projArray; Line line; // 计算垂点 for (int i = 0; i < edge.size(); ++i) { p1 = edge[i].points[0]; for (int j = 1; j < edge[i].points.size(); ++j) { p2 = edge[i].points[j]; MakeLine(&line, &p1, &p2); PointF vp; if (VerticalPointOnLine(point, line, vp)) { projArray.push_back(vp); } p1 = p2; } } if (projArray.size() == 0) { // 没有任何垂点,找到最近的点 proj = edge[0].points[0]; for (int i = 0; i < edge.size(); ++i) { for (int j = 1; j < edge[i].points.size(); ++j) { if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, proj)) { proj = edge[i].points[j]; } } } } else { // 最近的垂点 proj = projArray[0]; for (int i = 1; i < projArray.size(); ++i) { if (DistanceOf(point, projArray[i]) < DistanceOf(point, proj)) { proj = projArray[i]; } } } return proj; } static projection_t CalcProjectionWithRoadEdgeEx(vector &edge, PointF point) { PointF p1, p2; Line line; projection_t theProj; vector projSet; // 计算垂点 for (int i = 0; i < edge.size(); ++i) { p1 = edge[i].points[0]; for (int j = 1; j < edge[i].points.size(); ++j) { p2 = edge[i].points[j]; MakeLine(&line, &p1, &p2); PointF vp; if (VerticalPointOnLine(point, line, vp)) { projection_t proj; proj.edgeIndex = i; proj.pointIndex = j - 1; proj.point = vp; projSet.push_back(proj); } p1 = p2; } } if (projSet.size() == 0) { // 没有任何垂点,找到最近的点 theProj.edgeIndex = 0; theProj.pointIndex = 0; theProj.point = edge[0].points[0]; for (int i = 0; i < edge.size(); ++i) { for (int j = 1; j < edge[i].points.size(); ++j) { if (DistanceOf(point, edge[i].points[j]) < DistanceOf(point, theProj.point)) { theProj.edgeIndex = i; theProj.pointIndex = j - 1; theProj.point = edge[i].points[j]; } } } } else { // 最近的垂点 theProj = projSet[0]; for (int i = 1; i < projSet.size(); ++i) { if (DistanceOf(point, projSet[i].point) < DistanceOf(point, theProj.point)) { theProj = projSet[i]; } } } return theProj; } static double CalcDistanceReference(PointF point, PointF refPoint, vector &edge) { double distance = 0; projection_t projection, refProjection; projection = CalcProjectionWithRoadEdgeEx(edge, point); refProjection = CalcProjectionWithRoadEdgeEx(edge, refPoint); if (projection.edgeIndex != refProjection.edgeIndex || projection.pointIndex != refProjection.pointIndex) { bool ahead = false; if (projection.edgeIndex != refProjection.edgeIndex) { if (projection.edgeIndex > refProjection.edgeIndex) { projection_t temp; temp = projection; projection = refProjection; refProjection = temp; ahead = true; } int a = projection.edgeIndex; int b = projection.pointIndex + 1; if (b < edge[a].points.size()) { distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]); } for (; a < refProjection.edgeIndex; ++a) { for (; b < edge[a].points.size() - 1; ++b) { distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]); } b = 0; } for (int i = 0; i < refProjection.pointIndex; i++) { distance += DistanceOf(edge[a].points[i], edge[a].points[i+1]); } distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]); } else { if (projection.pointIndex > refProjection.pointIndex) { projection_t temp; temp = projection; projection = refProjection; refProjection = temp; ahead = true; } int a = projection.edgeIndex; int b = projection.pointIndex + 1; if (b < edge[a].points.size()) { distance += DistanceOf(projection.point, edge[projection.edgeIndex].points[b]); } for (; b < refProjection.pointIndex; b++) { distance += DistanceOf(edge[a].points[b], edge[a].points[b+1]); } distance += DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex]); } if (ahead) { distance *= -1.0; } } else { distance = DistanceOf(projection.point, refProjection.point); if (DistanceOf(projection.point, edge[projection.edgeIndex].points[projection.pointIndex]) > DistanceOf(refProjection.point, edge[refProjection.edgeIndex].points[refProjection.pointIndex])) { distance *= -1.0; } } return distance; } /*************************************************************** * 车辆所在道路,根据车辆的中轴前点 * @param currRoadIndex, 优先检测当前道路 * @param RoadMap * @param car * @return */ static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car) { int n = 0; int index = currRoadIndex; if (index < 0) { index = 0; } while (n++ < RoadMap.roads.size()) { bool changeSegment = false; vector roadOutLine; Polygon area; for (int i = 0; i < RoadMap.roads[index].leftEdge.size(); ++i) { for (int j = 0; j < RoadMap.roads[index].leftEdge[i].points.size(); ++j) { if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) { continue; } changeSegment = false; roadOutLine.push_back(RoadMap.roads[index].leftEdge[i].points[j]); } changeSegment = true; } for (int i = 0; i < RoadMap.roads[index].rightEdge.size(); ++i) { for (int j = RoadMap.roads[index].rightEdge[i].points.size() - 1; j >= 0; --j) { if (changeSegment && roadOutLine.size() > 0 && IsSamePoint(roadOutLine.back(), RoadMap.roads[index].leftEdge[i].points[j])) { continue; } changeSegment = false; roadOutLine.push_back(RoadMap.roads[index].rightEdge[i].points[j]); } changeSegment = true; } area.num = roadOutLine.size(); area.point = (PointF *) malloc(area.num * sizeof(PointF)); for (int i = 0; i < area.num; ++i) { area.point[i] = roadOutLine[i]; } if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &area) == GM_Containment) { free(area.point); break; } free(area.point); index = (index + 1) % RoadMap.roads.size(); } if (n >= RoadMap.roads.size()) { index = -1; } return index; } /************************************************ * 车轮压实线,前后轴交叉,前后轮迹交叉 * @param currCrashLineType * @param RoadMap * @param car * @param CarModelList * @return */ 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::iterator iter = CarModelList.begin(); 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]]; p41 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]]; struct RtkTime time1 = ((car_model *)(*iter))->tm; ++iter; p12 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]]; p22 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]]; p32 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]]; p42 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_rear_tire[TIRE_OUTSIDE]]; struct RtkTime time2 = ((car_model *)(*iter))->tm; if (TimeGetDiff(&time1, &time2) <= D_SEC(1)) { MakeLine(&frontLeftTireTrack, &p11, &p12); MakeLine(&frontRightTireTrack, &p21, &p22); MakeLine(&rearLeftTireTrack, &p31, &p32); MakeLine(&rearRightTireTrack, &p41, &p42); track = true; } } PointF p1, p2; Line redLine; // 左右路边实线 for (int n = 0; n < RoadMap.roads.size(); ++n) { for (int m = 0; m < RoadMap.roads[n].leftEdge.size(); ++m) { if (RoadMap.roads[n].leftEdge[m].character == LINE_SOLID && RoadMap.roads[n].leftEdge[m].points.size() >= 2) { p1 = RoadMap.roads[n].leftEdge[m].points[0]; for (int l = 1; l < RoadMap.roads[n].leftEdge[m].points.size(); ++l) { p2 = RoadMap.roads[n].leftEdge[m].points[l]; 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; } } } for (int m = 0; m < RoadMap.roads[n].rightEdge.size(); ++m) { if (RoadMap.roads[n].rightEdge[m].character == LINE_SOLID && RoadMap.roads[n].rightEdge[m].points.size() >= 2) { p1 = RoadMap.roads[n].rightEdge[m].points[0]; for (int l = 1; l < RoadMap.roads[n].rightEdge[m].points.size(); ++l) { p2 = RoadMap.roads[n].rightEdge[m].points[l]; 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; } } } // 分道实线 for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) { // 一组车道 for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) { // 多根分道线 for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) { // 一根分道线中线型相同的 int character = RoadMap.roads[n].separate[m].lines[l][a].character; if (RoadMap.roads[n].separate[m].lines[l][a].points.size() < 2) continue; p1 = RoadMap.roads[n].separate[m].lines[l][a].points[0]; for (int b = 1; b < RoadMap.roads[n].separate[m].lines[l][a].points.size(); ++b) { p2 = RoadMap.roads[n].separate[m].lines[l][a].points[b]; MakeLine(&redLine, &p1, &p2); if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection || IntersectionOf(redLine, rearTireAxle) == GM_Intersection) { lineType = character; 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 = character; 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) { // 非法跨线 lineType += 100; } } else if (lineType == LINE_HALF_SOLID_RIGHT && currCrashLineType != lineType && track) { if (IntersectionOfLine(p11, redLine) == 1 && IntersectionOfLine(p12, redLine) == -1) { // 非法跨线 lineType += 100; } } return lineType; } static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo) { for (int i = 0; i < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct.size(); ++i) { double d1 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].start, RoadMap.roads[roadIndex].rightEdge); double d2 = CalcDistanceReference(point, RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].end, RoadMap.roads[roadIndex].rightEdge); if (fabs(d1) < 1e-6 && d1 < 0.0 && d2 > 1e-6 && laneNo < RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct.size()) { return RoadMap.roads[roadIndex].separate[sepIndex].lane_direct[i].direct[laneNo]; } } return 0; } /******************************************** * 计算某点在哪个车道 * @param point * @param RoadMap * @param roadIndex * @return */ static bool GetLane(lane_t &lane, PointF point, road_exam_map &RoadMap, int roadIndex) { Line leftProjLine, rightProjLine; Line sep; PointF p0, p1, p2; if (roadIndex < 0 || roadIndex >= RoadMap.roads.size()) return false; p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge, point); p0 = p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, point); MakeLine(&leftProjLine, &point, &p1); MakeLine(&rightProjLine, &point, &p2); for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) { struct car_lane_pos { int lane_no; char pos; // 点在车道的左右位置 bool operator == (const int &x) { return (this->lane_no == x); } }; vector CLP; bool oneMoreIntersection = false; for (int j = 0; j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) { // 每一根线分组 bool intersection = false; for (int k = 0; !intersection && k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) { // 同一线型的线分组 p1 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0]; for (int m = 1; m < RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size(); ++m) { p2 = RoadMap.roads[roadIndex].separate[i].lines[j][k].points[m]; MakeLine(&sep, &p1, &p2); if (IntersectionOf(sep, leftProjLine) == GM_Intersection) { intersection = true; struct car_lane_pos temp; temp.lane_no = j; temp.pos = 'R'; CLP.push_back(temp); break; } if (IntersectionOf(sep, rightProjLine) == GM_Intersection) { intersection = true; struct car_lane_pos temp; temp.lane_no = j; temp.pos = 'L'; CLP.push_back(temp); break; } p1 = p2; } } if (!intersection) { struct car_lane_pos temp; temp.lane_no = j; temp.pos = 0; CLP.push_back(temp); } else { oneMoreIntersection = true; } } if (oneMoreIntersection) { // 考虑到采集偏差可能每一根线无法对齐,只要其中一根相交,其它不相交的也参与 for (vector::iterator it = CLP.begin(); it != CLP.end(); ++it) { if ((*it).pos == 0) { int m = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no].size(); int n = RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points.size(); if (DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], point) > DistanceOf(RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1], point)) { // 车辆接近车道尾部 MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-2], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][m-1].points[n-1]); } else { // 车辆接近车道头部 MakeLine(&sep, &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[0], &RoadMap.roads[roadIndex].separate[i].lines[(*it).lane_no][0].points[1]); } struct car_lane_pos temp; temp.lane_no = (*it).lane_no; if (IntersectionOfLine(point, sep) == -1) { temp.pos = 'R'; } else { temp.pos = 'L'; } (*it) = temp; } } lane.road = roadIndex; lane.sep = i; lane.no = CLP.size(); for (int x = 0; x < CLP.size(); ++x) { if (CLP[x].pos == 'L') { lane.no = x; break; } } 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(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) { AppTimer_delete(TurnSignalError13ColdTimer); turnError13Cold = true; } static void TurnSignalError14ColdTimer(union sigval sig) { AppTimer_delete(TurnSignalError14ColdTimer); turnError14Cold = true; } static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime) { if (err == 13 && turnError13Cold) { DEBUG("起步不开转向灯"); AddExamFault(13, rtkTime); turnError13Cold = false; AppTimer_delete(TurnSignalError13ColdTimer); AppTimer_add(TurnSignalError13ColdTimer, TURN_ERROR_COLD_TIME); } else if (err == 14 && turnError14Cold) { DEBUG("起步转向灯不足3秒"); AddExamFault(14, rtkTime); turnError14Cold = false; AppTimer_delete(TurnSignalError14ColdTimer); AppTimer_add(TurnSignalError14ColdTimer, TURN_ERROR_COLD_TIME); } } static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime) { double moveDistance; if (startCar == START_CAR_NOT_DO) { startPoint = car->basePoint; reportRPMOver = false; startCar = START_CAR_DOING; PlayTTS("请起步"); } else if (startCar == START_CAR_DOING) { moveDistance = DistanceOf(startPoint, car->basePoint); DEBUG("起步行驶距离 %f", moveDistance); if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) { startCarLeftTurnSignal = true; Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime); } if (!checkStartCarSignal && moveDirect == 1) { checkStartCarSignal = true; if (!startCarLeftTurnSignal) { ReportTurnSignalError(13, rtkTime); } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) { ReportTurnSignalError(14, rtkTime); } } if (moveDistance > START_CAR_MOVE_DISTANCE) { if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) { DEBUG("Handbreak active move over 10m"); // 手刹拉起状态下,行驶了10米以上,不合格 AddExamFault(25, rtkTime); } else if (handBreakActive) { // 手刹拉起状态下,行驶了1米以上,扣10分 DEBUG("Handbreak active move over 1M"); AddExamFault(26, rtkTime); } PlayTTS("完成起步"); DEBUG("############# 完成起步 ############"); startCar = START_CAR_DONE; } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) { if (!checkDoor) { checkDoor = true; if (ReadCarStatus(DOOR) == DOOR_OPEN) { // 车门未完全关闭,不合格 DEBUG("车门未关闭"); AddExamFault(23, rtkTime); } if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) { handBreakActive = true; } } } if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) { // 转速超标,不合格 DEBUG("转速超标"); AddExamFault(29, rtkTime); reportRPMOver = true; } } else { } 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(CrashLineType, 0, RoadMap, car, CarModelList); lane_t laneInfo; double redist = -1; laneInfo.road = -1; laneInfo.sep = -1; laneInfo.no = -1; if (ri >= 0) { GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri); int m = RoadMap.roads[ri].rightEdge.size(); int n = RoadMap.roads[ri].rightEdge[m-1].points.size(); PointF base; base.X = 428922.2985; base.Y = 3292119.5457; redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base, RoadMap.roads[ri].rightEdge); } DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts); UpdateCarSensor(rtkTime); UpdataOdo(speed, moveDirect, rtkTime); // 超速检测 if (moveDirect != 0 && speed > MAX_SPEED) { if (!occurOverSpeed) { occurOverSpeed = true; // 超速,不合格 DEBUG("超速 %f", speed); AddExamFault(10, rtkTime); } } else { occurOverSpeed = false; } // 副刹车检测 if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) { // 副刹车踩下,不合格 if (!occurSecondBreak) { DEBUG("副刹车动作了"); occurSecondBreak = true; AddExamFault(17, rtkTime); } } else { occurSecondBreak = false; } // 挡位匹配检测 bool currGearError = false; bool currGearNSlide = false; switch (ReadCarStatus(GEAR)) { case GEAR_N: if (moveDirect != 0) { // 空档滑行 currGearNSlide = true; } break; case GEAR_1: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) { currGearError = true; } break; case GEAR_2: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) { currGearError = true; } break; case GEAR_3: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) { currGearError = true; } break; case GEAR_4: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) { currGearError = true; } break; case GEAR_5: if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) { currGearError = true; } break; default:break; } // 空档滑行超时 if (currGearNSlide && prevGearNSlide) { gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10); } if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) { // 空档滑行超5秒,不合格 DEBUG("挡位滑行,超过5秒"); AddExamFault(8, rtkTime); gearNSlideTime = 0; } prevGearNSlide = currGearNSlide; if (prevGearNSlide) { Rtk2DriveTimer(gearNSlideTimePoint, rtkTime); } else { gearNSlideTime = 0; } // 挡位不匹配超时 if (currGearError && prevGearError) { gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10, gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10); } if (gearErrorTime > GEAR_ERROR_TIMEOUT) { // 累计15秒,挡位-车速不匹配,不合格 DEBUG("挡位错误超过15秒"); AddExamFault(6, rtkTime); gearErrorTime = 0; } prevGearError = currGearError; if (prevGearError) { Rtk2DriveTimer(gearErrorTimePoint, rtkTime); } // 起步后滑 if (moveDirect != prevMoveDirect) { if (moveDirect == 0) { stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); reportStopCarOnRedArea = false; DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss); if (slideNormalDistance) { // 后滑,扣10分 AddExamFault(18, rtkTime); DEBUG("后滑超过10厘米, 但不超过30厘米"); } slideNormalDistance = false; slideLongDistance = false; occurSlide = false; } else if (moveDirect == -1 && prevMoveDirect == 0) { DEBUG("开始后滑"); stopPoint = car->basePoint; occurSlide = true; } 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)) { // 停车超2秒,停在红区,不合格 AddExamFault(16, rtkTime); DEBUG("禁停区停车"); reportStopCarOnRedArea = true; }*/ } else if (moveDirect == -1) { // 持续后滑 if (occurSlide) { double slideDistance = DistanceOf(stopPoint, car->basePoint); if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) { slideNormalDistance = true; } if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) { // 后滑超过30厘米, 不合格 AddExamFault(5, rtkTime); DEBUG("后滑超过30厘米"); slideLongDistance = true; slideNormalDistance = false; occurSlide = false; } } } else { // 前进 } // 检测通过路口、人行道等区域时,释放刹车或减速 CheckBreakActive(RoadMap, car, CarModelList, speed, moveDirect, rtkTime); // 检测离开此路段,全车需不在范围内 currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car); DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime); DetectLane(RoadMap, car, currExamMapIndex, rtkTime); // 检测压线状态 TestRoadStartCar(car, speed, moveDirect, rtkTime); if (startCar != START_CAR_DONE) return; if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) { // 在合适条件下停车结束考试 StartStopCarExam("请靠边停车"); stopCar = STOP_CAR_DOING; } else if (stopCar == STOP_CAR_DOING) { if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0) stopCar = STOP_CAR_DONE; } // 执行某个项目 if (currRoadItem != NULL) { if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) { if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) { DEBUG("变道距离超标"); AddExamFault(3, rtkTime); currRoadItem = NULL; } } else if (currRoadItem->active == ROAD_ITEM_OVERTAKE) { if (!overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_RANGE) { DEBUG("超车距离超标"); AddExamFault(3, rtkTime); currRoadItem = NULL; } else if (overtake && TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss * 10, overTakeCmpTime.hour, overTakeCmpTime.min, overTakeCmpTime.sec, overTakeCmpTime.msec * 10) > OVERTAKE_HOLD_TIME) { DEBUG("回原车道"); PlayTTS("请返回原车道"); currRoadItem = NULL; } } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) { if (ExecuteOperateGearExam(rtkTime) < 0) { currRoadItem = NULL; } } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) { if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) { currRoadItem = NULL; } } } // 检测由触发线控制的项目 else if (currExamMapIndex >= 0) { trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList); if (new_item != NULL && !new_item->cmp) { currRoadItem = new_item; if (!currRoadItem->tts.empty()) PlayTTS(currRoadItem->tts.c_str()); // 初始时间和距离限制 roadItemStartPoint = car->basePoint; Rtk2DriveTimer(roadItemStartTime, rtkTime); if (currRoadItem->active == ROAD_ITEM_OVERTAKE) { overtake = false; } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) { StartOperateGearExam(rtkTime); } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) { StartDriveStraightExam(currRoadItem->tts); } } } } void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime) { tm.hour = rtkTime->hh; tm.min = rtkTime->mm; tm.sec = rtkTime->ss; tm.msec = rtkTime->mss; } static char isTurn(int currYaw, int prevYaw, int &ang) { // DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw); int deltaAng = 0; if (ABS(currYaw - prevYaw) > 180) { deltaAng = 360 - ABS(currYaw-prevYaw); } else { deltaAng = ABS(currYaw - prevYaw); } ang = deltaAng; // DEBUG("角度差值 %d", deltaAng); if (deltaAng >= TURN_THRESHOLD) { if((( currYaw + 360 - prevYaw) % 360) < 180) { // DEBUG("右转"); return 'R'; } else { // DEBUG("左转"); return 'L'; } } return 'N'; } static char CheckCarTurn(LIST_CAR_MODEL &CarModelList) { // 最近2秒内,每0.5秒的角度差大于5度,且方向相同,连续4次;或突现超30度的转向;认为转向。 if (CarModelList.size() < 1) return false; list::iterator iter = CarModelList.begin(); car_model *c1 = *iter, *c2; ++iter; char turn[TURN_CHECK_CNT] = {0}; int checkCnt = 0; // 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) { int ang = 0; turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang); // 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] == 'N') { break; } else if (ang >= 30) { DEBUG("左右转确认 %c", turn[checkCnt]); return turn[checkCnt]; } c1 = c2; checkCnt++; } ++iter; } int i = 0; for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) { if (turn[i] != turn[i+1]) break; } if (i == TURN_CHECK_CNT-1) { DEBUG("左右转确认 %c", turn[0]); return turn[0]; } return 0; } /********************************************************** * 按整个车身是否覆盖计算 * @param RoadMapList * @param car * @return */ /*static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car) { bool ret = false; Polygon carBody; 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; } } break; } } free(carBody.point); return ret; }*/ bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList) { if (CarModelList.size() < 2) return false; list::iterator iter = CarModelList.begin(); Line trace; PointF p1, p2; p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]]; ++iter; p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]]; MakeLine(&trace, &p1, &p2); if (IntersectionOf(trace, line) == GM_Intersection && IntersectionOfLine(p1, line) == -1) { return true; } return false; } /*************************************************** * 接近路口时,提示下一步怎么走 * @param road * @param car * @param CarModelList */ /*static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList) { // 计算车前进轨迹延长线 double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]); PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw); Line extLine; MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint); if (IntersectionOf(extLine, road.stopLine) == GM_Intersection && IntersectionOfLine(extPoint, road.stopLine) == -1) { if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) { // 接近路口后,要检查车辆是否进入错误车道 DEBUG("接近路口"); road.arrivedTail = true; if (!road.tts.empty()) PlayTTS(road.tts.c_str()); } } else if (road.arrivedTail) { road.arrivedTail = false; } }*/ // 路口,提示,前30米进入, 离开 // 人行道,车站,前30米进入,离开 // 学校,进入,离开 map CrossingStatus; static void ChangeCrossingStatus(int roadIndex, int index, int status) { int key = roadIndex * 100 + index; auto it = CrossingStatus.find(key); if (it != CrossingStatus.end()) { CrossingStatus.erase(it); } CrossingStatus.insert(pair(key, status)); } static int GetCrossingStatus(int roadIndex, int index) { int key = roadIndex * 100 + index; auto it = CrossingStatus.find(key); if (it != CrossingStatus.end()) { return it->second; } return -1; } enum { CROSSING_STATUS_INVALID = -1, CROSSING_STATUS_GUIDE, CROSSING_STATUS_REDUCE_SPEED, CROSSING_STATUS_BREAK_CAR, CROSSING_STATUS_STOP_CAR }; static void CheckCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList) { for (int i = 0; i < road.stopLine.size(); ++i) { if (CrashTheLine(road.stopLine[i].line, car, CarModelList)) { if (road.stopLine[i].stopFlag) { if (GetCrossingStatus(roadIndex, i) != CROSSING_STATUS_STOP_CAR) { } } else { if (GetCrossingStatus(roadIndex, i) != CROSSING_STATUS_BREAK_CAR) { } } ChangeCrossingStatus(roadIndex, i, CROSSING_STATUS_INVALID); } else { } PointF point; double distance; point.X = road.stopLine[i].line.X1; point.Y = road.stopLine[i].line.Y1; distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, road.rightEdge); if (distance > 10 && distance < 50) { if (GetCrossingStatus(roadIndex, i) == CROSSING_STATUS_INVALID) { if (!road.stopLine[i].tts.empty()) { PlayTTS(road.stopLine[i].tts.c_str()); } ChangeCrossingStatus(roadIndex, i, CROSSING_STATUS_GUIDE); } } else if (distance > 0 && distance < 30) { if (GetCrossingStatus(roadIndex, i) != 1) { ChangeCrossingStatus(roadIndex, i, 1); } } } } static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList) { for (int i = 0; i < RoadMap.triggerLines.size(); ++i) { if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) { Line triggerLine; if (RoadMap.triggerLines[i].leftPoints.size() != RoadMap.triggerLines[i].points.size()) { RoadMap.triggerLines[i].leftPoints.clear(); for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) { RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j])); } for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) { DEBUG("触发线补齐 road %d id %d type %d (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.roads[index].id, RoadMap.triggerLines[i].id, RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].points[0].X, RoadMap.triggerLines[i].points[0].Y, RoadMap.triggerLines[i].leftPoints[0].X, RoadMap.triggerLines[i].leftPoints[0].Y); } } MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &RoadMap.triggerLines[i].leftPoints[0]); if (CrashTheLine(triggerLine, car, CarModelList)) { DEBUG("触发项目 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2); return &RoadMap.triggerLines[i]; } } } return NULL; } static bool AllCmp(road_exam_map &map) { for (int i = 0; i < map.triggerLines.size(); ++i) { if (!map.triggerLines[i].cmp) return false; } return true; } /************************************************************************ * 开始新的考试后,清除地图所有的刹车、停车记录 * @param map */ 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; // } } static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { 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); Line extLine; MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint); // 路口刹车点 /* 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) { map.roads[i].activeBreak = true; } if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], map.roads[i].stopLine) < DISTANCE_STOP_CAR_TO_STOP_LINE && moveDirect == 0) { map.roads[i].activeStop = true; } } // 跨线后,检查刹车动作 if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) { if (map.roads[i].stopFlag != 0 && !map.roads[i].activeStop) { // 不停车瞭望,不合格 DEBUG("不停车瞭望"); if (map.roads[i].active == ROAD_ACTIVE_FORWARD) { AddExamFault(42, rtkTime); } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) { AddExamFault(44, rtkTime); } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) { AddExamFault(47, rtkTime); } } if (!map.roads[i].activeBreak) { // 不按规定减速,不合格 DEBUG("不按规定减速"); if (map.roads[i].active == ROAD_ACTIVE_FORWARD) { AddExamFault(41, rtkTime); } else if (map.roads[i].active == ROAD_ACTIVE_TURN_LEFT) { AddExamFault(43, rtkTime); } else if (map.roads[i].active == ROAD_ACTIVE_TURN_RIGHT) { AddExamFault(46, rtkTime); } } } }*/ // 人行道、公交站刹车点;学校限速区 // DEBUG("补全特殊区域 size = %d", map.specialAreas.size()); 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 = CalcProjectionWithRoadEdge(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 = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]); // 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) { // DEBUG("斑马线"); 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("进入减速区 %d", map.specialAreas[i].type); if (BreakDone == BREAK_ACTIVE) { map.specialAreas[i].activeBreak = true; } } // 跨线后,检查刹车动作 if (CrashTheLine(startLine, car, CarModelList)) { if (!map.specialAreas[i].activeBreak) { // 不按规定减速,不合格 DEBUG("不按规定减速"); if (map.specialAreas[i].type == ZEBRA_CROSSING) { AddExamFault(48, rtkTime); } else { AddExamFault(50, rtkTime); } } else { DEBUG("按规定减速"); } } } else if (map.specialAreas[i].type == SCHOOL_AREA) { Polygon school; // DEBUG("学校"); school.num = 4; school.point = (PointF *) malloc(school.num * sizeof(PointF)); school.point[0] = map.specialAreas[i].area[0]; school.point[1] = map.specialAreas[i].area[1]; school.point[2] = map.specialAreas[i].leftPoints[1]; school.point[3] = map.specialAreas[i].leftPoints[0]; if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) { if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) { if (!map.specialAreas[i].overSpeed) { DEBUG("通过学校区域超速"); AddExamFault(49, rtkTime); map.specialAreas[i].overSpeed = true; } } } free(school.point); } } // DEBUG("补全特殊区域 over"); } #if 0 typedef struct { int road; int segment; int lane; } CarOnTrackInfo_t; static CarOnTrackInfo_t CarOnTrackInfo; void TestRoadGeneral(struct road_exam2_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { // 检测车道变更 CarOnTrackInfo_t newCarOnTrackInfo; if (newCarOnTrackInfo.road == CarOnTrackInfo.road && newCarOnTrackInfo.segment == CarOnTrackInfo.segment && newCarOnTrackInfo.lane != CarOnTrackInfo.lane) { } CarOnTrackInfo = newCarOnTrackInfo; } /************************************************** * 车辆当前所在路段,车头需越过起始线一定距离 * @param currIndex * @param map * @param car */ static void EnterRoad(int &currIndex, struct road_exam2_map &map, const car_model *car) { Polygon carBody; 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]]; } Polygon roadArea; roadArea.num = 0; roadArea.point = NULL; for (int i = 0; i < map.roads.size(); ++i) { if (roadArea.point != NULL) { free(roadArea.point); } for (int x = 0; x < map.roads[i].leftEdge.size(); ++x) { roadArea.num += map.roads[i].leftEdge[x].points.size(); } for (int x = 0; x < map.roads[i].rightEdge.size(); ++x) { roadArea.num += map.roads[i].rightEdge[x].points.size(); } roadArea.point = (PointF *) malloc(roadArea.num * sizeof(PointF)); int n = 0; for (int j = 0; j < map.roads[i].leftEdge.size(); j++) { for (int x = 0; x < map.roads[i].leftEdge[j].points.size(); ++x) { if (n > 0 && x == 0 && isEqual2(roadArea.point[n-1].X, map.roads[i].leftEdge[j].points[x].X) && isEqual2(roadArea.point[n-1].Y, map.roads[i].leftEdge[j].points[x].Y)) { // 第一个点已经存在于上一条线 } else { roadArea.point[n++] = map.roads[i].leftEdge[j].points[x]; } } } for (int j = map.roads[i].rightEdge.size(); j > 0; j--) { for (int x = map.roads[i].rightEdge[j].points.size(); x > 0; --x) { if (n > 0 && x == map.roads[i].rightEdge[j].points.size() && isEqual2(roadArea.point[n-1].X, map.roads[i].rightEdge[j - 1].points[x-1].X) && isEqual2(roadArea.point[n-1].Y, map.roads[i].rightEdge[j - 1].points[x-1].Y)) { // 第一个点已经存在于上一条线 } else { roadArea.point[n++] = map.roads[i].rightEdge[j - 1].points[x - 1]; } } } roadArea.num = n; if (IntersectionOf(car->carXY[car->axial[AXIAL_FRONT]], &roadArea) == GM_Containment) { currIndex = i; goto CHECK_CAR_ON_ROAD_END; } } currIndex = INVALID_ROAD; CHECK_CAR_ON_ROAD_END: if (roadArea.point != NULL) { free(roadArea.point); } free(carBody.point); } /****************************************************** * 全车都需离开这个区域 * @param currIndex * @param map * @param car * @return */ static bool ExitRoad(int currIndex, struct road_exam2_map &map, const car_model *car) { // 全车都需不在地图中 bool ret = false; if (currIndex == INVALID_ROAD || currIndex >= map.roads.size()) return ret; Polygon carBody; 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]]; } if (IntersectionOf(&carBody, &map.roads[currIndex].area) == GM_None) { ret = true; } free(carBody.point); return ret; } static bool CrashSeparateLine(int currIndex, struct road_exam2_map &map, const car_model *car) { Line frontAxle, rearAxle; if (currIndex == INVALID_ROAD || currIndex >= map.roads.size()) return false; 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 < map.roads[currIndex].separate.size(); i++) { // 分段中的每条线 for (int j = 0; j < map.roads[currIndex].separate[i].line.size(); j++) { Line theLine; int p1 = 0; for (int p2 = 1; p2 < map.roads[currIndex].separate[i].line[j].num; ++p2) { MakeLine(&theLine, &map.roads[currIndex].separate[i].line[j].point[p1], &map.roads[currIndex].separate[i].line[j].point[p2]); if (IntersectionOf(theLine, frontAxle) == GM_Intersection || IntersectionOf(theLine, rearAxle) == GM_Intersection) { return true; } p1 = p2; } } } return false; } static void DetectSeparate(int currIndex, struct road_exam2_map &map, const car_model *car) { int segment; int lane = -1; CarOnTrackInfo_t newInfo; if (currIndex == INVALID_ROAD || currIndex >= map.roads.size()) return; // 遍历每一分段 for (int i = 0; i < map.roads[currIndex].separate.size(); i++) { int separate_line_num = map.roads[currIndex].separate[i].lines.size(); struct vrecord_ { int valid; int p1; int p2; }; vector vrecord; vrecord.clear(); bool match1 = false; // 遍历当前分段的每一条线 for (int j = 0; j < separate_line_num; ++j) { Line theLine; int p1 = 0; struct vrecord_ v; v.valid = 0; // 单独的一条虚线 for (int p2 = 1; p2 < map.roads[currIndex].separate[i].lines[j].num; ++p2) { MakeLine(&theLine, &map.roads[currIndex].separate[i].lines[j].point[p1], &map.roads[currIndex].separate[i].lines[j].point[p2]); if (VerticalPointOnLine(car->basePoint, theLine)) { v.valid = 1; v.p1 = p1; v.p2 = p2; match1 = true; break; } p1 = p2; } vrecord.push_back(v); lane = separate_line_num;// } if (match1) { for (int x = 0; x < vrecord.size(); ++x) { if (vrecord[i].valid == 0) { // 首尾两段线的延申必有一个垂点 if (DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[0]) < DistanceOf(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[currIndex].separate[i].lines[x].point[1])) { vrecord[x].p1 = 0; vrecord[x].p2 = 1; } else { vrecord[x].p1 = map.roads[currIndex].separate[i].lines[x].num - 2; vrecord[x].p2 = map.roads[currIndex].separate[i].lines[x].num - 1; } } int rel = IntersectionOfLine(map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p1], map.roads[currIndex].separate[i].lines[x].point[vrecord[x].p2], car->basePoint); if (rel != -1) { newInfo.road = currIndex; newInfo.segment = i; newInfo.lane = x; break; } } newInfo.road = currIndex; newInfo.segment = i; newInfo.lane = vrecord.size(); break; } } } #endif