坐标

yy1717

2020-08-13 372659e4d884f390f2f5031c55ba7b3fa9fd5912

坐标

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

@@ -808,7 +808,7 @@

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

                                vector<stop_line_t> crossing;
@@ -916,13 +916,14 @@
                            }
                            if (itr->HasMember("separate")) {
                                const Value &a2 = (*itr)["separate"];
                                DEBUG("段数量 %d", a2.Size());
                                DEBUG("分道数量 %d", a2.Size());

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
                                    separate_t sep;
                                    if (!itr2->IsObject())
                                        break;
                                    if (itr2->HasMember("lane_guide") && itr2->IsArray()) {

                                    if (itr2->HasMember("lane_guide")) {
                                        const Value &a3 = (*itr2)["lane_guide"];

                                        for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {
@@ -946,11 +947,11 @@
                                                    temp.direct.push_back((*itr4).GetInt());
                                                }
                                            }

                                            DEBUG("\t得到一组导向线");
                                            sep.lane_direct.push_back(temp);
                                        }
                                    }
                                    if (itr2->HasMember("lane_line") && itr2->IsArray() ) {
                                    if (itr2->HasMember("lane_line")) {
                                        const Value &a3 = (*itr2)["lane_line"];
                                        DEBUG("\t线数量 %d", a3.Size());
                                        for (Value::ConstValueIterator itr3 = a3.Begin(); itr3 != a3.End(); ++itr3) {

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

@@ -42,6 +42,11 @@
    STOP_CAR_DONE
};

enum {
    CROSSING_NOT_HINT,
    CROSSING_HAS_HINT,
};

typedef struct {
    int road;
    int sep;
@@ -61,6 +66,11 @@
    int value;
    struct RtkTime time;
} car_sensor_value_t;

typedef struct {
    int gain;
    struct RtkTime time;
} change_lane_t;

static const int INVALID_ROAD = -1;

@@ -103,6 +113,7 @@
static bool startCarLeftTurnSignal, checkStartCarSignal;

static struct RtkTime crashGreenRunTime, crashGreenStartTime;

static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct drive_timer startCarLeftTurnSignalTime;
@@ -123,17 +134,13 @@
static bool handBreakActive = false;
static bool reportRPMOver = false;

static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);

static bool turnError13Cold, turnError14Cold;

static lane_t Lane;
static change_lane_t ChangeLane;

static bool laneChanging;
static int changeLaneDirect;
static int CrashLineType;

static map<int, car_sensor_value_t> CarSensorValue;
static map<int, int> CrossingHint;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -158,10 +165,6 @@

static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);

static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);


static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -211,11 +214,7 @@

    checkStartCarSignal = startCarLeftTurnSignal = false;

    turnError13Cold = turnError14Cold = true;

    Lane.road = Lane.sep = Lane.no = -1;
    laneChanging = false;
    changeLaneDirect = 0;

    nextRoadId = -1;
    checkTurn = false;
@@ -234,9 +233,6 @@
    TerminateStopCarExam();
    TerminateOperateGearExam();
    TerminateDriveStraightExam();

    AppTimer_delete(TurnSignalError13ColdTimer);
    AppTimer_delete(TurnSignalError14ColdTimer);
}

/*********************************************************************
@@ -598,10 +594,16 @@
        }

        // 分道实线
//        DEBUG("%d 分道段 %d", n, RoadMap.roads[n].separate.size());

        for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
            // 一组车道
//            DEBUG("%d %d 车道数 %d", n, m, RoadMap.roads[n].separate[m].lines.size());

            for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
                // 多根分道线

//                DEBUG("%d %d %d 线段树 %d", n, m, l, RoadMap.roads[n].separate[m].lines[l].size());
                for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) {
                    // 一根分道线中线型相同的
                    int character = RoadMap.roads[n].separate[m].lines[l][a].character;
@@ -827,39 +829,6 @@
    WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
}

static void TurnSignalError13ColdTimer(union sigval sig)
{
    AppTimer_delete(TurnSignalError13ColdTimer);

    turnError13Cold = true;
}

static void TurnSignalError14ColdTimer(union sigval sig)
{
    AppTimer_delete(TurnSignalError14ColdTimer);

    turnError14Cold = true;
}

static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime)
{
    if (err == 13 && turnError13Cold) {
        DEBUG("起步不开转向灯");
        AddExamFault(13, rtkTime);

        turnError13Cold = false;
        AppTimer_delete(TurnSignalError13ColdTimer);
        AppTimer_add(TurnSignalError13ColdTimer, TURN_ERROR_COLD_TIME);
    } else if (err == 14 && turnError14Cold) {
        DEBUG("起步转向灯不足3秒");
        AddExamFault(14, rtkTime);

        turnError14Cold = false;
        AppTimer_delete(TurnSignalError14ColdTimer);
        AppTimer_add(TurnSignalError14ColdTimer, TURN_ERROR_COLD_TIME);
    }
}

static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double moveDistance;
@@ -882,10 +851,10 @@
        if (!checkStartCarSignal && moveDirect == 1) {
            checkStartCarSignal = true;
            if (!startCarLeftTurnSignal) {
                ReportTurnSignalError(13, rtkTime);
                AddExamFault(13, rtkTime);
            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                   startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                ReportTurnSignalError(14, rtkTime);
                AddExamFault(14, rtkTime);
            }
        }

@@ -997,36 +966,46 @@
    if (GetLane(newLane, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, roadIndex)) {
        if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
            gain = newLane.no - Lane.no;
        } else {
            ChangeLane.gain = 0;
        }

        // 检查转向灯
        if (gain != 0) {
            DEBUG("变道 gain %d", gain);
            car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
            if (lamp.name == TURN_SIGNAL_LAMP) {
                if (gain < 0) {
                    if (lamp.value != LEFT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        ReportTurnSignalError(13, rtkTime);
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                        AddExamFault(14, rtkTime);
                    }
                } else {
                    if (lamp.value != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        ReportTurnSignalError(13, rtkTime);
                    } else {
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                        AddExamFault(14, rtkTime);
                    }
                }
            }

            if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < CHANGE_LANE_MIN_INTERVAL) {
                DEBUG("连续变道");
                AddExamFault(15, rtkTime);
            }
            ChangeLane.gain = gain;
            ChangeLane.time = *rtkTime;
        }

        Lane = newLane;
@@ -1034,33 +1013,222 @@
    return gain;
}

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
static void ChangeCrossingStatus(int roadIndex, int index, int status)
{
    uint32_t cts = AppTimer_GetTickCount();
    int ri = CalcRoadIndex(-1, RoadMap, car);
    bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList);
    lane_t laneInfo;
    double redist = -1;
    int key = roadIndex * 100 + index;

    laneInfo.road = -1;
    laneInfo.sep = -1;
    laneInfo.no = -1;
    auto it = CrossingHint.find(key);

    if (ri >= 0) {
        GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
    if (it != CrossingHint.end()) {
        CrossingHint.erase(it);
    }
    CrossingHint.insert(pair<int, int>(key, status));
}

        int m = RoadMap.roads[ri].rightEdge.size();
        int n = RoadMap.roads[ri].rightEdge[m-1].points.size();
static int GetCrossingStatus(int roadIndex, int index)
{
    int key = roadIndex * 100 + index;

        PointF base;
    auto it = CrossingHint.find(key);

        base.X = 428922.2985; base.Y = 3292119.5457;
    if (it != CrossingHint.end()) {
        return it->second;
    }
    return CROSSING_NOT_HINT;
}

        redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
                              RoadMap.roads[ri].rightEdge);
static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    for (int i = 0; i < road.stopLine.size(); ++i) {
        PointF point;
        double distance;

        point.X = road.stopLine[i].line.X1;
        point.Y = road.stopLine[i].line.Y1;

        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, road.rightEdge);

        if (distance > 10 && distance < 50) {
            // 提示路口怎么走
            if (GetCrossingStatus(roadIndex, i) == CROSSING_NOT_HINT) {
                if (!road.stopLine[i].tts.empty()) {
                    DEBUG("路口提示 %s", road.stopLine[i].tts.c_str());
                    PlayTTS(road.stopLine[i].tts.c_str());
                }
                ChangeCrossingStatus(roadIndex, i, CROSSING_HAS_HINT);
            }
        } else if (distance > 55 && GetCrossingStatus(roadIndex, i) != CROSSING_NOT_HINT) {
            ChangeCrossingStatus(roadIndex, i, CROSSING_NOT_HINT);
        }
    }
}

static map <int, int> TargetReduceRec;

enum {
    NOT_REDUCE_SPEED,
    REDUCE_SPEED,
    STOP_CAR,
    OVER_SPEED
};

static void ChangeTargetReduceRec(int roadIndex, int index, int status)
{
    int key = roadIndex * 100 + index;

    auto it = CrossingHint.find(key);

    if (it != CrossingHint.end()) {
        CrossingHint.erase(it);
    }
    CrossingHint.insert(pair<int, int>(key, status));
}

static int GetTargetReduceRec(int roadIndex, int index)
{
    int key = roadIndex * 100 + index;

    auto it = CrossingHint.find(key);

    if (it != CrossingHint.end()) {
        return it->second;
    }
    return -1;
}

void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, double speed, int moveDirect)
{
    car_sensor_value_t brk = ReadCarSensorValue(BREAK);

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;

    for (int n = 0; n < RoadMap.roads[roadIndex].stopLine.size(); ++n) {
        PointF point;
        double distance;

        point.X = road.stopLine[i].line.X1;
        point.Y = road.stopLine[i].line.Y1;

        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);

        if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
            // 记录刹车，停车
            if (brk.value == BREAK_ACTIVE) {

            }
        } else if (distance > LASTEST_BREAK_POINT + 5) {

        }
    }

    DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);
    for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
        if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
            continue;

        if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
            // 计算点到左侧路边线的垂点
            int road = 0;
            for (road = 0; road < map.roads.size(); ++road) {
                if (map.roads[road].id == map.specialAreas[i].road)
                    break;
            }

            PointF vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
//            DEBUG("计算垂点1 (%f, %f)", vPoint.X, vPoint.Y);

            map.specialAreas[i].leftPoints.push_back(vPoint);

            vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
//            DEBUG("计算垂点2 (%f, %f)", vPoint.X, vPoint.Y);
            map.specialAreas[i].leftPoints.push_back(vPoint);
        }

        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
//            DEBUG("斑马线");
            Line startLine;

            MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]);

            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&
                IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
                DEBUG("进入减速区 %d", map.specialAreas[i].type);
                if (BreakDone == BREAK_ACTIVE) {
                    map.specialAreas[i].activeBreak = true;
                }
            }

            // 跨线后，检查刹车动作
            if (CrashTheLine(startLine, car, CarModelList)) {
                if (!map.specialAreas[i].activeBreak) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");
                    if (map.specialAreas[i].type == ZEBRA_CROSSING) {
                        AddExamFault(48, rtkTime);
                    } else {
                        AddExamFault(50, rtkTime);
                    }
                } else {
                    DEBUG("按规定减速");
                }

            }
        } else if (map.specialAreas[i].type == SCHOOL_AREA) {
            Polygon school;
//            DEBUG("学校");
            school.num = 4;
            school.point = (PointF *) malloc(school.num * sizeof(PointF));

            school.point[0] = map.specialAreas[i].area[0];
            school.point[1] = map.specialAreas[i].area[1];
            school.point[2] = map.specialAreas[i].leftPoints[1];
            school.point[3] = map.specialAreas[i].leftPoints[0];

            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
                    if (!map.specialAreas[i].overSpeed) {
                        DEBUG("通过学校区域超速");
                        AddExamFault(49, rtkTime);
                        map.specialAreas[i].overSpeed = true;
                    }
                }
            }

            free(school.point);
        }
    }
}



void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
//    uint32_t cts = AppTimer_GetTickCount();
//    int ri = CalcRoadIndex(-1, RoadMap, car);
//    bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList);
//    lane_t laneInfo;
//    double redist = -1;
//
//    laneInfo.road = -1;
//    laneInfo.sep = -1;
//    laneInfo.no = -1;
//
//    if (ri >= 0) {
//        GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
//
//        int m = RoadMap.roads[ri].rightEdge.size();
//        int n = RoadMap.roads[ri].rightEdge[m-1].points.size();
//
//        PointF base;
//
//        base.X = 428922.2985; base.Y = 3292119.5457;
//
//        redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
//                              RoadMap.roads[ri].rightEdge);
//    }
//
//    DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);

    UpdateCarSensor(rtkTime);

@@ -1225,15 +1393,27 @@
                                 speed, moveDirect, rtkTime);

    // 检测离开此路段，全车需不在范围内
    int oldid = currExamMapIndex;
    currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
    if (currExamMapIndex >= 0) {
        HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
    }

    if (oldid != currExamMapIndex) {
        DEBUG("道路ID切换 %d", currExamMapIndex);
    }

    oldid = CrashLineType;

    DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);

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

    DetectLane(RoadMap, car, currExamMapIndex, rtkTime);

    // 检测压线状态


    TestRoadStartCar(car, speed, moveDirect, rtkTime);

    if (startCar != START_CAR_DONE)
@@ -1482,84 +1662,6 @@
        road.arrivedTail = false;
    }
}*/

// 路口，提示，前30米进入， 离开
// 人行道，车站，前30米进入，离开
// 学校，进入，离开

map<int, int> CrossingStatus;

static void ChangeCrossingStatus(int roadIndex, int index, int status)
{
    int key = roadIndex * 100 + index;

    auto it = CrossingStatus.find(key);

    if (it != CrossingStatus.end()) {
        CrossingStatus.erase(it);
    }
    CrossingStatus.insert(pair<int, int>(key, status));
}

static int GetCrossingStatus(int roadIndex, int index)
{
    int key = roadIndex * 100 + index;

    auto it = CrossingStatus.find(key);

    if (it != CrossingStatus.end()) {
        return it->second;
    }
    return -1;
}

enum {
    CROSSING_STATUS_INVALID = -1,
    CROSSING_STATUS_GUIDE,
    CROSSING_STATUS_REDUCE_SPEED,
    CROSSING_STATUS_BREAK_CAR,
    CROSSING_STATUS_STOP_CAR
};

static void CheckCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    for (int i = 0; i < road.stopLine.size(); ++i) {
        if (CrashTheLine(road.stopLine[i].line, car, CarModelList)) {
            if (road.stopLine[i].stopFlag) {
                if (GetCrossingStatus(roadIndex, i) != CROSSING_STATUS_STOP_CAR) {

                }
            } else {
                if (GetCrossingStatus(roadIndex, i) != CROSSING_STATUS_BREAK_CAR) {

                }
            }
            ChangeCrossingStatus(roadIndex, i, CROSSING_STATUS_INVALID);
        } else {

        }
        PointF point;
        double distance;

        point.X = road.stopLine[i].line.X1;
        point.Y = road.stopLine[i].line.Y1;

        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, road.rightEdge);

        if (distance > 10 && distance < 50) {
            if (GetCrossingStatus(roadIndex, i) == CROSSING_STATUS_INVALID) {
                if (!road.stopLine[i].tts.empty()) {
                    PlayTTS(road.stopLine[i].tts.c_str());
                }
                ChangeCrossingStatus(roadIndex, i, CROSSING_STATUS_GUIDE);
            }
        } else if (distance > 0 && distance < 30) {
            if (GetCrossingStatus(roadIndex, i) != 1) {
                ChangeCrossingStatus(roadIndex, i, 1);
            }
        }
    }
}

static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{