坐标

fctom1215

2020-07-27 bb7622ee9632eb25a58d4371a900aea9a26651ad

坐标

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

@@ -48,7 +48,7 @@
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const double CHANGE_LANE_RANGE = 100.0;
const double OVERTAKE_RANGE = 150.0;
const int OVERTAKE_HOLD_TIME = D_SEC(11);                // 在超车道行驶的一段时间
const int OVERTAKE_HOLD_TIME = D_SEC(3);                // 在超车道行驶的一段时间
const double EXAM_RANGE = 3000.0;                       // 至少驾驶距离

static const double LASTEST_BREAK_POINT = 30.0;
@@ -112,6 +112,8 @@
    int type;           // 实线，虚线
} CurrentLane;
static bool laneChanging;
static int changeLaneDirect;

static double odoGraph;
static struct drive_timer odoTimer;
static double odoPrevSpeed;
@@ -199,6 +201,7 @@

    CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
    laneChanging = false;
    changeLaneDirect = 0;

    nextRoadId = -1;
    checkTurn = false;
@@ -704,8 +707,15 @@
                                                        crashGreenCmpTime.min,
                                                        crashGreenCmpTime.sec,
                                                        crashGreenCmpTime.msec * 10);
                            int laneDirect = 0;

                            if (diff < CHANGE_LANE_MIN_INTERVAL) {
                            if (CurrentLane.lane > lane.lane) {
                                laneDirect = 1;
                            } else {
                                laneDirect = -1;
                            }

                            if (diff < CHANGE_LANE_MIN_INTERVAL && laneDirect == changeLaneDirect) {
                                DEBUG("===================== 连续变道 ============!!");
                                // 连续变道，不合格
                                AddExamFault(15, rtkTime);
@@ -764,16 +774,24 @@
                    if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
                        DEBUG("变更车道项目完成");
                        currRoadItem = NULL;
                        PlayTTS("完成变道");
                    } else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) {
                        if (CurrentLane.lane > lane.lane) {
                            DEBUG("超车变道完成");
                            overtake = true;
                            Rtk2DriveTimer(overTakeCmpTime, rtkTime);
                            PlayTTS("完成超车");
                        } else {
                            DEBUG("右道超车，错误");
                            AddExamFault(3, rtkTime);
                            currRoadItem = NULL;
                        }
                    }

                    if (CurrentLane.lane > lane.lane) {
                        changeLaneDirect = -1;
                    } else {
                        changeLaneDirect = 1;
                    }

                    CurrentLane = lane;
@@ -1256,448 +1274,6 @@
    }
    return false;
}

#if 0
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 起步检测
    TestRoadStartCar(car, speed, moveDirect, rtkTime);

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
            AddExamFault(10, rtkTime);
        }
    } else {
        occurOverSpeed = false;
    }

    // 副刹车检测
    if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
        // 副刹车踩下，不合格
        if (!occurSecondBreak) {
            DEBUG("副刹车动作了");
            occurSecondBreak = true;
            AddExamFault(17, rtkTime);
        }
    } else {
        occurSecondBreak = false;
    }

    // 挡位匹配检测
    bool currGearError = false;
    bool currGearNSlide = false;

    switch (ReadCarStatus(GEAR)) {
        case GEAR_N:
            if (moveDirect != 0) {
                // 空档滑行
                currGearNSlide = true;
            }
            break;
        case GEAR_1:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
                currGearError = true;
            }
            break;
        case GEAR_2:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
                currGearError = true;
            }
            break;
        case GEAR_3:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
                currGearError = true;
            }
            break;
        case GEAR_4:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
                currGearError = true;
            }
            break;
        case GEAR_5:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
                currGearError = true;
            }
            break;
        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 (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);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        // 累计15秒，挡位-车速不匹配，不合格
        DEBUG("挡位错误超过15秒");
        AddExamFault(6, rtkTime);
        gearErrorTime = 0;
    }

    prevGearError = currGearError;
    if (prevGearError) {
        Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
    }
    
    // 起步后滑
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarOnRedArea = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else if (moveDirect == -1 && prevMoveDirect == 0) {
            DEBUG("开始后滑");
            stopPoint = car->basePoint;
            occurSlide = true;
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("禁停区停车");
            reportStopCarOnRedArea = true;
        }
    } else if (moveDirect == -1) {
        if (occurSlide) {
            double slideDistance = DistanceOf(stopPoint, car->basePoint);

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            }

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
                // 后滑超过30厘米, 不合格
                AddExamFault(5, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
            }
        }
    } else {
        if (slideNormalDistance) {
            // 后滑，扣10分
            AddExamFault(18, rtkTime);
            DEBUG("后滑超过10厘米, 但不超过30厘米");
        }

        slideNormalDistance = false;
        slideLongDistance = false;
        occurSlide = false;
    }

    switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
        case LEFT_TURN_LIGHT:
            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                currTurnSignalStatus = LEFT_TURN_LIGHT;
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        case RIGHT_TURN_LIGHT:
            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                currTurnSignalStatus = RIGHT_TURN_LIGHT;
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        default:
            currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
            break;
    }

    // 检查是否持续转向
    char turnDirect = CheckCarTurn(CarModelList);
    if (turnDirect == 'L') {
//        PlayTTS("左1");
        if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打左转灯");
                // 没打左转灯，不合格
                reportTurnSignalError = true;
                ReportTurnSignalError(13, rtkTime);
            }
        } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                               turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
            if (!reportTurnSignalError) {
                DEBUG("转向灯时间不足");
                // 不足3秒，不合格
                reportTurnSignalError = true;
                ReportTurnSignalError(14, rtkTime);
            }
        }
    } else if (turnDirect == 'R') {
//        PlayTTS("右1");
        if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打右转灯");
                // 没打右转灯，不合格
                reportTurnSignalError = true;
                ReportTurnSignalError(13, rtkTime);
            }
        } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                               turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
            if (!reportTurnSignalError) {
                DEBUG("转向灯时间不足");
                // 不足3秒，不合格
                reportTurnSignalError = true;
                ReportTurnSignalError(14, rtkTime);
            }
        }
    } else {
        reportTurnSignalError = false;
    }

    // 撞红线
    if (CrashRedLine(RoadMapList, car)) {
        if (!occurCrashRedLine) {
            // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
            DEBUG("撞道路边缘线");
            AddExamFault(11, rtkTime);
            occurCrashRedLine = true;
        }
    } else {
        occurCrashRedLine = false;
    }

    // 撞绿线
    static PointF p1, p2;
    if (CrashGreenLine(RoadMapList, car, p1, p2)) {
        // 压虚线
        if (moveDirect != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                Rtk2DriveTimer(crashGreenRunTime, rtkTime);         // 运动中压虚线的开始时间点
            } else if (checkCrashGreenTimeout == 1) {
                uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                        crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);

                if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
                    DEBUG("长时间压虚线");
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶，不合格
                    AddExamFault(12, rtkTime);
                }
            }
        } else {
            // 停车就不计时了
            checkCrashGreenTimeout = 0;
        }

        // 检测当前车辆于虚线的位置，做变道检测;
        // 检测是否3秒前有开启对应之转向灯
        if (!occurCrashGreenLine) {
            occurCrashGreenLine = true;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            Rtk2DriveTimer(crashGreenStartTime, rtkTime);
            turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
        }

        // p1 ---------------> p2
        double angle = car->yaw - YawOf(p2, p1);
        if (angle < 0 || angle > 180) {
            // 右侧
            carIntersectionOfGreenLine = 'R';
        } else {
            // 左侧
            carIntersectionOfGreenLine = 'L';
        }
    } else {
        // 不再压虚线
        if (occurCrashGreenLine) {
            int inter = IntersectionOfLine(p1, p2, car->basePoint);

            // 完成跨线动作
            if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
                    (inter == -1 && carIntersectionOfGreenLine == 'L')) {
                // 比较上次跨线时间
                if (crashGreenCmpTime.hour >= 0) {
                    uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                rtkTime->mss * 10,
                                                crashGreenCmpTime.hour, crashGreenCmpTime.min,
                                                crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10);

                    if (diff < CHANGE_LANE_MIN_INTERVAL) {
                        DEBUG("===================== 连续变道 ============!!");
                        // 连续变道，不合格
                        AddExamFault(15, rtkTime);
                    }
                }

                // 记录本次变道时间点
                Rtk2DriveTimer(crashGreenCmpTime, rtkTime);

                // 检查变道前，是否提前转向灯
                if (inter == 1) {
//                    PlayTTS("左2");
                    // 向左侧变道
                    DEBUG("向左侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        ReportTurnSignalError(13, rtkTime);
                    }
                } else {
//                    PlayTTS("右2");
                    // 向右侧变道
                    DEBUG("向右侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        ReportTurnSignalError(14, rtkTime);
                    }
                }
            }
        }
        occurCrashGreenLine = false;
        checkCrashGreenTimeout = 0;
    }

    // 完成起步后，触发线检测
    if (currExamMapIndex == -1 && startCar == START_CAR_DONE) {
        currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
        if (currExamMapIndex != -1) {
            DEBUG("碰撞触发线");

            MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);

            if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP &&
                    RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
                StartThroughExam(currExamMapIndex, RoadMapList);
            } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {

            } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {

            } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {

            }
        }
    } else if (startCar == START_CAR_DONE) {
        int prevIdx = currExamMapIndex;

        if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
            currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
                                                  CarModelList, speed, moveDirect, rtkTime);
        }
        else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {

        } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {

        } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {

        }

        if (currExamMapIndex == -1) {
            DEBUG("离开区域 index %d id %d type %d", prevIdx, RoadMapList[prevIdx].id, RoadMapList[prevIdx].type);

            MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 0);
        }
    }
}


bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car)
{
    bool ret = false;

    if (index < 0 || index > RoadMapList.size())
        return true;

    if (RoadMapList[index].area.point != NULL) {
        // 需要车身全部离开
        Polygon carBody;

        carBody.num = car->bodyNum;
        carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
        for (int i = 0; i < carBody.num; ++i) {
            carBody.point[i] = car->carXY[car->body[i]];
        }

        if (IntersectionOf(&carBody, &RoadMapList[index].area) == GM_None) {
            ret = true;
        }

        free(carBody.point);
    }

    return ret;
}

bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    bool ret = false;

    if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size())
        return ret;

    Polygon trace;

    trace.num = 5;          // 最近5个轮轨迹
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);

    list<car_model *>::iterator iter = CarModelList.begin();

    int pn = 0;
    while (iter != CarModelList.end() && pn < trace.num) {
        trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        ++iter;
    }

    // 每条线都检测
    for (int j = 0; j < RoadMapList[index].redLineNum; ++j) {
        Line red_line;

        int kp = 0;

        // 触发线一般应该只有首尾2点（id, p1, p2）
        for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) {
            MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp],
                     &RoadMapList[index].redLine[j].point[k]);

            int pp = 0;
            for (int p = 1; p < pn; ++p) {
                Line trace_line;
                MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);

                if (IntersectionOf(trace_line, red_line) == GM_Intersection) {
                    // 碰到触发线
                    ret = true;
                    goto SEARCH_SON_RED_LINE_END;
                }

                pp = p;
            }

            kp = k;
        }
    }

    SEARCH_SON_RED_LINE_END:
    free(trace.point);

    return ret;
}
#endif

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{