坐标

yy1717

2020-11-24 6e0f29b08a040d14576d7053c1206a8439936570

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

@@ -106,6 +106,12 @@
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
@@ -134,7 +140,7 @@
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);
@@ -142,7 +148,7 @@

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);

@@ -187,6 +193,8 @@
    RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;

    RoadExamStatus = ROAD_EXAM_READY_NEXT;

    TriggerDetect.clear();

    win = false;
}
@@ -1276,7 +1284,7 @@
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);
@@ -1436,7 +1444,7 @@
            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);
@@ -1491,6 +1499,34 @@
        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);

@@ -1554,7 +1590,7 @@
        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);
    }
}

@@ -1621,6 +1657,7 @@
        }
    } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
        if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
            DEBUG("寻找下一个子项目");
            RoadExamStatus = ROAD_EXAM_READY_NEXT;
        }
    } else {
@@ -1649,6 +1686,7 @@
        }

        if (!testing) {
            DEBUG("当前子项结束");
            RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
            if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
                freeRunExceptDistance = 60.0;
@@ -1665,9 +1703,13 @@
    }
}

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();
@@ -1919,13 +1961,35 @@
    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]);
@@ -1937,7 +2001,7 @@
                return RoadMap.triggerLines[i].active;
            }*/

            vector<double> vec;
           /* vector<double> vec;

            PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                   car->carXY[car->axial[AXIAL_FRONT]]);
@@ -1954,6 +2018,87 @@
            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));
            }
        }
    }