更新地图

yy1717

2020-03-16 4053e385bb8dc70ad8c8d12eeaec9c10ee87fcb9

 lib/src/main/cpp/driver_test.cpp

@@ -11,6 +11,10 @@
#include <pthread.h>
#include <cstring>
#include <vector>
#include <list>
#include <numeric>
#include <algorithm>

#include "driver_test.h"
#include "defs.h"
#include "Geometry.h"
@@ -84,6 +88,43 @@
    Polygon map2;
} MapList[MAP_LIST_SIZE];

struct trigger_line {
    int mapId;
    Line tirgger;
};

struct road_exam_parent_map {
    int id;
    int type;
    Polygon map;
    int redLineNum;
    Polygon *redLine;
    int greenLineNum;
    Polygon *greenLine;
    int redAreaNum;
    Polygon *redArea;
    int triggerLineNum;
    struct trigger_line *triggerLine;
};

static struct road_exam_parent_map * RoadParentMap;

struct road_exam_son_map {
    int id;
    int type;
    char tts[512];
    Line triggerLine;
    Line controlLine;
    Line targetLine;
    int stop_flag;

    int redLineNum;
    Polygon *redLine;
};

typedef list<struct road_exam_son_map *> LIST_ROAD_SON_MAP;
static LIST_ROAD_SON_MAP RoadSonMapList;

static int MapNum = 0;
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;
@@ -92,7 +133,10 @@
static int exam_dummy_light;

static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;

typedef list<car_model *> LIST_CAR_MODEL;

static LIST_CAR_MODEL CarModelList;

static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
@@ -128,7 +172,10 @@
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;
    CarModelList.clear();

    RoadParentMap = NULL;
    RoadSonMapList.clear();

    CarSensorInit();

@@ -205,6 +252,8 @@
    DEBUG("AddMap num %d", MapNum);
}



void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
@@ -223,24 +272,6 @@
        free(CarModel);
        CarModel = NULL;
    }

    if (CarModelPrev != NULL) {
        if (CarModelPrev->body != NULL)
            free(CarModelPrev->body);
        if (CarModelPrev->carDesc != NULL)
            free(CarModelPrev->carDesc);
        if (CarModelPrev->carXY != NULL)
            free(CarModelPrev->carXY);
        free(CarModelPrev);
        CarModelPrev = NULL;
    }

    // 仅分配carXY, body和carDesc均没有
    CarModelPrev = (car_model *)malloc(sizeof(car_model));
    CarModelPrev->body = NULL;
    CarModelPrev->carDesc = NULL;
    CarModelPrev->pointNum = 0;
    CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);

    CarModel = (car_model *)malloc(sizeof(car_model));
    CarModel->basePoint.X = basePoint[0];
@@ -623,19 +654,19 @@
            switch (mtype) {
                case MAP_TYPE_PARK_BUTTOM:
                    testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
                                             CarModel, CarModelPrev, speed, move, rtkTime);
                                             CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_STOP_START:
                    testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
                    testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_PART_EDGE:
                    testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
                    testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_CURVE:
                    testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, rtkTime);
                    testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_TURN_90:
                    testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, rtkTime);
                    testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, NULL, azimuth, speed, move, rtkTime);
                    break;
                default:
                    break;
@@ -715,21 +746,51 @@
    main_ant_coord.X = RtkBuffer[p1].x;
    main_ant_coord.Y = RtkBuffer[p1].y;

    // 保存上一个车身轮廓点
    if (CarModelPrev->pointNum > 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }
    UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
    if (CarModelPrev->pointNum == 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }

    car_model *newModel = (car_model *)malloc(sizeof(car_model));

    newModel->basePoint = CarModel->basePoint;
    newModel->axial[0] = CarModel->axial[0];
    newModel->axial[1] = CarModel->axial[1];
    newModel->left_front_tire[0] = CarModel->left_front_tire[0];
    newModel->left_front_tire[1] = CarModel->left_front_tire[1];
    newModel->right_front_tire[0] = CarModel->right_front_tire[0];
    newModel->right_front_tire[1] = CarModel->right_front_tire[1];
    newModel->left_rear_tire[0] = CarModel->left_rear_tire[0];
    newModel->left_rear_tire[1] = CarModel->left_rear_tire[1];
    newModel->right_rear_tire[0] = CarModel->right_rear_tire[0];
    newModel->right_rear_tire[1] = CarModel->right_rear_tire[1];
    newModel->bodyNum = CarModel->bodyNum;
    newModel->body = (int *) malloc(sizeof(int) * newModel->bodyNum);
    for (int i = 0; i < newModel->bodyNum; ++i) {
        newModel->body[i] = CarModel->body[i];
    }
    newModel->pointNum = CarModel->pointNum;
    newModel->carXY = (PointF *) malloc(sizeof(PointF) * newModel->pointNum);
    for (int i = 0; i < newModel->pointNum; ++i) {
        newModel->carXY[i] = CarModel->carXY[i];
    }
    newModel->carDesc = NULL;
    newModel->antPitch = CarModel->antPitch;
    newModel->yaw = CarModel->yaw;
    newModel->pitch = CarModel->pitch;

    CarModelList.push_front(newModel);
    while (CarModelList.size() > 25) {
        DEBUG("删除尾结点");
        car_model *ptr = CarModelList.back();

        if (ptr->body != NULL)
            free(ptr->body);
        if (ptr->carXY != NULL)
            free(ptr->carXY);
        if (ptr->carDesc != NULL)
            free(ptr->carDesc);

        free(ptr);

        CarModelList.pop_back();
    }

    // 计算速度(米/秒)、前进后退
@@ -801,77 +862,74 @@
{
    // 车的最前点是否进入地图
    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;
//
//            x9 = 2*xo - mapList[i].map.point[8].X;
//            y9 = 2*yo - mapList[i].map.point[8].Y;
//
//            Polygon map;
//
//            map.num = 4;
//            map.point = (PointF *) malloc(map.num * sizeof(PointF));
//
//            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 (mapList[i].type == MAP_TYPE_CURVE) {
//
//        }
//        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1, enterLine2;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
//                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
//                    return i;
//            }
//        }
//        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
//
//                MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
//                    return i;
//            }
//        }
//        if (mapList[i].type == MAP_TYPE_TURN_90) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
//                    return 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;

            x9 = 2*xo - mapList[i].map.point[8].X;
            y9 = 2*yo - mapList[i].map.point[8].Y;

            Polygon map;

            map.num = 4;
            map.point = (PointF *) malloc(map.num * sizeof(PointF));

            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 (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1, enterLine2;

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

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
                    return i;
            }
        }
        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine;

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

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
                    return i;
            }
        }
        if (mapList[i].type == MAP_TYPE_TURN_90) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1;

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

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    return i;
            }
        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            Line startLine;
            Line carAxial;
@@ -1008,6 +1066,9 @@
{
    carModel->basePoint = main_ant;

    carModel->yaw = azimuth;
    carModel->pitch = pitch;

    pitch = pitch - carModel->antPitch;

    for (int i = 0; i < carModel->pointNum; ++i) {