坐标

yy1717

2020-05-15 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba

 lib/src/main/cpp/test_items2/stop_car.cpp

@@ -33,12 +33,8 @@

static void PlayTTSTimeout(union sigval sig);

void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList) {
    if (index == -1)
        return;

    DEBUG("进入靠边停车地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);

void StartStopCarExam(std::string tts) {
    DEBUG("靠边停车");
    ttsPlayEnd = 0;
    moveDistance = 0;
    prevMoveDirect = 0;
@@ -49,8 +45,8 @@
    doorNotClose = false;
    checkRoadDistance = false;

    if (!RoadMapList[index].tts.empty()) {
        examTtsSeq = PlayTTS(RoadMapList[index].tts.c_str());
    if (!tts.empty()) {
        examTtsSeq = PlayTTS(tts.c_str());
    } else {
        examTtsSeq = PlayTTS("请靠边停车");
    }
@@ -73,7 +69,7 @@
    AppTimer_delete(PlayTTSTimeout);
}

int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
@@ -114,42 +110,62 @@
        if (tp - stopTimepoint >= STOP_CAR_TIME && !checkRoadDistance) {
            // 停车超2秒，开始判断
            DEBUG("检测和路边的距离");
            PointF pc;
            PointF pc, vp;
            bool get_vp = false;
            double dis2roadEdge = 0;

            checkRoadDistance = true;

            // 前后轮的中点
            pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2;
            pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2;

            // 检测道路边缘线
            for (int i = 0; i < RoadMapList[index].roadEdgeLineNum; ++i) {
                PointF p1, p2, pv;
            for (int i = 0; i < road.rightEdge.size(); ++i) {
                PointF p1, p2;
                Line edge;

                p1 = RoadMapList[index].roadEdgeLine[i].point[0];
                for (int j = 1; j < RoadMapList[index].roadEdgeLine[i].num; ++j) {
                    p2 = RoadMapList[index].roadEdgeLine[i].point[j];
                    pv = GetVerticalPoint(p1, p2, pc);
                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 (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
                            (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
                        double dis2roadEdge = DistanceOf(pc, pv);

                        DEBUG("停车距路边距离 %f", dis2roadEdge);

                        if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
                            DEBUG("停车超出路边0.5米");
                            // 停车距离超过50厘米，不合格
                            AddExamFault(36, rtkTime);
                        } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
                            DEBUG("停车超出路边0.3米");
                            // 停车距离超过30厘米，扣10分
                            AddExamFault(37, rtkTime);
                        }
                    if (VerticalPointOnLine(pc, edge, vp)) {
                        get_vp = true;
                        goto FIND_VP_END;
                    }

                    p1 = p2;
                }
            }

FIND_VP_END:
            if (get_vp) {
                dis2roadEdge = DistanceOf(pc, vp);
            } else {
                // 没有找到匹配线端，按最小距离顶点计算
                dis2roadEdge = 100;
                for (int i = 0; i < road.rightEdge.size(); ++i) {
                    for (int j = 0; j < road.rightEdge[i].points.size(); ++j) {
                        double dis;
                        if (dis2roadEdge > (dis = DistanceOf(pc, road.rightEdge[i].points[j]))) {
                            dis2roadEdge = dis;
                        }
                    }
                }
            }

            DEBUG("停车距路边距离 %f", dis2roadEdge);

            if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
                DEBUG("停车超出路边0.5米");
                // 停车距离超过50厘米，不合格
                AddExamFault(36, rtkTime);
            } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
                DEBUG("停车超出路边0.3米");
                // 停车距离超过30厘米，扣10分
                AddExamFault(37, rtkTime);
            }
        }
    }