加入地图标定模式

yy1717

2020-09-16 611f51ed073de48e83f46d47c82cd5ebe61258d7

加入地图标定模式

 lib/src/main/cpp/driver_test.cpp

@@ -54,7 +54,8 @@
enum {
    TEST_TYPE_AREA = 2,
    TEST_TYPE_ROAD_DUMMY_LIGHT,
    TEST_TYPE_ROAD_TRUE_LIGHT
    TEST_TYPE_ROAD_TRUE_LIGHT,
    TEST_TYPE_ROAD_CALIBRATE
};

static bool ExamStart = false;
@@ -109,7 +110,8 @@

    RoadMap.roads.clear();
    RoadMap.specialAreas.clear();
    RoadMap.triggerLines.clear();
    RoadMap.forbidLines.clear();
    RoadMap.examScheme.clear();

    CarSensorInit();

@@ -192,18 +194,22 @@

    vector<road_t>().swap(RoadMap.roads);
    vector<special_area_t>().swap(RoadMap.specialAreas);
    vector<trigger_line_t>().swap(RoadMap.triggerLines);
    vector<forbid_line_t>().swap(RoadMap.forbidLines);
    vector<scheme_t>().swap(RoadMap.examScheme);
}

void SetRoadMap(road_exam_map &map)
void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme)
{
    if (ExamStart) return;

    RoadMap.roads.assign(map.roads.begin(), map.roads.end());
    RoadMap.specialAreas.assign(map.specialAreas.begin(), map.specialAreas.end());
    RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
//    RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
    RoadMap.forbidLines.assign(map.forbidLines.begin(), map.forbidLines.end());
    DEBUG("得到新的路考地图 路数量 %d 特殊区域数量 %d 触发线数量 %d 其他禁止线数量 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.triggerLines.size(), RoadMap.forbidLines.size());

    RoadMap.examScheme.assign(scheme.begin(), scheme.end());

    DEBUG("得到新的路考地图 路数量 %d 特殊区域数量 %d 其他禁止线数量 %d 项目数量 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.forbidLines.size(), RoadMap.examScheme.size());
}

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -402,12 +408,18 @@

            if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                exam_dummy_light = 0;           //0
                RoadMap.calibrate = 0;
            }
            if (type == TEST_TYPE_ROAD_TRUE_LIGHT) {
                RoadMap.calibrate = 0;
                InitRoadExam(RoadMap);
            }
            if (type == TEST_TYPE_AREA) {
                InitAreaExam();
            }
            if (type == TEST_TYPE_ROAD_CALIBRATE) {
                RoadMap.calibrate = 1;
                InitRoadExam(RoadMap);
            }
        }
        MA_SendExamStatus(1, 0);
@@ -865,6 +877,27 @@
    return true;
}

void RoadChange(int road, int status)
{
    struct roadStatusBrief brief;

    brief.road_id = road;
    brief.status = status;

    MA_SendRoadStatus(&brief);
}

void CrossingChange(int road, int crossing, int status)
{
    struct crossingStatusBrief brief;

    brief.road_id = road;
    brief.crossing_index = crossing;
    brief.status = status;

    MA_SendCrossingStatus(&brief);
}

void AddExamFault(int wrong, const struct RtkTime *rtkTime)
{
    struct ExamFault fault;

 lib/src/main/cpp/driver_test.h

@@ -137,16 +137,14 @@
} separate_t;

typedef struct {
    int active;
    string tts;
    bool stopFlag;
    Line line;
    bool centrePointValid;
    PointF centrePoint;
} stop_line_t;

typedef struct {
    int id;
    Line startLine;
//    Line stopLine;
//    int active;         // 到达路口尾部的行进方向
    bool activeBreak;   // 路口刹车减速
@@ -175,15 +173,9 @@
} special_area_t;

typedef struct {
    int id;
    int road;
    int active;
    int time;                   // 项目最大完成时间
    int distance;               // 项目最大完成距离
    string tts;
    bool cmp;
    std::vector<PointF> points;
    std::vector<PointF> leftPoints; // 对应到道路左侧的点
} trigger_line_t;

typedef struct {
@@ -192,11 +184,25 @@
    std::vector<PointF> points;
} forbid_line_t;

typedef struct {
    int road_id;
    int index;
    int active;
} crossing_active_t;

typedef struct {
    std::string name;
    std::vector<crossing_active_t> crossingActive;
    std::vector<trigger_line_t> triggerLines;
} scheme_t;

struct road_exam_map {
    int calibrate;
    std::vector<road_t> roads;
    std::vector<special_area_t> specialAreas;
    std::vector<trigger_line_t> triggerLines;
//    std::vector<trigger_line_t> triggerLines;
    std::vector<forbid_line_t> forbidLines;
    std::vector<scheme_t> examScheme;
};

struct area_exam_map {
@@ -218,7 +224,7 @@


void CleanRoadMap(void);
void SetRoadMap(road_exam_map &map);
void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme);

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
@@ -228,6 +234,8 @@

void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
void RoadChange(int road, int status);
void CrossingChange(int road, int crossing, int status);

void SystemShutdown(int event, int timeout);


 lib/src/main/cpp/master/comm_if.cpp

@@ -64,6 +64,9 @@
#define ID_SM_DISTANCE              0x0020
#define ID_SM_CARSENSOR              0x0013

#define ID_MS_ROAD_BRIEF        0x0015              // 科目三项目标定，进出某条路的提示
#define ID_MS_CROSSING_BRIEF    0x0016              // 科目三项目标定，接近/驶离某各路口的提示

#define MA_OUT_GPS_BRIEF        0x0001
#define MA_OUT_RTK_BRIEF        0x0002
#define MA_OUT_CAR_BRIEF        0x0004
@@ -438,6 +441,38 @@
    writer.EndObject();

    SendMsgToMainProcIndep(ID_SM_DISTANCE, sb.GetString());
}

void MA_SendRoadStatus(const struct roadStatusBrief *brief)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartObject();
    writer.Key("road_id");
    writer.Int(brief->road_id);
    writer.Key("status");
    writer.Int(brief->status);
    writer.EndObject();

    SendMsgToMainProcIndep(ID_MS_ROAD_BRIEF, sb.GetString());
}

void MA_SendCrossingStatus(const struct crossingStatusBrief *brief)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartObject();
    writer.Key("road_id");
    writer.Int(brief->road_id);
    writer.Key("crossing_index");
    writer.Int(brief->crossing_index);
    writer.Key("status");
    writer.Int(brief->status);
    writer.EndObject();

    SendMsgToMainProcIndep(ID_MS_CROSSING_BRIEF, sb.GetString());
}

void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
@@ -844,10 +879,6 @@

                road_exam_map map;

                map.roads.clear();
                map.specialAreas.clear();
                map.triggerLines.clear();

                if (doc.HasMember("road")) {
                    const Value &a = doc["road"];

@@ -860,23 +891,6 @@
                                DEBUG("路id %d", s.GetInt());
                                road.id = s.GetInt();
                            }
                            if (itr->HasMember("start_line")) {
                                const Value &a2 = (*itr)["start_line"];

                                PointF p1, p2;
                                int n = 0;

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2, ++n) {
                                    DEBUG("起始线 %d", (*itr2).GetInt());
                                    if (n == 0) {
                                        p1 = mapPoints[(*itr2).GetInt()];
                                    } else if (n == 1) {
                                        p2 = mapPoints[(*itr2).GetInt()];
                                    }
                                }

                                MakeLine(&road.startLine, &p1, &p2);
                            }
                            if (itr->HasMember("crossing")) {
                                const Value &a2 = (*itr)["crossing"];

@@ -887,20 +901,12 @@
                                    if (!itr2->IsObject()) {
                                        break;
                                    }
                                    if (itr2->HasMember("active")) {
                                        const Value &s = (*itr2)["active"];
                                        temp.active = s.GetInt();
                                        DEBUG("路口动作 %d", temp.active);
                                    }
                                    if (itr2->HasMember("stop_flag")) {
                                        const Value &s = (*itr2)["stop_flag"];
                                        temp.stopFlag = s.GetInt();
                                        DEBUG("路口停车 %d", temp.stopFlag);
                                    }
                                    if (itr2->HasMember("tts")) {
                                        const Value &s = (*itr2)["tts"];
                                        temp.tts = s.GetString();
                                        DEBUG("路口提示 %s", temp.tts.c_str());
                                    } else {
                                        temp.stopFlag = false;
                                    }
                                    if (itr2->HasMember("line")) {
                                        const Value &s = (*itr2)["line"];
@@ -920,8 +926,10 @@
                                    }
                                    if (itr2->HasMember("center_point")) {
                                        const Value &s = (*itr2)["center_point"];

                                        temp.centrePoint = mapPoints[s.GetInt()];
                                        temp.centrePointValid = true;
                                    } else {
                                        temp.centrePointValid = false;
                                    }
                                    crossing.push_back(temp);
                                }
@@ -1092,7 +1100,7 @@
                        map.specialAreas.push_back(specialArea);
                    }
                }
                if (doc.HasMember("trigger_line")) {
                /*if (doc.HasMember("trigger_line")) {
                    const Value &a = doc["trigger_line"];

                    for (Value::ConstValueIterator itr = a.Begin();
@@ -1139,7 +1147,7 @@

                        map.triggerLines.push_back(trigger);
                    }
                }
                }*/
                if (doc.HasMember("red_line")) {
                    const Value &a = doc["red_line"];

@@ -1169,10 +1177,86 @@
                    }
                }

                vector<scheme_t> schemes;

                if (doc.HasMember("scheme")) {
                    const Value &a = doc["scheme"];

                    for (Value::ConstValueIterator itr = a.Begin();
                         itr != a.End(); ++itr) {
                        scheme_t scheme;
                        if (itr->HasMember("name")) {
                            const Value &s = (*itr)["name"];
                            scheme.name = s.GetString();
                        }
                        if (itr->HasMember("crossing_active")) {
                            const Value &a2 = (*itr)["crossing_active"];

                            for (Value::ConstValueIterator itr2 = a2.Begin();
                                 itr2 != a2.End(); ++itr2) {
                                crossing_active_t act;

                                if (itr2->HasMember("road")) {
                                    const Value &s = (*itr2)["road"];
                                    act.road_id = s.GetInt();
                                }
                                if (itr2->HasMember("idx")) {
                                    const Value &s = (*itr2)["idx"];
                                    act.index = s.GetInt();
                                }
                                if (itr2->HasMember("active")) {
                                    const Value &s = (*itr2)["active"];
                                    act.active = s.GetInt();
                                }

                                scheme.crossingActive.push_back(act);
                            }
                        }
                        if (itr->HasMember("trigger_line")) {
                            const Value &a2 = (*itr)["trigger_line"];

                            for (Value::ConstValueIterator itr2 = a2.Begin();
                                 itr2 != a2.End(); ++itr2) {
                                trigger_line_t ins;

                                if (itr2->HasMember("x_y")) {
                                    const Value &a3 = (*itr2)["x_y"];
                                    if (a3.IsArray()) {
                                        PointF point;
                                        int n = 0;
                                        for (Value::ConstValueIterator itr3 = a3.Begin();
                                             itr3 != a3.End(); ++itr3) {
                                            if (n == 0) {
                                                point.X = (*itr3).GetDouble();
                                                n = 1;
                                            } else if (n == 1) {
                                                point.Y = (*itr3).GetDouble();
                                                ins.points.push_back(point);
                                                n = 0;
                                            }
                                        }
                                    }
                                }
                                if (itr2->HasMember("road")) {
                                    const Value &s = (*itr2)["road"];
                                    ins.road = s.GetInt();
                                }
                                if (itr2->HasMember("type")) {
                                    const Value &s = (*itr2)["type"];
                                    ins.active = s.GetInt();
                                }

                                scheme.triggerLines.push_back(ins);
                            }
                        }
                        schemes.push_back(scheme);
                    }
                }

                DEBUG("地图解析完毕");

                CleanRoadMap();
                SetRoadMap(map);
                SetRoadMap(map, schemes);
            } else {
                DEBUG("############## 地图解析出错###################");
            }

 lib/src/main/cpp/master/comm_if.h

@@ -104,6 +104,17 @@
    int pointNum;
};

struct roadStatusBrief {
    int road_id;
    int status;
};

struct crossingStatusBrief {
    int road_id;
    int crossing_index;
    int status;
};

void MA_MainProcMsgEntry(int cmd, const char *value);
void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length);

@@ -131,4 +142,7 @@

void MA_SendDistance(double l, double r);

void MA_SendRoadStatus(const struct roadStatusBrief *brief);
void MA_SendCrossingStatus(const struct crossingStatusBrief *brief);

#endif //MYAPPLICATION2_COMM_IF_H

 lib/src/main/cpp/native-lib.cpp

@@ -27,8 +27,8 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};

const char *VIRTUAL_RTK_IP = "192.168.1.7";
const int VIRTUAL_RTK_PORT = 9001;
const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9002;

static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;


 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -214,7 +214,7 @@
        }*/

        if (RxBufLen > 0) {
#if 1
#if 0
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));

 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,6 +1296,7 @@
        turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
    }

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

@@ -1292,31 +1336,37 @@
            }
            break;
        case GEAR_1:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][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]) {
                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]) {
                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]) {
                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]) {
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
                currGearError = true;
            }
            break;
        default:break;
            default:
                break;
    }
    // 空档滑行超时
    if (currGearNSlide) {
@@ -1325,7 +1375,8 @@
            GearNSlideStatus = 1;
            gearNSlideTimePoint = *rtkTime;
        }
        if (GearNSlideStatus == 1 && TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
            if (GearNSlideStatus == 1 &&
                TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
            // 空档滑行超5秒，不合格
            DEBUG("挡位滑行，超过5秒");
            AddExamFault(8, rtkTime);
@@ -1338,7 +1389,8 @@
    
    // 挡位不匹配超时
    if (currGearError && prevGearError) {
        DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
            DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
                  ReadCarStatus(GEAR), ConvertMs2KMh(speed));
        gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
@@ -1359,7 +1411,8 @@
            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);
                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分
@@ -1378,7 +1431,8 @@
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        // 持续停车
        if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea && StopOnRedArea(RoadMap, car)) {
            if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea &&
                StopOnRedArea(RoadMap, car)) {
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("禁停区停车");
@@ -1405,6 +1459,7 @@
    } else {
        // 前进
    }
    }

    // 检测离开此路段，全车需不在范围内
    int oldid = currExamMapIndex;
@@ -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,6 +1490,7 @@
    }
    ExitTarget(RoadMap, car, CarModelList, rtkTime);

    if (RoadMap.calibrate == 0) {
    oldid = CrashLineType;
    // 检测压线状态
    DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
@@ -1434,18 +1499,19 @@
        DEBUG("压线类型切换 %d", CrashLineType);
    }


    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);
            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);
                    DEBUG("不按规定车道标向行驶 %d: %d  期望 = %d guide = %d", currExamMapIndex,
                          stop_line_index, act, Lane.guide);
                AddExamFault(9, rtkTime);
                SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
            }
@@ -1455,12 +1521,13 @@
    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 @@
    }

    // 额外的转向检测

    if (RoadMap.calibrate == 0) {
    DetectTurn(car, moveDirect, rtkTime);

    ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
        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;
            }
        }
    }

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

@@ -29,7 +29,8 @@

car_sensor_value_t ReadCarSensorValue(int name);

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);
int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex);

void InitRoadExam(road_exam_map &RoadMap);
void TerminateRoadExam(void);

 lib/src/main/cpp/test_items2/through_something.cpp

@@ -104,6 +104,10 @@
            RemoveTargetReduceRec(TargetReduceRec, key);
        }
    }

    if (RoadMap.calibrate == 1)
        goto GET_DISTANCE_OF_NEARBY_TARGET;

    // 人行道、公交站、学校
    for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
        if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
@@ -165,6 +169,7 @@
        }
    }

    GET_DISTANCE_OF_NEARBY_TARGET:
    if (nearbyTarget.size() > 0) {
        sort(nearbyTarget.begin(), nearbyTarget.end());
        return nearbyTarget[0];
@@ -182,18 +187,19 @@
            if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
                DEBUG("越过路口 road %d 路口 %d", road, x);

                CrossRoadCallback(road, x, RoadMap.roads[road].stopLine[x].active, car);
                CrossRoadCallback(RoadMap, road, x, ExamSchemeCrossing(RoadMap, road, x), car);

                if (RoadMap.calibrate == 0) {
                if (!(it->second & REDUCE_SPEED)) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");

                    if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
                        if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
                        AddExamFault(41, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                               ROAD_ACTIVE_TURN_LEFT) {
                        AddExamFault(43, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                               ROAD_ACTIVE_TURN_RIGHT) {
                        AddExamFault(46, rtkTime);
                    }
@@ -201,22 +207,25 @@
                if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
                    // 不停车瞭望，不合格
                    DEBUG("不停车瞭望");
                    if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
                        if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
                        AddExamFault(42, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                               ROAD_ACTIVE_TURN_LEFT) {
                        AddExamFault(44, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                               ROAD_ACTIVE_TURN_RIGHT) {
                        AddExamFault(47, rtkTime);
                    }
                }

                }
                RemoveTargetReduceRec(TargetReduceRec, it->first);
                goto RECHECK;
            }
    }

    if (RoadMap.calibrate == 1)
        return;

    RECHECK2:
    for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
            int x = it->first;