加入地图标定模式

yy1717

2020-09-16 611f51ed073de48e83f46d47c82cd5ebe61258d7

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

@@ -155,7 +155,7 @@
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);

static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
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);

@@ -1109,7 +1109,7 @@
    int stop_line_index;
    PointF nearbyStopPoint;

    NearbyCrossingGuide(stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
    NearbyCrossingGuide(RoadMap, stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
    nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
    nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;

@@ -1159,6 +1159,20 @@
    return rec;
}

int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex)
{
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) {
        return -1;
    }
    for (int i = 0; i < RoadMap.examScheme[0].crossingActive.size(); ++i) {
        if (RoadMap.examScheme[0].crossingActive[i].road_id == RoadMap.roads[roadIndex].id &&
                RoadMap.examScheme[0].crossingActive[i].index == crossIndex) {
            return RoadMap.examScheme[0].crossingActive[i].active;
        }
    }
    return -1;
}

static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;
@@ -1166,18 +1180,47 @@
    if (distance > 5 && distance < 70) {
        // 提示路口怎么走
        if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
            if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
                DEBUG("路口提示 %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
                PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str(), NULL);
            // 项目标定模式下，这里提示标定本路口动作
            if (RoadMap.calibrate) {
                CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 1);
            } else {
                int act = ExamSchemeCrossing(RoadMap, roadIndex, stopIndex);

                switch (act) {
                    case ROAD_ACTIVE_FORWARD:
                        DEBUG("路口提示 直行");
                        PlayTTS("前方路口请直行", NULL);
                        break;
                    case ROAD_ACTIVE_TURN_LEFT:
                        DEBUG("路口提示 左转");
                        PlayTTS("前方路口请左转", NULL);
                        break;
                    case ROAD_ACTIVE_TURN_RIGHT:
                        DEBUG("路口提示 右转");
                        PlayTTS("前方路口请右转", NULL);
                        break;
                    case ROAD_ACTIVE_TURN_BACKWARD:
                        DEBUG("路口提示 掉头");
                        PlayTTS("前方路口请掉头", NULL);
                        break;
                    default:
                        DEBUG("路口提示 未配置");
                        PlayTTS("前方路口听教练指令", NULL);
                        break;
                }
            }
            ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
        }
    } else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
        ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);

        if (RoadMap.calibrate) {
            CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 0);
        }
    }
}

static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
{
    int guide = 0, stopLine = 0;
    double distance;
@@ -1203,7 +1246,7 @@
            struct nearby_crossing temp;

            temp.stopLine = i;
            temp.guide = road.stopLine[i].active;
            temp.guide = ExamSchemeCrossing(RoadMap, roadIndex, i);
            temp.distance = distance;

            set.push_back(temp);
@@ -1253,157 +1296,169 @@
        turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
    }

    // 刹车提示音
    RingBreak();
    if (RoadMap.calibrate == 0) {
        // 刹车提示音
        RingBreak();

    // 超速检测
    if (speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
            DEBUG("超速 %f", ConvertMs2KMh(speed));
            AddExamFault(10, rtkTime);
        // 超速检测
        if (speed > MAX_SPEED) {
            if (!occurOverSpeed) {
                occurOverSpeed = true;
                // 超速，不合格
                DEBUG("超速 %f", ConvertMs2KMh(speed));
                AddExamFault(10, rtkTime);
            }
        } else if (speed < DEC_MAX_SPEED) {
            occurOverSpeed = false;
        }
    } else if (speed < DEC_MAX_SPEED ) {
        occurOverSpeed = false;
    }

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

    // 挡位匹配检测
    bool currGearError = false;
    bool currGearNSlide = 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) {
        if (GearNSlideStatus == 0) {
            DEBUG("检测到空挡滑行");
            GearNSlideStatus = 1;
            gearNSlideTimePoint = *rtkTime;
        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 (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 (currGearError && prevGearError) {
        DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
        gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        // 累计15秒，挡位-车速不匹配，不合格
        DEBUG("挡位错误超过15秒");
        AddExamFault(6, rtkTime);
        gearErrorTime = 0;
    }

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

    // 起步后滑
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = *rtkTime;
            StopCarOnRedArea = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

            if (slideNormalDistance) {
                // 后滑，扣10分
                AddExamFault(18, rtkTime);
                DEBUG("后滑超过10厘米, 但不超过30厘米");
        // 空档滑行超时
        if (currGearNSlide) {
            if (GearNSlideStatus == 0) {
                DEBUG("检测到空挡滑行");
                GearNSlideStatus = 1;
                gearNSlideTimePoint = *rtkTime;
            }

            slideNormalDistance = false;
            slideLongDistance = false;
            occurSlide = false;
        } else if (moveDirect == -1 && prevMoveDirect == 0) {
            DEBUG("开始后滑");
            stopPoint = car->basePoint;
            occurSlide = true;
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        // 持续停车
        if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea && StopOnRedArea(RoadMap, car)) {
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("禁停区停车");
            StopCarOnRedArea = true;
        }
    } else if (moveDirect == -1) {
        // 持续后滑
        if (occurSlide) {
            double slideDistance = DistanceOf(stopPoint, car->basePoint);

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            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 (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
                // 后滑超过30厘米, 不合格
                AddExamFault(5, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
        // 挡位不匹配超时
        if (currGearError && prevGearError) {
            DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
                  ReadCarStatus(GEAR), ConvertMs2KMh(speed));
            gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
        }
        if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
            // 累计15秒，挡位-车速不匹配，不合格
            DEBUG("挡位错误超过15秒");
            AddExamFault(6, rtkTime);
            gearErrorTime = 0;
        }

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

        // 起步后滑
        if (moveDirect != prevMoveDirect) {
            if (moveDirect == 0) {
                stopTimepoint = *rtkTime;
                StopCarOnRedArea = false;

                DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD,
                      rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

                if (slideNormalDistance) {
                    // 后滑，扣10分
                    AddExamFault(18, rtkTime);
                    DEBUG("后滑超过10厘米, 但不超过30厘米");
                }

                slideNormalDistance = false;
                slideLongDistance = false;
                occurSlide = false;
            } else if (moveDirect == -1 && prevMoveDirect == 0) {
                DEBUG("开始后滑");
                stopPoint = car->basePoint;
                occurSlide = true;
            }
            prevMoveDirect = moveDirect;
        } else if (moveDirect == 0) {
            // 持续停车
            if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea &&
                StopOnRedArea(RoadMap, car)) {
                // 停车超2秒，停在红区，不合格
                AddExamFault(16, rtkTime);
                DEBUG("禁停区停车");
                StopCarOnRedArea = 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;
                    slideNormalDistance = false;
                    occurSlide = false;
                }
            }
        } else {
            // 前进
        }
    } else {
        // 前进
    }

    // 检测离开此路段，全车需不在范围内
@@ -1415,6 +1470,15 @@
        if (oldid >= 0) {
            ResetCrossingStatus(oldid);
            ResetErrorLaneRpt(oldid);

        }

        if (RoadMap.calibrate) {
            if (currExamMapIndex < 0 && oldid >= 0) {
                RoadChange(oldid, 0);
            } else if (currExamMapIndex >= 0) {
                RoadChange(currExamMapIndex, 1);
            }
        }
        Lane.guide = 0;
    }
@@ -1426,41 +1490,44 @@
    }
    ExitTarget(RoadMap, car, CarModelList, rtkTime);

    oldid = CrashLineType;
    // 检测压线状态
    DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
    if (RoadMap.calibrate == 0) {
        oldid = CrashLineType;
        // 检测压线状态
        DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);

    if (oldid != CrashLineType) {
        DEBUG("压线类型切换 %d", CrashLineType);
    }
        if (oldid != CrashLineType) {
            DEBUG("压线类型切换 %d", CrashLineType);
        }


    oldid = Lane.guide;
    DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
        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_index;
        int act = NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car);
        if (Lane.guide > 0 && currExamMapIndex >= 0) {
            int stop_line_index;
            int act = NearbyCrossingGuide(RoadMap, stop_line_index, currExamMapIndex,
                                          RoadMap.roads[currExamMapIndex], car);

        if (act != 0 && !(act & Lane.guide)) {
            if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                DEBUG("不按规定车道标向行驶 %d: %d  期望 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide);
                AddExamFault(9, rtkTime);
                SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
            if (act != 0 && !(act & Lane.guide)) {
                if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                    DEBUG("不按规定车道标向行驶 %d: %d  期望 = %d guide = %d", currExamMapIndex,
                          stop_line_index, act, Lane.guide);
                    AddExamFault(9, rtkTime);
                    SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
                }
            }
        }
    }

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

    if (currExamMapIndex >= 0 && Lane.guide == 0) {
        BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);

        road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);

        // 提示路口怎么走
        HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);

        double freeSepDis = SeparateLength(RoadMap, Lane, car);
@@ -1476,10 +1543,15 @@
    }

    // 额外的转向检测
    DetectTurn(car, moveDirect, rtkTime);

    ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
    ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
    if (RoadMap.calibrate == 0) {
        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)
@@ -1605,8 +1677,11 @@
    }
}

void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car)
{
    if (RoadMap.calibrate) {
        CrossingChange(RoadMap.roads[road].id, stop_line, 0);
    }
    SetErrorLaneRpt(road, stop_line, false);
    if (active != ROAD_ACTIVE_FORWARD) {
        ResetTurnDetect(car);
@@ -1823,12 +1898,12 @@

static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    if (index < 0 || index >= RoadMap.roads.size())
    if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0)
        return -1;

    for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
        if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
            Line triggerLine;
    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]);

@@ -1837,6 +1912,25 @@
            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].active;
            }*/

            vector<double> vec;

            PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                   car->carXY[car->axial[AXIAL_FRONT]]);
            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                   RoadMap.examScheme[0].triggerLines[j].points[0]);

            vec.push_back(DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
                                     RoadMap.examScheme[0].triggerLines[j].points[0]));
            vec.push_back(DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]));
            vec.push_back(DistanceOf(p1, car->carXY[car->axial[AXIAL_FRONT]]));

            sort(vec.begin(), vec.end());

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