坐标

yy1717

2020-05-15 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba

 lib/src/main/cpp/test_items2/drive_straight.cpp

@@ -15,88 +15,117 @@
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;
}