坐标

yy1717

2020-08-14 576d819c4d9aabd1a8d102ae9c3bc13c9165ee1f

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

@@ -12,151 +12,209 @@
#include "../jni_log.h"
#include "../test_common/car_sensor.h"
#include "road_exam.h"
#include "../utils/xconvert.h"

#define DEBUG(fmt, args...)     LOGD("<through_something> <%s>: " fmt, __func__, ##args)

#if 0
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 map <int, int> TargetReduceRec;
static map <int, int> TargetReduceRec2;

#define NOT_ENTER           1
#define ENTER_Z             2
#define REDUCE_SPEED        4
#define STOP_CAR            8
#define OVER_SPEED          16

static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 3.0;
static const double PASS_SCHOOL_MAX_SPEED = 30.0;           // kmh
static const double LASTEST_BREAK_POINT = 30.0;

static int breakActive;
static int stopActive;
static bool crashRedLine;



void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
static void SetTargetReduceRec(map<int, int> &table, int key, int status)
{
    if (index == -1)
    auto it = table.find(key);

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

static int GetTargetReduceRec(map<int, int> &table, int key)
{
    auto it = table.find(key);

    if (it != table.end()) {
        return it->second;
    }
    return NOT_ENTER;
}

static void RemoveTargetReduceRec(map<int, int> &table, int key)
{
    auto it = table.find(key);

    if (it != table.end()) {
        table.erase(it);
    }
}

void ResetTarget(road_exam_map &RoadMap)
{
    TargetReduceRec.clear();
    TargetReduceRec2.clear();
}

void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;
    DEBUG("进入路考通过something地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS %s", RoadMapList[index].tts.c_str());
        PlayTTS(RoadMapList[index].tts.c_str());
    } else {
        DEBUG("没有TTS");
    }
    breakActive = 0;
    stopActive = 0;
    crashRedLine = false;
}
    // 路口
    for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
        PointF point;
        double distance;

void InitThroughSomething(road_exam_map &RoadMap)
{
    breakRecord.clear();
    // 为每一个刹车检测点
    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
        point.X = RoadMap.roads[roadIndex].stopLine[i].line.X1;
        point.Y = RoadMap.roads[roadIndex].stopLine[i].line.Y1;

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

        int key = roadIndex * 100 + i;
        int rec = GetTargetReduceRec(TargetReduceRec, key);

        if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
            if (rec == NOT_ENTER) {
                SetTargetReduceRec(TargetReduceRec, key, ENTER_Z);
            }
            // 记录刹车
            if (dobreak && !(rec & REDUCE_SPEED)) {
                SetTargetReduceRec(TargetReduceRec, key, rec | REDUCE_SPEED);
            }
        } else if (distance > 1e-3 && distance < DISTANCE_STOP_CAR_TO_STOP_LINE) {
            // 路口停车观察
            if (moveDirect == 0 && !(rec & STOP_CAR)) {
                SetTargetReduceRec(TargetReduceRec, key, rec | STOP_CAR);
            }
        } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
            RemoveTargetReduceRec(TargetReduceRec, key);
        }
    }
    // 人行道、公交站、学校
    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 (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
            breakRecord.insert(pair<int, bool>(RoadMap.specialAreas[i].id, false));
        }
    }
    for (int i = 0; i < RoadMap.roads.size(); ++i) {
        breakRecord.insert(pair<int, bool>(RoadMap.roads[i].id, false));
    }
}
            double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);

int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
        LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    PointF p1, p2;
            int key =  i;
            int rec = GetTargetReduceRec(TargetReduceRec2, key);

    double distance2StopLine = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
                                          RoadMapList[index].stopLine);
    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->axial[AXIAL_FRONT]]) == -1) {
        distance2StopLine = -distance2StopLine;
    }

    // 距离停止线30米内是否有刹车动作
    if (breakActive == 0) {
        if (distance2StopLine > 0 && distance2StopLine <= LASTEST_BREAK_POINT) {
            if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
                breakActive = 1;
            if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
                if (rec == NOT_ENTER) {
                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                }
                // 记录刹车，停车
                if (dobreak && !(rec & REDUCE_SPEED)) {
                    SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
                }
            } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
                RemoveTargetReduceRec(TargetReduceRec2, key);
            }
        }
        } else if (RoadMap.specialAreas[i].type == SCHOOL_AREA) {
            double distance1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
            double distance2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[1], RoadMap.roads[roadIndex].rightEdge);
            int key =  i;
            int rec = GetTargetReduceRec(TargetReduceRec2, key);

        if (distance2StopLine < 0 && breakActive != 1){
            breakActive = -1;
            // 不按规定减速，不合格
            DEBUG("不踩下刹车，不合格");
            switch (RoadMapList[index].type) {
                case THROUGH_INTERSECTION_MAP:
                    AddExamFault(41, rtkTime);
                    break;
                case TURN_LEFT_MAP:
                    AddExamFault(43, rtkTime);
                    break;
                case TURN_RIGHT_MAP:
                    AddExamFault(46, rtkTime);
                    break;
                case THROUGH_ZEBRA_CROSSING_MAP:
                    AddExamFault(48, rtkTime);
                    break;
                case THROUGH_SCHOOL_MAP:
            if (distance1 < -1e-3 && distance2 > 1e-3) {
                if (rec == NOT_ENTER) {
                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                }
                if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED && !(rec & OVER_SPEED)) {
                    SetTargetReduceRec(TargetReduceRec2, key, rec | OVER_SPEED);

                    DEBUG("通过学校区域超速");
                    AddExamFault(49, rtkTime);
                    break;
                case THROUGH_BUS_STATION_MAP:
                    AddExamFault(50, rtkTime);
                    break;
                default:
                    break;
                }
            } else if (rec != NOT_ENTER) {
                RemoveTargetReduceRec(TargetReduceRec2, key);
            }
        }
    }

    // 有停止标记的，是否停车（停止线5米内有停车动作）
    if (RoadMapList[index].flagStop != 0 && stopActive == 0) {
        if (distance2StopLine < 0.0) {
            // 不停车瞭望，不合格
            DEBUG("不停车瞭望");
            switch (RoadMapList[index].type) {
                case THROUGH_INTERSECTION_MAP:
                    AddExamFault(42, rtkTime);
                    break;
                case TURN_LEFT_MAP:
                    AddExamFault(44, rtkTime);
                    break;
                case TURN_RIGHT_MAP:
                    AddExamFault(47, rtkTime);
                    break;
                default:
                    break;
            }
            stopActive = -1;
        } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
            if (moveDirect == 0) {
                stopActive = 1;
            }
        }
    }

     if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) {
         if (!crashRedLine) {
             DEBUG("不按考试员指令行驶");
             crashRedLine = true;
             // 不按考试员指令行驶
             AddExamFault(3, rtkTime);
         }
     } else {
         crashRedLine = false;
     }

    if (ExitSonArea(index, RoadMapList, car)) {
        DEBUG("离开通过something区域");
        return -1;
    }

    return index;
}
#endif

void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
{
    RECHECK:
    for (auto it = TargetReduceRec.begin(); it != TargetReduceRec.end(); ++it) {
            int road = it->first / 100;
            int x = it->first % 100;

            if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
                DEBUG("越过路口 road %d 路口 %d", road, x);
                if (!(it->second & REDUCE_SPEED)) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");

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

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

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

            for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
                if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
                    break;
            }

            PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
                                                       RoadMap.specialAreas[x].area[1]);
            MakeLine(&line, &RoadMap.specialAreas[x].area[1], &point2);

            if (CrashTheLine(line, car, CarModelList)) {
                if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
                    !(it->second & REDUCE_SPEED)) {
                    DEBUG("不按规定减速");
                    AddExamFault(48, rtkTime);
                }
                if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
                    !(it->second & REDUCE_SPEED)) {
                    DEBUG("不按规定减速");
                    AddExamFault(50, rtkTime);
                }
                RemoveTargetReduceRec(TargetReduceRec2, it->first);
                goto RECHECK2;
            }
        }
}