坐标

yy1717

2020-08-10 d9dc7419b35b7762bc5222cf874ccc39d2d66f43

坐标

 lib/src/main/cpp/driver_test.h

@@ -141,23 +141,32 @@

// 一组平行的分道线
typedef struct {
    std::vector<lane_direct_t> lane_direct;
    std::vector<lane_direct_t> lane_direct;         // 每一车道的运行方向
    std::vector<std::vector<segment_t>> lines;      // 每段域下的平行的一组线
} separate_t;

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

typedef struct {
    int id;
    Line startLine;
    Line stopLine;
    int active;         // 到达路口尾部的行进方向
//    Line stopLine;
//    int active;         // 到达路口尾部的行进方向
    bool activeBreak;   // 路口刹车减速
    bool activeStop;    // 路口停车瞭望
    bool errorLane;     // 错误车道
    int targetRoad;
    int stopFlag;
    string tts;
    bool arrivedTail;
//    int stopFlag;
//    string tts;
//    bool arrivedTail;
    Polygon area;       // 整个道路区域

    std::vector<stop_line_t> stopLine;
    std::vector<edge_t> leftEdge;
    std::vector<edge_t> rightEdge;
    std::vector<separate_t> separate;

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

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

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

                                PointF p1, p2;
@@ -824,7 +824,52 @@
                                }

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

                                vector<stop_line_t> crossing;

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2) {
                                    stop_line_t temp;
                                    if (!itr2->IsObject()) {
                                        break;
                                    }
                                    if (itr2->HasMember("active")) {
                                        const Value &s = (*itr2)["active"];
                                        temp.active = s.GetInt();
                                    }
                                    if (itr2->HasMember("stop_flag")) {
                                        const Value &s = (*itr2)["stop_flag"];
                                        temp.stopFlag = s.GetInt();
                                    }
                                    if (itr2->HasMember("tts")) {
                                        const Value &s = (*itr2)["tts"];
                                        temp.tts = s.GetString();
                                    }
                                    if (itr2->HasMember("line")) {
                                        const Value &s = (*itr2)["line"];
                                        PointF p1, p2;
                                        int n = 0;

                                        if (s.IsArray() && s.Size() >= 2) {
                                            for (Value::ConstValueIterator itr3 = s.Begin(); itr3 != s.End(); ++itr3, ++n) {
                                                if (n == 0) {
                                                    p1 = mapPoints[(*itr3).GetInt()];
                                                } else if (n == 1) {
                                                    p2 = mapPoints[(*itr3).GetInt()];
                                                }
                                            }
                                            MakeLine(&temp.line, &p1, &p2);
                                        }
                                    }

                                    crossing.push_back(temp);
                                }

                                road.stopLine.assign(crossing.begin(), crossing.end());
                            }

                            if (itr->HasMember("active")) {
                                const Value &s = (*itr)["active"];
                                DEBUG("路口动作 %d", s.GetInt());

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

@@ -1379,11 +1379,11 @@

        ArrivedRoadEnd(RoadMap.roads[currExamMapIndex], car, CarModelList);

        if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) {
            DEBUG("下一个目标路 id = %d", RoadMap.roads[currExamMapIndex].targetRoad);
            nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad;
            checkTurn = true;
        }
//        if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) {
//            DEBUG("下一个目标路 id = %d", RoadMap.roads[currExamMapIndex].targetRoad);
//            nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad;
//            checkTurn = true;
//        }

        if (checkTurn) {
            // 检查是否持续转向
@@ -1988,7 +1988,7 @@
 * @param car
 * @param CarModelList
 */
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
/*static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    // 计算车前进轨迹延长线
    double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
@@ -2009,6 +2009,84 @@
            }
    } else if (road.arrivedTail) {
        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);
            }
        }
    }
}

@@ -2084,7 +2162,7 @@
    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);

    // 路口刹车点
    for (int i = 0; i < map.roads.size(); ++i) {
/*    for (int i = 0; i < map.roads.size(); ++i) {
        // 车头和路口距离不足30米
        if (IntersectionOf(extLine, map.roads[i].stopLine) == GM_Intersection &&
            IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], map.roads[i].stopLine) == 1 ) {
@@ -2122,7 +2200,8 @@
                }
            }
        }
    }
    }*/

    // 人行道、公交站刹车点；学校限速区
//    DEBUG("补全特殊区域 size = %d", map.specialAreas.size());
    for (int i = 0; i < map.specialAreas.size(); i++) {