坐标

yy1717

2020-08-18 efc508204e781a1ce1d3f55c2b986198c9e32509

 lib/src/main/cpp/test_items2/road_exam.cpp

@@ -51,6 +51,7 @@
    int road;
    int sep;
    int no;
    int total;
    int guide;
} lane_t;

@@ -173,6 +174,8 @@
static bool AllCmp(road_exam_map &map);

static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);

void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);

void InitRoadExam(road_exam_map &RoadMap)
{
@@ -768,6 +771,7 @@
            lane.road = roadIndex;
            lane.sep = i;
            lane.no = CLP.size();
            lane.total = CLP.size() + 1;

            for (int x = 0; x < CLP.size(); ++x) {
                if (CLP[x].pos == 'L') {
@@ -779,6 +783,12 @@
            return true;
        }
    }

    lane.road = roadIndex;
    lane.sep = 0;
    lane.no = 0;
    lane.total = 0;
    lane.guide = 0;

    return false;
}
@@ -1380,6 +1390,8 @@

    oldid = Lane.guide;
    DetectLane(RoadMap, car, currExamMapIndex, rtkTime);

//    DEBUG("Lane信息 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
    if (Lane.guide > 0 && currExamMapIndex >= 0) {
        int stop_line;

@@ -1394,6 +1406,10 @@

    if (Lane.guide != oldid) {
        DEBUG("导向类型切换 %d", Lane.guide);
    }

    if (currExamMapIndex >= 0) {
        AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
    }

    // 检测压线状态
@@ -1792,3 +1808,22 @@
    }
    return true;
}

void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
{
    double distance = 0;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;

    for (int i = 0; i < RoadMap.roads[roadIndex].rightEdge.size(); ++i) {
        for (int j = 1; j < RoadMap.roads[roadIndex].rightEdge[i].points.size(); ++j) {
            PointF point = RoadMap.roads[roadIndex].rightEdge[i].points[j];
            distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
            if (distance > 1e-3) {
                DEBUG("直道剩余距离 %f", distance);
                return;
            }
        }
    }
}