坐标

yy1717

2020-05-15 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba

 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("变更车道");
        struct car_on_lane lane;
        if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {

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

            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);
                }
                CarOnLane = lane;
            }

            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 {