| | |
| | | static map<int, int> CrossingHint; |
| | | static map<int, bool> ErrorLaneReport; |
| | | |
| | | typedef struct { |
| | | int road_id; |
| | | double distance; |
| | | } trigger_detect_t; |
| | | static map<int, trigger_detect_t> TriggerDetect; // 距离本路段各触发点的距离 |
| | | |
| | | #define ROAD_EXAM_READY_NEXT 0 |
| | | #define ROAD_EXAM_FREE_RUN 1 |
| | | #define ROAD_EXAM_ITEM_CAR_START 2 |
| | |
| | | static int prevTurnWise; |
| | | |
| | | static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end); |
| | | static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList); |
| | | static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect); |
| | | static int isTurn(int currYaw, int prevYaw, int thres); |
| | | static void ResetTurnDetect(const car_model *car); |
| | | static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime); |
| | |
| | | |
| | | static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car); |
| | | |
| | | static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList); |
| | | static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect); |
| | | |
| | | static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car); |
| | | |
| | |
| | | RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC; |
| | | |
| | | RoadExamStatus = ROAD_EXAM_READY_NEXT; |
| | | |
| | | TriggerDetect.clear(); |
| | | |
| | | win = false; |
| | | } |
| | |
| | | void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) |
| | | { |
| | | double BigStraightRoadFree = 0, RoadCrossingFree = 0, TargetFree = 0; |
| | | |
| | | int forward = 0; // 车辆相对当前道路的 |
| | | UpdateCarSensor(rtkTime); |
| | | |
| | | UpdataOdo(speed, moveDirect, rtkTime); |
| | |
| | | prevMoveDirect = moveDirect; |
| | | } else if (moveDirect == 0) { |
| | | // 持续停车 |
| | | if (TimeGetDiff(rtkTime, &stopTimepoint) >= examParam.road_pause_criteria && !StopCarOnRedArea && |
| | | if (TimeGetDiff(rtkTime, &stopTimepoint) >= CorrectPauseCriteria(examParam.road_pause_criteria) && !StopCarOnRedArea && |
| | | StopOnRedArea(RoadMap, car)) { |
| | | // 停车超2秒,停在红区,不合格 |
| | | AddExamFault(16, rtkTime); |
| | |
| | | car_sensor_value_t brk = ReadCarSensorValue(BREAK); |
| | | // 检测通过路口、人行道等区域时,释放刹车或减速 |
| | | TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime); |
| | | // 检查 |
| | | projection_t projection; |
| | | |
| | | uint32_t tm1 = AppTimer_GetTickCount(); |
| | | |
| | | projection = CalcProjectionWithRoadEdgeEx(RoadMap.roads[currExamMapIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]); |
| | | |
| | | if (projection.edgeIndex < RoadMap.roads[currExamMapIndex].rightEdge.size()) { |
| | | if (projection.pointIndex < RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points.size() - 1) { |
| | | double roadYaw = YawOf(RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex + 1], |
| | | RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex]); |
| | | |
| | | int deltaAng; |
| | | |
| | | if (ABS((int)roadYaw - (int)car->yaw) > 180) { |
| | | deltaAng = 360 - ABS((int)roadYaw - (int)car->yaw); |
| | | } else { |
| | | deltaAng = ABS((int)roadYaw - (int)car->yaw); |
| | | } |
| | | |
| | | if (deltaAng < 90) { |
| | | forward = 1; |
| | | } else { |
| | | forward = -1; |
| | | } |
| | | // DEBUG("路段方向 %f 车辆方向 %f 差值 %d 耗时 %ld", roadYaw, car->yaw, deltaAng, AppTimer_GetTickCount() - tm1); |
| | | } |
| | | } |
| | | } |
| | | ExitTarget(RoadMap, car, CarModelList, rtkTime); |
| | | |
| | |
| | | ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, |
| | | BigStraightRoadFree, |
| | | TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree); |
| | | ItemExam2(RoadMap, currExamMapIndex, car, CarModelList); |
| | | ItemExam2(RoadMap, currExamMapIndex, car, CarModelList, forward, moveDirect); |
| | | } |
| | | } |
| | | |
| | |
| | | } |
| | | } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) { |
| | | if (ReadOdo() - freeRunDistance > freeRunExceptDistance) { |
| | | DEBUG("寻找下一个子项目"); |
| | | RoadExamStatus = ROAD_EXAM_READY_NEXT; |
| | | } |
| | | } else { |
| | |
| | | } |
| | | |
| | | if (!testing) { |
| | | DEBUG("当前子项结束"); |
| | | RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED; |
| | | if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) { |
| | | freeRunExceptDistance = 60.0; |
| | |
| | | } |
| | | } |
| | | |
| | | static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect) |
| | | { |
| | | int item = EntryItem(roadIndex, RoadMap, car, CarModelList); |
| | | int item = 0; |
| | | |
| | | if (RoadExamStatus == ROAD_EXAM_READY_NEXT) { |
| | | item = EntryItem(roadIndex, RoadMap, car, CarModelList, forward, moveDirect); |
| | | } |
| | | |
| | | if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) { |
| | | StartDriveStraightExam(); |
| | |
| | | return false; |
| | | } |
| | | |
| | | static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList) |
| | | static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect) |
| | | { |
| | | if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) |
| | | if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0 || CarModelList.size() < 2) |
| | | return -1; |
| | | |
| | | list<car_model *>::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); |
| | | |
| | | double trace_length = DistanceOf(p1, p2); |
| | | |
| | | if (TriggerDetect.size() > 0) { |
| | | auto it = TriggerDetect.begin(); |
| | | |
| | | if (it->second.road_id != RoadMap.roads[index].id) { |
| | | TriggerDetect.clear(); |
| | | } |
| | | } |
| | | |
| | | for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) { |
| | | if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) { |
| | | |
| | | |
| | | /*Line triggerLine; |
| | | |
| | | PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]); |
| | |
| | | return RoadMap.triggerLines[i].active; |
| | | }*/ |
| | | |
| | | vector<double> vec; |
| | | /* vector<double> vec; |
| | | |
| | | PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge, |
| | | car->carXY[car->axial[AXIAL_FRONT]]); |
| | |
| | | if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) { |
| | | DEBUG("触发项目 %d", RoadMap.examScheme[0].triggerLines[j].active); |
| | | return RoadMap.examScheme[0].triggerLines[j].active; |
| | | }*/ |
| | | |
| | | // double angleOf = DeltaYaw(YawOf(RoadMap.examScheme[0].triggerLines[j].points[0], car->carXY[car->axial[AXIAL_FRONT]]), car->yaw); |
| | | // auto it = TriggerDetect.find(j); |
| | | // |
| | | // if (it != TriggerDetect.end()) { |
| | | // if (fabs(angleOf - it->second.angleOf) > 2.0) { |
| | | // bool trigger = false; |
| | | // |
| | | // trigger_detect_t up; |
| | | // |
| | | // if (moveDirect == 1 && forward == 1 && it->second.angleOf <= 90 && angleOf > 90) { |
| | | // trigger = true; |
| | | // } |
| | | // DEBUG("观察子<%d> 更新 %f -> %f", j, it->second.angleOf, angleOf); |
| | | // |
| | | // up.angleOf = angleOf; |
| | | // up.road_id = RoadMap.roads[index].id; |
| | | // |
| | | // TriggerDetect[j] = up; |
| | | // |
| | | // |
| | | // |
| | | // if (trigger) { |
| | | // DEBUG("pos %d 触发项目 %d", j, RoadMap.examScheme[0].triggerLines[j].active); |
| | | // return RoadMap.examScheme[0].triggerLines[j].active; |
| | | // } |
| | | // } |
| | | // } else { |
| | | // trigger_detect_t up; |
| | | // |
| | | // up.road_id = RoadMap.roads[index].id; |
| | | // up.angleOf = angleOf; |
| | | // |
| | | // TriggerDetect.insert(pair<int, trigger_detect_t>(j, up)); |
| | | // |
| | | // DEBUG("观察子<%d> 加入 %f", j, angleOf); |
| | | // } |
| | | |
| | | // double dist = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], |
| | | // RoadMap.examScheme[0].triggerLines[j].points[0]); |
| | | auto it = TriggerDetect.find(j); |
| | | |
| | | if (it != TriggerDetect.end()) { |
| | | PointF verticalPoint; |
| | | |
| | | if (moveDirect == 1 && |
| | | forward == 1 && |
| | | trace_length > 0.01 && |
| | | VerticalPointOnLine(RoadMap.examScheme[0].triggerLines[j].points[0], trace, verticalPoint) && |
| | | DistanceOf(verticalPoint, RoadMap.examScheme[0].triggerLines[j].points[0]) < it->second.distance) { |
| | | DEBUG("pos %d 触发项目 %d", j, RoadMap.examScheme[0].triggerLines[j].active); |
| | | |
| | | DEBUG("(%f, %f) ON (%f, %f)-(%f, %f) IS (%f, %f) trace %f", |
| | | RoadMap.examScheme[0].triggerLines[j].points[0].X, |
| | | RoadMap.examScheme[0].triggerLines[j].points[0].Y, |
| | | trace.X1, |
| | | trace.Y1, |
| | | trace.X2, |
| | | trace.Y2, |
| | | verticalPoint.X, |
| | | verticalPoint.Y, |
| | | trace_length); |
| | | |
| | | return RoadMap.examScheme[0].triggerLines[j].active; |
| | | } |
| | | } else { |
| | | PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.examScheme[0].triggerLines[j].points[0]); |
| | | PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge, RoadMap.examScheme[0].triggerLines[j].points[0]); |
| | | |
| | | double dist1 = DistanceOf(p1, RoadMap.examScheme[0].triggerLines[j].points[0]); |
| | | double dist2 = DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]); |
| | | |
| | | trigger_detect_t up; |
| | | |
| | | up.road_id = RoadMap.roads[index].id; |
| | | up.distance = MAX(dist1, dist2); |
| | | |
| | | TriggerDetect.insert(pair<int, trigger_detect_t>(j, up)); |
| | | |
| | | DEBUG("观察子<%d> 加入 %f", j, MAX(dist1, dist2)); |
| | | } |
| | | } |
| | | } |