进出科目二区域调整

yy1717

2020-04-02 24428a1b69afcf113883e9cede1159443201a1b8

 lib/src/main/cpp/driver_test.cpp

@@ -40,12 +40,6 @@

using namespace std;

#define MAP_TYPE_PARK_BUTTOM        1
#define MAP_TYPE_STOP_START         2
#define MAP_TYPE_PART_EDGE         3
#define MAP_TYPE_CURVE         4
#define MAP_TYPE_TURN_90         5

#define RTK_INVALID         0
#define RTK_SINGLE_POINT   1
#define RTK_DIFF            2
@@ -117,7 +111,8 @@
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);

static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum);
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
static int GetMapId(int index, const struct map_list *mapList, int mapNum);
static int GetMapType(int index, const struct map_list *mapList, int mapNum);
@@ -650,7 +645,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -712,18 +707,18 @@
    }
    if (CurrExamMapIndex < 0) {
        if (CurrEnterMapIndex < 0) {
            CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
            CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum);
            if (CurrEnterMapIndex >= 0) {
                DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
//                DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);

                CurrExamMapIndex = CurrEnterMapIndex;
                CurrExamStatus = EXAM_AREA_START;
            }
        } else {
            if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
                DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
//                DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                CurrEnterMapIndex = -1;
            }
        }
@@ -894,33 +889,33 @@
                    DEBUG("进入倒车入库场地");
                    MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                    StartParkBottom(move, rtkTime);
                    StartParkBottom(CurrExamMapIndex, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地");
                    MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartSAS(move, rtkTime);
                    StartSAS(CurrExamMapIndex, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:
                    DEBUG("进入侧方位停车场地");
                    MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartParkEdge(move, rtkTime);
                    StartParkEdge(CurrExamMapIndex, move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_CURVE:
                    DEBUG("进入曲线行驶场地");
                    MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartDrivingCurve(move, rtkTime);
                    StartDrivingCurve(CurrExamMapIndex, move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_TURN_90:
                    DEBUG("进入直角转弯场地");
                    MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartTurnA90(move, azimuth, rtkTime);
                    StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                default:break;
@@ -1145,15 +1140,79 @@
    return true;
}

static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    // 车的最前点是否进入地图
    bool trigger = false;

    if (CarModelList.size() < 5)
        return trigger;

    Polygon trace, trace2;
    int pn = 0;

    trace2.num = trace.num = 5;
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
    trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);

    list<car_model *>::iterator iter = CarModelList.begin();

    car_model *c1 = *iter;

    trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
    trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];

    ++iter;

    while (iter != CarModelList.end() && pn < trace.num) {
        car_model *c2 = *iter;

        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= D_SEC(1)) {
            trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
            trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
            c1 = c2;
        }
        ++iter;
    }

    PointF p1, p2;

    p1.X = triggerLine.X1;
    p1.Y = triggerLine.Y1;
    p2.X = triggerLine.X2;
    p2.Y = triggerLine.Y2;

    int pp = 0;
    for (int p = 1; p < pn; ++p) {
        Line trace_line, trace2_line;

        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);

        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
            IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
            // 碰到触发线
            DEBUG("碰撞触发线 引发地图");
            trigger = true;
            goto SEARCH_TRIGGER_LINE_END;
        }
    }

SEARCH_TRIGGER_LINE_END:
    free(trace.point);

    return trigger;
}

static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum)
{
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        // 车前轮或后轮轨迹越过触发线
        if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            bool enter = false;

            xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
            yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
@@ -1161,31 +1220,18 @@
            x9 = 2*xo - mapList[i].map.point[8].X;
            y9 = 2*yo - mapList[i].map.point[8].Y;

            Polygon map;
            Line triggerLine;

            map.num = 4;
            map.point = (PointF *) malloc(map.num * sizeof(PointF));
            triggerLine.X1 = mapList[i].map.point[0].X;
            triggerLine.Y1 = mapList[i].map.point[0].Y;
            triggerLine.X2 = x9;
            triggerLine.Y2 = y9;

            map.point[0] = mapList[i].map.point[0];
            map.point[1] = mapList[i].map.point[8];
            map.point[2] = mapList[i].map.point[7];
            map.point[3].X = x9;
            map.point[3].Y = y9;

            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
                Line enterLine1;

                MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    enter = true;
            }

            free(map.point);

            if (enter) return i;
            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            // 车头顶点在场地内
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1, enterLine2;

@@ -1198,6 +1244,7 @@
            }
        }
        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
            // 车头顶点在场地内
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine;

@@ -1208,26 +1255,20 @@
            }
        }
        if (mapList[i].type == MAP_TYPE_TURN_90) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1;
            // 车前轮或后轮轨迹越过触发线
            Line triggerLine;

                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
            MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    return i;
            }
            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            Line startLine;
            Line carAxial;
            Line triggerLine;

            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
            MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);

            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
                if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
                    return i;
            }
            MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
    }
    return -1;