坐标

yy1717

2020-05-15 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba

坐标

 lib/src/main/cpp/driver_test.cpp

@@ -196,106 +196,9 @@

    DEBUG("清除旧的路考地图");

    if (RoadMapPoints.point != NULL) {
        free(RoadMapPoints.point);
    }
    RoadMapPoints.num = 0;

    for (int i = 0; i < RoadMap.roads.size(); ++i) {
        for (int j = 0; j < RoadMap.roads[i].leftEdge.size(); ++j) {
            RoadMap.roads[i].leftEdge[j].points.clear();
            vector<PointF>().swap(RoadMap.roads[i].leftEdge[j].points);
        }
        RoadMap.roads[i].leftEdge.clear();
        vector<edge_t>().swap(RoadMap.roads[i].leftEdge);

        for (int j = 0; j < RoadMap.roads[i].rightEdge.size(); ++j) {
            RoadMap.roads[i].rightEdge[j].points.clear();
            vector<PointF>().swap(RoadMap.roads[i].rightEdge[j].points);
        }
        RoadMap.roads[i].rightEdge.clear();
        vector<edge_t>().swap(RoadMap.roads[i].rightEdge);

        for (int j = 0; j < RoadMap.roads[i].separate.size(); ++j) {
            for (int k = 0; k < RoadMap.roads[i].separate[j].lines.size(); ++k) {
                for (int m = 0; m < RoadMap.roads[i].separate[j].lines[k].size(); ++m) {
                    RoadMap.roads[i].separate[j].lines[k][m].points.clear();
                    vector<PointF>().swap(RoadMap.roads[i].separate[j].lines[k][m].points);
                }

                RoadMap.roads[i].separate[j].lines[k].clear();
                vector<segment_t>().swap(RoadMap.roads[i].separate[j].lines[k]);
            }
            RoadMap.roads[i].separate[j].lines.clear();
            vector<vector<segment_t>>().swap(RoadMap.roads[i].separate[j].lines);
        }
        RoadMap.roads[i].separate.clear();
        vector<separate_t>().swap(RoadMap.roads[i].separate);
    }

    RoadMap.roads.clear();
    vector<road_t>().swap(RoadMap.roads);

    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
        RoadMap.specialAreas[i].area.clear();
        vector<PointF>().swap(RoadMap.specialAreas[i].area);
    }
    RoadMap.specialAreas.clear();
    vector<special_area_t>().swap(RoadMap.specialAreas);

    RoadMap.triggerLines.clear();
    vector<trigger_line_t>().swap(RoadMap.triggerLines);
/*
    for (int i = 0; i < RoadMapList.size(); ++i) {
        struct road_exam_map map = RoadMapList[i];

        if (map.redLine != NULL) {
            for (int j = 0; j < map.redLineNum; ++j) {
                if (map.redLine[j].point != NULL)
                    free(map.redLine[j].point);
            }
            free(map.redLine);
        }

        if (map.greenLine != NULL) {
            for (int j = 0; j < map.greenLineNum; ++j) {
                if (map.greenLine[j].point != NULL)
                    free(map.greenLine[j].point);
            }
            free(map.greenLine);
        }

        if (map.redArea != NULL) {
            for (int j = 0; j < map.redAreaNum; ++j) {
                if (map.redArea[j].point != NULL)
                    free(map.redArea[j].point);
            }
            free(map.redArea);
        }

        if (map.triggerLine != NULL) {
            for (int j = 0; j < map.triggerLineNum; ++j) {
                if (map.triggerLine[j].line.point != NULL)
                    free(map.triggerLine[j].line.point);
            }
            free(map.triggerLine);
        }

        if (map.area.point != NULL) {
            free(map.area.point);
        }

        if (map.roadEdgeLine != NULL) {
            for (int j = 0; j < map.roadEdgeLineNum; ++j) {
                if (map.roadEdgeLine[j].point != NULL)
                    free(map.roadEdgeLine[j].point);
            }

            free(map.roadEdgeLine);
        }
    }

    RoadMapList.clear();*/
}

void SetRoadMap(road_exam_map &map)

 lib/src/main/cpp/driver_test.h

@@ -168,6 +168,7 @@
    int targetRoad;
    int stopFlag;
    string tts;
    bool arrivedTail;
    Polygon area;       // 整个道路区域
    std::vector<edge_t> leftEdge;
    std::vector<edge_t> rightEdge;
@@ -186,6 +187,8 @@
    int id;
    int road;
    int active;
    int time;                   // 项目最大完成时间
    int distance;               // 项目最大完成距离
    string tts;
    Line line;
} trigger_line_t;

 lib/src/main/cpp/test_common/Geometry.cpp

@@ -536,10 +536,10 @@
    p2.Y = line.Y2;

    PointF pv = GetVerticalPoint(p1, p2, point);
    vp = pv;

    if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
        (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
        vp = pv;
        return true;
    }
    return false;

 lib/src/main/cpp/test_items2/drive_straight.cpp

@@ -15,65 +15,50 @@
static const double CHECK_STAGE_DISTANCE = 100.0;
static const double MAX_OFFSET_DISTANCE = 0.3;

static int ttsPlayEnd;
static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;
static double distanceToStartSum;

void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) {
    if (index == -1)
        return;
static double CalcDistance2Edge(road_t &road,  const car_model *car);

    DEBUG("进入路考直线行驶地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
void StartDriveStraightExam(std::string tts) {
    DEBUG("开始直线行驶");

    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS");
        PlayTTS(RoadMapList[index].tts.c_str());
    if (!tts.empty()) {
        PlayTTS(tts.c_str());
    } else {
        DEBUG("没有TTS");
    }

    crossStartLine = false;
    distanceToStartSum = 0;
    reportOffsetOver = false;
}

int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
int ExecuteDriveStraightExam(road_t &road,  const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    Line road_edge;

    static PointF startPoint;

    MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);
    double dis2roadEdge = 0;

    {
        double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge);

        double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge);

        MA_SendDistance(6 - (l1+l2)/2.0, (l1+l2)/2.0);
    }

    if (!crossStartLine) {
        PointF p1, p2;

        p1.X = RoadMapList[index].stopLine.X1;
        p1.Y = RoadMapList[index].stopLine.Y1;
        p2.X = RoadMapList[index].stopLine.X2;
        p2.Y = RoadMapList[index].stopLine.Y2;

        if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) {
            DEBUG("跨过标记线，开始直线测试");
            crossStartLine = true;
    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
        edgeDistance = CalcDistance2Edge(road, car);           // 基准边距

            DEBUG("当前基准路边间距 %f", edgeDistance);
        }
    } else {

    if (ttsPlayEnd != 2)
        return 1;

        double distanceToStart = DistanceOf(car->basePoint, startPoint);
    dis2roadEdge = CalcDistance2Edge(road, car);

        DEBUG("路边间距 %f --- %f", DistanceOf(car->basePoint, road_edge), edgeDistance);
    DEBUG("路边间距 %f --- %f", dis2roadEdge, edgeDistance);

        if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
    if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米");
            // 偏移大于30厘米，不合格
            AddExamFault(30, rtkTime);
@@ -81,22 +66,66 @@

            //////////////////////////////////////////////
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
        edgeDistance = dis2roadEdge;
            reportOffsetOver = false;
        }

        if (distanceToStart > CHECK_STAGE_DISTANCE) {
            DEBUG("复位边距偏移量");
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
        edgeDistance = dis2roadEdge;
            reportOffsetOver = false;
        }
        distanceToStartSum += distanceToStart;
        distanceToStart = 0;
    }

    if (ExitSonArea(index, RoadMapList, car)) {
    if (distanceToStart + distanceToStartSum > 150) {
        DEBUG("离开直线行驶区域");
        return -1;
    }
    return 1;
}

    return index;
static double CalcDistance2Edge(road_t &road,  const car_model *car)
{
    PointF vp;
    bool get_vp = false;
    double distance = 0;

    // 检测道路边缘线
    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (VerticalPointOnLine(car->basePoint, edge, vp)) {
                get_vp = true;
                goto FIND_VP_END;
            }

            p1 = p2;
        }
    }

FIND_VP_END:
    if (get_vp) {
        distance = DistanceOf(car->basePoint, vp);
    } else {
        // 没有找到匹配线端，按最小距离顶点计算
        distance = 100;
        for (int i = 0; i < road.leftEdge.size(); ++i) {
            for (int j = 0; j < road.leftEdge[i].points.size(); ++j) {
                double x;
                if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) {
                    distance = x;
                }
            }
        }
    }

    return distance;
}

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

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

#include "../driver_test.h"

void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList);
int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
void StartDriveStraightExam(std::string tts);
int ExecuteDriveStraightExam(road_t &road,  const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_DRIVE_STRAIGHT_H

 lib/src/main/cpp/test_items2/operate_gear.cpp

@@ -30,9 +30,7 @@

static void CheckGear(union sigval sig);

void StartOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const struct RtkTime *rtkTime) {
    if (index == -1)
        return;
void StartOperateGearExam(const struct RtkTime *rtkTime) {
    DEBUG("开始加减挡操作");

    testing = true;
@@ -148,10 +146,9 @@
    }
}

int ExecuteOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                       LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
int ExecuteOperateGearExam(const struct RtkTime *rtkTime) {
    currRtkTime = *rtkTime;
    return testing ? index : -1;
    return testing ? 1 : -1;
}

void OperateGearTTSDone(int id)

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

@@ -7,9 +7,8 @@

#include "../driver_test.h"

void StartOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const struct RtkTime *rtkTime);
int ExecuteOperateGearExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                           LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
void StartOperateGearExam(const struct RtkTime *rtkTime);
int ExecuteOperateGearExam(const struct RtkTime *rtkTime);
void OperateGearTTSDone(int id);
void TerminateOperateGearExam(void);


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

@@ -89,7 +89,8 @@
    int road;
    int separate;
    int lane;
} CarOnLane;
} CurrentLane;
static bool laneChanging;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -100,6 +101,8 @@
static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);

static const double NEXT_ROAD_TIP = 100.0;                  // 到达路口前提示下一个怎么走

static const int CRL_NONE = 0;
static const int CRL_SEP_DOTTED = 1;
@@ -127,6 +130,7 @@
static int CrashRoadLine(road_t &road, const car_model *car);
static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
static bool LaneIsValid(struct car_on_lane lane);
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);

void InitRoadExam(road_exam_map &RoadMap)
{
@@ -167,7 +171,8 @@

    currCarOnRoadIndex = FIND_POSITION;

    CarOnLane.road = CarOnLane.separate = CarOnLane.lane = -1;
    CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
    laneChanging = false;

    InitThroughSomething(RoadMap);
}
@@ -395,6 +400,16 @@
            reportStopCarOnRedArea = 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;
@@ -402,6 +417,7 @@
        }
        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)) {
@@ -411,6 +427,7 @@
            reportStopCarOnRedArea = true;
        }*/
    } else if (moveDirect == -1) {
        // 持续后滑
        if (occurSlide) {
            double slideDistance = DistanceOf(stopPoint, car->basePoint);

@@ -423,18 +440,32 @@
                AddExamFault(5, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
                slideNormalDistance = false;
                occurSlide = false;
            }
        }
    } 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;
    }

    // 检测通过路口、人行道等区域时，释放刹车或减速
@@ -563,32 +594,174 @@
        }
    }

    // 检测压线状态
    bool crashRedLineNow = false;
    bool crashGreenLineNow = false;

    if (currExamMapIndex >= 0) {
        int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);

        if (crl == CRL_NONE) {
            DEBUG("什么都没压");
//            DEBUG("什么都没压");
        } else if (crl == CRL_SEP_DOTTED) {
            DEBUG("压分道虚线");
//            DEBUG("压分道虚线");
            crashGreenLineNow = true;
        } else if (crl == CRL_SEP_SOLID) {
            DEBUG("压分道实线");
//            DEBUG("压分道实线");
            crashRedLineNow = true;
        } else if (crl == CRL_EDGE_DOTTED) {
            DEBUG("压边沿虚线");
//            DEBUG("压边沿虚线");
            crashGreenLineNow = true;
        } else if (crl == CRL_EDGE_SOLID) {
            DEBUG("压边沿实线");
//            DEBUG("压边沿实线");
            crashRedLineNow = true;
        }

        if (crl != CRL_SEP_DOTTED || crl != CRL_SEP_SOLID) {
            struct car_on_lane lane;
            UpdateLane(lane, RoadMap.roads[currExamMapIndex], car);
            if (!LaneIsSame(lane, CarOnLane)) {
                if (LaneIsValid(CarOnLane)) {
                    // 车道变换
                    DEBUG("变更车道");
        if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {

            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
                lane.lane == CurrentLane.lane) {
                laneChanging = false;
                }
                CarOnLane = lane;

            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
                lane.lane != CurrentLane.lane) {
                if (crl == CRL_SEP_DOTTED) {
                    // 变道进行中
                    if (!laneChanging) {
                        laneChanging = true;
                        DEBUG("发生变道");
                        // 比较上次跨线时间
                        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_ROAD_MIN_INTERVAL) {
                                DEBUG("===================== 连续变道 ============!!");
                                // 连续变道，不合格
                                AddExamFault(15, rtkTime);
            }
        }

                        // 检查变道前，是否提前转向灯
                        if (lane.lane < CurrentLane.lane) {
                            // 向左侧变道
                            DEBUG("向左侧变道");
                            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                                DEBUG("变调未打灯!!");
                                // 没打灯，不合格
                                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 {
                            // 向右侧变道
                            DEBUG("向右侧变道");
                            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                                DEBUG("变调未打灯!!");
                                // 没打灯，不合格
                                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 {
                    // 变道完成
                    DEBUG("变道完成");
                    CurrentLane = lane;
                    laneChanging = false;

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

            if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
                // 路或段变更，撤销变道跟踪
                DEBUG("============== 路或段变更 CURR %d, %d NEW %d, %d", CurrentLane.road,
                      CurrentLane.separate,
                      lane.road, lane.separate);
                CurrentLane = lane;
                laneChanging = false;
            }
        }
    }

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

    // 压虚线
    if (crashGreenLineNow) {
        // 压虚线
        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;
        }
    } else {
        // 不再压虚线
        occurCrashGreenLine = false;
        checkCrashGreenTimeout = 0;
    }
}

@@ -636,6 +809,8 @@
                rightExt = true;
                MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto RIGHT_EXT_CMP;
            } else {
                DEBUG("右侧不垂点 %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
            }
            p1 = p2;
        }
@@ -643,6 +818,7 @@
RIGHT_EXT_CMP:

    if (!leftExt || !rightExt) {
        DEBUG("左右边界不匹配");
        return false;
    }

@@ -669,20 +845,20 @@
                        orthogonal.insert(pair<int, int>(j, 1));
                        orthogonalInSegment = true;
                        intersection = true;
                        DEBUG("分道线 %d 左正交", j);
//                        DEBUG("分道线 %d 左正交", j);
                        break;
                    } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
                        orthogonal.insert(pair<int, int>(j, 2));
                        orthogonalInSegment = true;
                        intersection = true;
                        DEBUG("分道线 %d 右正交", j);
//                        DEBUG("分道线 %d 右正交", j);
                        break;
                    }
                    p1 = p2;
                }
            }
        }
        DEBUG("目标 %d 当前 %d", road.separate[i].lines.size(), orthogonal.size());
//        DEBUG("目标 %d 当前 %d", road.separate[i].lines.size(), orthogonal.size());

        if (orthogonal.size() > 0) {
            if (orthogonal.size() == road.separate[i].lines.size()) {
@@ -696,7 +872,7 @@
                            lane.separate = i;
                            lane.lane = itx->first;

                            DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
//                            DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
                            break;
                        }
                    }
@@ -706,16 +882,18 @@
                    lane.separate = i;
                    lane.lane = orthogonal.size();

                    DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
//                    DEBUG("路 %d 段 %d 车道 %d", lane.road, lane.separate, lane.lane);
                }
                out = lane;
                return true;
            } else {
                // 不完全正交，直接退出
            }
            DEBUG("分道线长短不同，退出");
            return false;
        }
    }
    DEBUG("段匹配失败");
    return false;
}

@@ -800,12 +978,12 @@
                            // 压实线
                            return CRL_SEP_SOLID;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
                            if (LaneIsValid(CarOnLane) && CarOnLane.lane <= j) {
                            if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
                            if (LaneIsValid(CarOnLane) && CarOnLane.lane > j) {
                            if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
@@ -1164,11 +1342,11 @@
                    RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
                StartThroughExam(currExamMapIndex, RoadMapList);
            } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
                StartDriveStraightExam(currExamMapIndex, RoadMapList);

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

            } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
                StartOperateGearExam(currExamMapIndex, RoadMapList, rtkTime);

            }
        }
    } else if (startCar == START_CAR_DONE) {
@@ -1179,14 +1357,11 @@
                                                  CarModelList, speed, moveDirect, rtkTime);
        }
        else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
            currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
                                     CarModelList, speed, moveDirect, rtkTime);

        } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
            currExamMapIndex = ExecuteStopCarExam(currExamMapIndex, RoadMapList, car,
                                                        CarModelList, speed, moveDirect, rtkTime);

        } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
            currExamMapIndex = ExecuteOperateGearExam(currExamMapIndex, RoadMapList, car,
                                   CarModelList, speed, moveDirect, rtkTime);

        }

        if (currExamMapIndex == -1) {
@@ -1636,6 +1811,29 @@
    return px;
}

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] ]);

    PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
    Line extLine;

    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);

    if (IntersectionOf(extLine, road.stopLine) == GM_Intersection &&
        IntersectionOfLine(extPoint, road.stopLine) == -1) {
            if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) {
                // 接近路口后，要检查车辆是否进入错误车道
                road.arrivedTail = true;
                if (!road.tts.empty())
                    PlayTTS(road.tts.c_str());
            }
    } else if (road.arrivedTail) {
        road.arrivedTail = false;
    }
}

#if 0

typedef struct {

 lib/src/main/cpp/test_items2/stop_car.cpp

@@ -33,12 +33,8 @@

static void PlayTTSTimeout(union sigval sig);

void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList) {
    if (index == -1)
        return;

    DEBUG("进入靠边停车地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);

void StartStopCarExam(std::string tts) {
    DEBUG("靠边停车");
    ttsPlayEnd = 0;
    moveDistance = 0;
    prevMoveDirect = 0;
@@ -49,8 +45,8 @@
    doorNotClose = false;
    checkRoadDistance = false;

    if (!RoadMapList[index].tts.empty()) {
        examTtsSeq = PlayTTS(RoadMapList[index].tts.c_str());
    if (!tts.empty()) {
        examTtsSeq = PlayTTS(tts.c_str());
    } else {
        examTtsSeq = PlayTTS("请靠边停车");
    }
@@ -73,7 +69,7 @@
    AppTimer_delete(PlayTTSTimeout);
}

int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
@@ -114,25 +110,50 @@
        if (tp - stopTimepoint >= STOP_CAR_TIME && !checkRoadDistance) {
            // 停车超2秒，开始判断
            DEBUG("检测和路边的距离");
            PointF pc;
            PointF pc, vp;
            bool get_vp = false;
            double dis2roadEdge = 0;

            checkRoadDistance = true;

            // 前后轮的中点
            pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2;
            pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2;

            // 检测道路边缘线
            for (int i = 0; i < RoadMapList[index].roadEdgeLineNum; ++i) {
                PointF p1, p2, pv;
            for (int i = 0; i < road.rightEdge.size(); ++i) {
                PointF p1, p2;
                Line edge;

                p1 = RoadMapList[index].roadEdgeLine[i].point[0];
                for (int j = 1; j < RoadMapList[index].roadEdgeLine[i].num; ++j) {
                    p2 = RoadMapList[index].roadEdgeLine[i].point[j];
                    pv = GetVerticalPoint(p1, p2, pc);
                p1 = road.rightEdge[i].points[0];
                for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
                    p2 = road.rightEdge[i].points[j];
                    MakeLine(&edge, &p1, &p2);

                    if (isEqual2(pv.X, MIN(p1.X, p2.X)) || isEqual2(pv.X, MAX(p1.X, p2.X)) ||
                            (pv.X > MIN(p1.X, p2.X) && pv.X < MAX(p1.X, p2.X))) {
                        double dis2roadEdge = DistanceOf(pc, pv);

                    if (VerticalPointOnLine(pc, edge, vp)) {
                        get_vp = true;
                        goto FIND_VP_END;
                    }

                    p1 = p2;
                }
            }

FIND_VP_END:
            if (get_vp) {
                dis2roadEdge = DistanceOf(pc, vp);
            } else {
                // 没有找到匹配线端，按最小距离顶点计算
                dis2roadEdge = 100;
                for (int i = 0; i < road.rightEdge.size(); ++i) {
                    for (int j = 0; j < road.rightEdge[i].points.size(); ++j) {
                        double dis;
                        if (dis2roadEdge > (dis = DistanceOf(pc, road.rightEdge[i].points[j]))) {
                            dis2roadEdge = dis;
                        }
                    }
                }
            }

                        DEBUG("停车距路边距离 %f", dis2roadEdge);

@@ -144,11 +165,6 @@
                            DEBUG("停车超出路边0.3米");
                            // 停车距离超过30厘米，扣10分
                            AddExamFault(37, rtkTime);
                        }
                    }

                    p1 = p2;
                }
            }
        }
    }

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

@@ -7,10 +7,10 @@

#include "../driver_test.h"

void StartStopCarExam(int index, LIST_ROAD_MAP &RoadMapList);
void StopCarTTSDone(int id);
int ExecuteStopCarExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
void StartStopCarExam(std::string tts);
int ExecuteStopCarExam(int index, road_t &road, const car_model *car,
                       LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
void TerminateStopCarExam(void);
void StopCarTTSDone(int id);

#endif //MYAPPLICATION2_STOP_CAR_H

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

@@ -18,6 +18,7 @@
using namespace std;

static const double LASTEST_BREAK_POINT = 30.0;
static const double NEXT_ROAD_TIP = 100.0;                  // 到达路口前提示下一个怎么走
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;
static const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh

@@ -155,17 +156,52 @@
    return index;
}



void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    int BreakDone = ReadCarStatus(BREAK);

    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
    // 计算车前进轨迹延长线
            double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
            PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], LASTEST_BREAK_POINT, yaw);
            Line extLine;
    PointF extPoint2 = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
    Line extLine, extLine2;
            MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
    MakeLine(&extLine2, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint2);

    // 路口刹车点
    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 ) {
            DEBUG("进入减速区");
            if (BreakDone == BREAK_ACTIVE) {
                auto itx = breakRecord.find(map.roads[i].id);
                if (itx != breakRecord.end()) {
                    itx->second = true;
                }
            }
        }
        // 跨线后，检查刹车动作
        if (CrashTheLine(map.roads[i].stopLine, car, CarModelList)) {
            auto itx = breakRecord.find(map.roads[i].id);
            if (itx != breakRecord.end()) {
                if (itx->second == false) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");

                }
                itx->second = false;
            }
        }
    }
    // 人行道、公交站刹车点；学校限速区
    for (int i = 0; i < map.specialAreas.size(); i++) {
        if (map.specialAreas[i].type == GRID_AREA)
            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) {
@@ -174,11 +210,19 @@
            }

            PointF vPoint = GetSELine(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 = GetSELine(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
            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) {
            Line startLine;

            DEBUG("计算垂点 (%f, %f)", vPoint.X, vPoint.Y);

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

            // 车头和斑马线距离不足30米
            if (IntersectionOf(extLine, startLine) == GM_Intersection &&