坐标

fctom1215

2020-08-28 cff87234c496f0a4522cd2ac57941f14c729b6ad

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

@@ -83,7 +83,7 @@
static bool StopCarOnRedArea;
static PointF stopPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;
static int GearNSlideStatus = 0;

static bool slideLongDistance;
static bool slideNormalDistance;
@@ -91,11 +91,10 @@

static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;

static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct RtkTime gearErrorTimePoint;
static struct RtkTime gearNSlideTimePoint;

static int gearErrorTime;
static int gearNSlideTime;

static int currExamMapIndex;

@@ -147,6 +146,7 @@
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

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 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);
@@ -154,7 +154,7 @@

static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);

static trigger_line_t * 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);

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

@@ -176,8 +176,7 @@
    slideNormalDistance = false;
    prevGearError = false;
    gearErrorTime = 0;
    prevGearNSlide = false;
    gearNSlideTime = 0;
    GearNSlideStatus = 0;

    currExamMapIndex = -1;

@@ -880,6 +879,8 @@
            checkCrashGreenTimeout = 0;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            crashGreenStartTime = *rtkTime;

            DEBUG("开始压虚线 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
        }
        crashDottedLine = true;
    } else {
@@ -939,8 +940,11 @@
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足");
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                              crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
                              lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);

                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
@@ -949,8 +953,10 @@
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足");
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                              crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
                              lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
@@ -1298,27 +1304,26 @@
        default:break;
    }
    // 空档滑行超时
    if (currGearNSlide && prevGearNSlide) {
        gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                      gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
    if (currGearNSlide) {
        if (GearNSlideStatus == 0) {
            DEBUG("检测到空挡滑行");
            GearNSlideStatus = 1;
            gearNSlideTimePoint = *rtkTime;
        }
        if (GearNSlideStatus == 1 && TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
            // 空档滑行超5秒，不合格
            DEBUG("挡位滑行，超过5秒");
            AddExamFault(8, rtkTime);
            GearNSlideStatus = 2;
        }
    } else if (GearNSlideStatus != 0) {
        GearNSlideStatus = 0;
        DEBUG("空挡滑行结束");
    }
    if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
        // 空档滑行超5秒，不合格
        DEBUG("挡位滑行，超过5秒");
        AddExamFault(8, rtkTime);
        gearNSlideTime = 0;
    }

    prevGearNSlide = currGearNSlide;
    if (prevGearNSlide) {
        Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
    } else {
        gearNSlideTime = 0;
    }
    
    // 挡位不匹配超时
    if (currGearError && prevGearError) {
        gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                     gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
        gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        // 累计15秒，挡位-车速不匹配，不合格
@@ -1329,7 +1334,7 @@

    prevGearError = currGearError;
    if (prevGearError) {
        Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
        gearErrorTimePoint = *rtkTime;
    }

    // 起步后滑
@@ -1395,6 +1400,7 @@
            ResetCrossingStatus(oldid);
            ResetErrorLaneRpt(oldid);
        }
        Lane.guide = 0;
    }

    if (currExamMapIndex >= 0) {
@@ -1422,7 +1428,7 @@

        if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
            if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                DEBUG("不按规定车道标向行驶");
                DEBUG("不按规定车道标向行驶 %d: %d  guide = %d", currExamMapIndex, stop_line_index, Lane.guide);
                AddExamFault(9, rtkTime);
                SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
            }
@@ -1456,6 +1462,7 @@
    DetectTurn(car, moveDirect, rtkTime);

    ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
    ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
}

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)
@@ -1474,41 +1481,41 @@

            if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
                if (straight > 170 && road_end > 170) {
                    StartDriveStraightExam();
                    RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
                    return;
                }
//                if (straight > 170 && road_end > 170) {
//                    StartDriveStraightExam();
//                    RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
//                    return;
//                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
                if (road_end > 170) {
                    StartOperateGearExam();
                    RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
                    return;
                }
//                if (road_end > 170) {
//                    StartOperateGearExam();
//                    RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
//                    return;
//                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
                if (road_end > 150 && Lane.total > 1) {
                    StartChaneLaneExam(Lane.no);
                    RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
                    return;
                }
//                if (road_end > 150 && Lane.total > 1) {
//                    StartChaneLaneExam(Lane.no);
//                    RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
//                    return;
//                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
                if (road_end > 200 && Lane.total > 1) {
                    if (Lane.no == 0) {
                        // 已在最左车道，先变更车道？
                        StartChaneLaneExam(Lane.no);
                        RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
                    } else {
                        StartOvertakeExam(Lane.no);
                        RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
                    }
                    return;
                }
//                if (road_end > 200 && Lane.total > 1) {
//                    if (Lane.no == 0) {
//                        // 已在最左车道，先变更车道？
//                        StartChaneLaneExam(Lane.no);
//                        RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
//                    } else {
//                        StartOvertakeExam(Lane.no);
//                        RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
//                    }
//                    return;
//                }
            }

            if (!not_complete) {
@@ -1522,7 +1529,6 @@
    } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
        if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
            RoadExamStatus = ROAD_EXAM_READY_NEXT;

        }
    } else {
        bool testing = false;
@@ -1554,7 +1560,7 @@
            if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
                freeRunExceptDistance = 60.0;
            } else {
                freeRunExceptDistance = 250.0;
                freeRunExceptDistance = 2;//250.0;
            }

            RoadExamStatus = ROAD_EXAM_FREE_RUN;
@@ -1563,12 +1569,23 @@
    }
}

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    tm.hour = rtkTime->hh;
    tm.min = rtkTime->mm;
    tm.sec = rtkTime->ss;
    tm.msec = rtkTime->mss;
    int item = EntryItem(roadIndex, RoadMap, car, CarModelList);

    if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
        StartDriveStraightExam();
        RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
    } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 4) {
        StartOperateGearExam();
        RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
    } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 1) {
        StartChaneLaneExam(Lane.no);
        RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
    } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 2) {
        StartOvertakeExam(Lane.no);
        RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
    }
}

void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
@@ -1666,7 +1683,7 @@
                            DEBUG("变调未打灯!!");
                            // 没打灯，不合格
                            AddExamFault(13, rtkTime);
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
@@ -1676,7 +1693,7 @@
                            DEBUG("变调未打灯!!");
                            // 没打灯，不合格
                            AddExamFault(13, rtkTime);
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
@@ -1784,35 +1801,27 @@
    return false;
}

static trigger_line_t * 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)
{
    if (index < 0 || index >= RoadMap.roads.size())
        return -1;

    for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
        if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
            Line triggerLine;

            if (RoadMap.triggerLines[i].leftPoints.size() != RoadMap.triggerLines[i].points.size()) {
                RoadMap.triggerLines[i].leftPoints.clear();
            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);

                for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
                    RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
                }

                for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
                    DEBUG("触发线补齐 road %d id %d type %d (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.roads[index].id, RoadMap.triggerLines[i].id, RoadMap.triggerLines[i].active,
                          RoadMap.triggerLines[i].points[0].X, RoadMap.triggerLines[i].points[0].Y, RoadMap.triggerLines[i].leftPoints[0].X, RoadMap.triggerLines[i].leftPoints[0].Y);
                }
            }

            MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &RoadMap.triggerLines[i].leftPoints[0]);
            MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &p2);

            if (CrashTheLine(triggerLine, car, CarModelList)) {
                DEBUG("触发项目 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2);
                return &RoadMap.triggerLines[i];
                return RoadMap.triggerLines[i].active;
            }
        }
    }

    return NULL;
    return -1;
}

static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)