更新地图

yy1717

2020-03-16 4053e385bb8dc70ad8c8d12eeaec9c10ee87fcb9

更新地图

 Documents/Ä£Äâ¼Ý¿¼APPÄÚ²¿Í¨Ñ¶.docx

Binary files differ

 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) {

 lib/src/main/cpp/driver_test.h

@@ -38,7 +38,9 @@
    int bodyNum;
    int *body;
    int pointNum;
    double antPitch;
    double antPitch;                // å‰åŽå¤©çº¿å› å®‰è£…位置之固有pitch角
    double yaw;
    double pitch;
    struct car_desc_ *carDesc;      // ç»çŽ°åœºæµ‹é‡ç‚¹è®¡ç®—后的值
    PointF *carXY;                  // è®¡ç®—后的车身各点坐标
} car_model;

 lib/src/main/cpp/master/comm_if.cpp

@@ -35,6 +35,8 @@
#define ID_SM_RTK_PLAT_LOGIN     0x0005
#define ID_SM_READ_MAP          0x0006
#define ID_MS_MAP               0x8006
#define ID_MS_ROAD_MAP          0x8013

#define ID_SM_READ_CAR          0x0007
#define ID_MS_CAR               0x8007
#define ID_SM_MCU_BRIEF   0x0008
@@ -483,6 +485,70 @@
            ConfigPlatform(&cfg);
            break;
        }
        case ID_MS_ROAD_MAP: {
            Document doc;
            doc.Parse(value);

            if (!doc.HasParseError()) {
                const Value &a = doc.GetArray();
                if (a.IsArray()) {
                    for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
                        // a Map
                        int id, type, pointNum = 0;
                        double (*map)[2] = NULL;
                        int *line = NULL;

                        if (itr->IsObject()) {
                            if (itr->HasMember("id")) {
                                const Value &s = (*itr)["id"];
                                id = s.GetInt();
                            }

                            if (itr->HasMember("item")) {
                                const Value &s = (*itr)["item"];
                                type = s.GetInt();
                            }

                            if (itr->HasMember("map")) {
                                const Value &s = (*itr)["map"];
                                int i = 0, j = 0;

                                pointNum = s.Size() / 2;
                                map = (double (*)[2]) new double[pointNum][2];

                                for (Value::ConstValueIterator itr2 = s.Begin();
                                     itr2 != s.End(); ++itr2) {
                                    map[i][j] = (*itr2).GetDouble();
                                    if (++j == 2) {
                                        j = 0;
                                        i++;
                                    }
                                }
                            }

                            AddMap(id, type, map, pointNum, redLineNum, redLine, greenLine, redArea, triggerLine);

                            if (itr->HasMember("red_line")) {
                                const Value &s = (*itr)["red_line"];
                                for (Value::ConstValueIterator itr2 = s.Begin();
                                     itr2 != s.End(); ++itr2) {
                                    // å¯ä»¥è¿žæŽ¥ä¸ºä¸€æ¡çº¿çš„点
                                    const Value &s2 = (*itr2)["line"];

                                    lineNum = s2.Size() / 2;

                                    for (Value::ConstValueIterator itr3 = s2.Begin();
                                         itr3 != s2.End(); ++itr3) {
                                        line[x++] = (*itr3).GetInt();
                                    }
                                }
                            }
                        }
                    }
                }
            }
            break;
        }
        case ID_MS_MAP: {
            Document doc;
            doc.Parse(value);

 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -200,7 +200,7 @@
        }*/

        if (RxBufLen > 0) {
#if 1
#if 0
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));