| | |
| | | static const double CHECK_STAGE_DISTANCE = 100.0; |
| | | static const double MAX_OFFSET_DISTANCE = 0.3; |
| | | |
| | | static int ttsPlayEnd; |
| | | static bool crossStartLine; |
| | | static bool reportOffsetOver; |
| | | static double edgeDistance; |
| | | static double distanceToStartSum; |
| | | |
| | | void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) { |
| | | if (index == -1) |
| | | return; |
| | | static double CalcDistance2Edge(road_t &road, const car_model *car); |
| | | |
| | | DEBUG("进入路考直线行驶地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type); |
| | | void StartDriveStraightExam(std::string tts) { |
| | | DEBUG("开始直线行驶"); |
| | | |
| | | if (!RoadMapList[index].tts.empty()) { |
| | | DEBUG("播放TTS"); |
| | | PlayTTS(RoadMapList[index].tts.c_str()); |
| | | if (!tts.empty()) { |
| | | PlayTTS(tts.c_str()); |
| | | } else { |
| | | DEBUG("没有TTS"); |
| | | } |
| | | |
| | | crossStartLine = false; |
| | | distanceToStartSum = 0; |
| | | reportOffsetOver = false; |
| | | } |
| | | |
| | | int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, |
| | | int ExecuteDriveStraightExam(road_t &road, const car_model *car, |
| | | LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { |
| | | Line road_edge; |
| | | |
| | | static PointF startPoint; |
| | | |
| | | MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]); |
| | | double dis2roadEdge = 0; |
| | | |
| | | { |
| | | double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge); |
| | | if (ttsPlayEnd == 1) { |
| | | ttsPlayEnd = 2; |
| | | startPoint = car->basePoint; |
| | | edgeDistance = CalcDistance2Edge(road, car); // 基准边距 |
| | | |
| | | double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge); |
| | | |
| | | MA_SendDistance(6 - (l1+l2)/2.0, (l1+l2)/2.0); |
| | | DEBUG("当前基准路边间距 %f", edgeDistance); |
| | | } |
| | | |
| | | if (!crossStartLine) { |
| | | PointF p1, p2; |
| | | if (ttsPlayEnd != 2) |
| | | return 1; |
| | | |
| | | p1.X = RoadMapList[index].stopLine.X1; |
| | | p1.Y = RoadMapList[index].stopLine.Y1; |
| | | p2.X = RoadMapList[index].stopLine.X2; |
| | | p2.Y = RoadMapList[index].stopLine.Y2; |
| | | double distanceToStart = DistanceOf(car->basePoint, startPoint); |
| | | dis2roadEdge = CalcDistance2Edge(road, car); |
| | | |
| | | if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) { |
| | | DEBUG("跨过标记线,开始直线测试"); |
| | | crossStartLine = true; |
| | | startPoint = car->basePoint; |
| | | edgeDistance = DistanceOf(car->basePoint, road_edge); |
| | | DEBUG("路边间距 %f --- %f", dis2roadEdge, edgeDistance); |
| | | |
| | | DEBUG("当前基准路边间距 %f", edgeDistance); |
| | | } |
| | | } else { |
| | | double distanceToStart = DistanceOf(car->basePoint, startPoint); |
| | | if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米"); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | reportOffsetOver = true; |
| | | |
| | | DEBUG("路边间距 %f --- %f", DistanceOf(car->basePoint, road_edge), edgeDistance); |
| | | |
| | | if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) { |
| | | DEBUG("直线偏移大于30厘米"); |
| | | // 偏移大于30厘米,不合格 |
| | | AddExamFault(30, rtkTime); |
| | | reportOffsetOver = true; |
| | | |
| | | ////////////////////////////////////////////// |
| | | startPoint = car->basePoint; |
| | | edgeDistance = DistanceOf(car->basePoint, road_edge); |
| | | reportOffsetOver = false; |
| | | } |
| | | |
| | | if (distanceToStart > CHECK_STAGE_DISTANCE) { |
| | | DEBUG("复位边距偏移量"); |
| | | startPoint = car->basePoint; |
| | | edgeDistance = DistanceOf(car->basePoint, road_edge); |
| | | reportOffsetOver = false; |
| | | } |
| | | ////////////////////////////////////////////// |
| | | startPoint = car->basePoint; |
| | | edgeDistance = dis2roadEdge; |
| | | reportOffsetOver = false; |
| | | } |
| | | |
| | | if (ExitSonArea(index, RoadMapList, car)) { |
| | | if (distanceToStart > CHECK_STAGE_DISTANCE) { |
| | | DEBUG("复位边距偏移量"); |
| | | startPoint = car->basePoint; |
| | | edgeDistance = dis2roadEdge; |
| | | reportOffsetOver = false; |
| | | distanceToStartSum += distanceToStart; |
| | | distanceToStart = 0; |
| | | } |
| | | |
| | | if (distanceToStart + distanceToStartSum > 150) { |
| | | DEBUG("离开直线行驶区域"); |
| | | return -1; |
| | | } |
| | | return 1; |
| | | } |
| | | |
| | | return index; |
| | | static double CalcDistance2Edge(road_t &road, const car_model *car) |
| | | { |
| | | PointF vp; |
| | | bool get_vp = false; |
| | | double distance = 0; |
| | | |
| | | // 检测道路边缘线 |
| | | 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 (VerticalPointOnLine(car->basePoint, edge, vp)) { |
| | | get_vp = true; |
| | | goto FIND_VP_END; |
| | | } |
| | | |
| | | p1 = p2; |
| | | } |
| | | } |
| | | |
| | | FIND_VP_END: |
| | | if (get_vp) { |
| | | distance = DistanceOf(car->basePoint, vp); |
| | | } else { |
| | | // 没有找到匹配线端,按最小距离顶点计算 |
| | | distance = 100; |
| | | for (int i = 0; i < road.leftEdge.size(); ++i) { |
| | | for (int j = 0; j < road.leftEdge[i].points.size(); ++j) { |
| | | double x; |
| | | if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) { |
| | | distance = x; |
| | | } |
| | | } |
| | | } |
| | | } |
| | | |
| | | return distance; |
| | | } |