坐标

yy1717

2020-08-24 fb7b0660a319a9c54ff35c3944548348fae11b60

坐标

 lib/src/main/cpp/driver_test.cpp

@@ -779,6 +779,10 @@
    // 计算速度(米/秒)、前进后退
    double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
                (double)(tmDiff);

//    DEBUG("位移 %f 时间 %ld 速度 %f", sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)), tmDiff, speed);
//    DEBUG("%d %d %f, %f - %d %d %f, %f", RtkBuffer[p1].ss, RtkBuffer[p1].dss, RtkBuffer[p1].x, RtkBuffer[p1].y, RtkBuffer[p2].ss, RtkBuffer[p2].dss, RtkBuffer[p2].x, RtkBuffer[p2].y);

    int move = 0;
    double deg = 0.0;


 lib/src/main/cpp/driver_test.h

@@ -96,6 +96,7 @@
#define LINE_SOLID             1
#define LINE_HALF_SOLID_LEFT        2
#define LINE_HALF_SOLID_RIGHT       3
#define LINE_BOUNDARY               4

//车道方向（按位组合）,如果为0，则表无车道方向说明；
#define LANE_FORWARD    0x01
@@ -140,6 +141,7 @@
    string tts;
    bool stopFlag;
    Line line;
    PointF centrePoint;
} stop_line_t;

typedef struct {
@@ -184,10 +186,17 @@
    std::vector<PointF> leftPoints; // 对应到道路左侧的点
} trigger_line_t;

typedef struct {
    int id;
    int type;
    std::vector<PointF> points;
} forbid_line_t;

struct road_exam_map {
    std::vector<road_t> roads;
    std::vector<special_area_t> specialAreas;
    std::vector<trigger_line_t> triggerLines;
    std::vector<forbid_line_t> forbidLines;
};

struct area_exam_map {

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

@@ -918,7 +918,11 @@
                                            MakeLine(&temp.line, &p1, &p2);
                                        }
                                    }
                                    if (itr2->HasMember("center_point")) {
                                        const Value &s = (*itr2)["center_point"];

                                        temp.centrePoint = mapPoints[s.GetInt()];
                                    }
                                    crossing.push_back(temp);
                                }

@@ -1136,6 +1140,33 @@
                        map.triggerLines.push_back(trigger);
                    }
                }
                if (doc.HasMember("red_line")) {
                    const Value &a = doc["red_line"];

                    for (Value::ConstValueIterator itr = a.Begin();
                         itr != a.End(); ++itr) {
                        forbid_line_t forbid;

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

                            for (Value::ConstValueIterator itr2 = a2.Begin();
                                 itr2 != a2.End(); ++itr2) {
                                forbid.points.push_back(mapPoints[(*itr2).GetInt()]);
                            }
                        }
                        map.forbidLines.push_back(forbid);
                    }
                }

                DEBUG("地图解析完毕");

                CleanRoadMap();

 lib/src/main/cpp/test_items2/drive_straight.cpp

@@ -45,17 +45,22 @@
            yaw += car->yaw;
        }
        yaw_stat++;

        DEBUG("角度矫正 car %f yaw %f", car->yaw, yaw);

        if (yaw_stat == 5) {
            yaw = fmod(yaw, 360) / 5;

            DEBUG("创建基线 yaw %f", yaw);
            PointF extPoint = PointExtend(car->basePoint, 100, yaw);
            MakeLine(&baseLine, &car->basePoint, &extPoint);
            beginOdo = ReadOdo();
            setup = 2;
        }
    } else if (setup == 2) {
        if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine) > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米");
        double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);

        if (offset > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米 offset = %f", offset);
            // 偏移大于30厘米，不合格
            AddExamFault(30, rtkTime);
            return false;

 lib/src/main/cpp/test_items2/road_exam.cpp

@@ -79,8 +79,8 @@


static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;

static bool StopCarOnRedArea;
static PointF stopPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;
@@ -89,7 +89,7 @@
static bool slideNormalDistance;
static bool occurSlide;

static struct RtkTime crashGreenRunTime, crashGreenStartTime;
static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;

static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
@@ -150,6 +150,7 @@
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);

static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);

@@ -169,7 +170,7 @@

    prevMoveDirect = 0;

    reportStopCarOnRedArea = false;
    StopCarOnRedArea = false;
    occurSlide = false;
    slideLongDistance = false;
    slideNormalDistance = false;
@@ -611,6 +612,47 @@
        }
    }

    for (int n = 0; n < RoadMap.forbidLines.size(); ++n) {
        // 地图围栏
        if (RoadMap.forbidLines[n].type == LINE_BOUNDARY && track && RoadMap.forbidLines[n].points.size() >= 2) {
            p1 = RoadMap.forbidLines[n].points[0];
            for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
                p2 = RoadMap.forbidLines[n].points[m];
                MakeLine(&redLine, &p1, &p2);
                if (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                     IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection) {
                    lineType = LINE_BOUNDARY;
                    goto CRASH_LINE_CONFIRM;
                }
                p1 = p2;
            }
        }
        // 其它红线
        if (RoadMap.forbidLines[n].type == LINE_SOLID && RoadMap.forbidLines[n].points.size() >= 2) {
            p1 = RoadMap.forbidLines[n].points[0];
            for (int m = 1; m < RoadMap.forbidLines[n].points.size(); ++m) {
                p2 = RoadMap.forbidLines[n].points[m];
                MakeLine(&redLine, &p1, &p2);

                if (IntersectionOf(redLine, frontTireAxle) == GM_Intersection ||
                    IntersectionOf(redLine, rearTireAxle) == GM_Intersection) {
                    lineType = LINE_SOLID;
                    goto CRASH_LINE_CONFIRM;
                }

                if (track &&
                        (IntersectionOf(redLine, frontLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, frontRightTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearLeftTireTrack) == GM_Intersection ||
                        IntersectionOf(redLine, rearRightTireTrack) == GM_Intersection)) {
                    lineType = LINE_SOLID;
                    goto CRASH_LINE_CONFIRM;
                }
                p1 = p2;
            }
        }
    }

CRASH_LINE_CONFIRM:
    if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) {
        if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) {
@@ -826,6 +868,12 @@
        DEBUG("撞道路边缘线");
        AddExamFault(11, rtkTime);
    }
    if (newLineType == LINE_BOUNDARY && CrashLineType != LINE_BOUNDARY) {
        // 车辆越界，逃跑了，不合格
        DEBUG("车辆越界");
        AddExamFault(3, rtkTime);
    }

    if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
        if (!crashDottedLine) {
            checkCrashGreenTimeout = 0;
@@ -1163,7 +1211,7 @@

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double BigStraightRoadFree = 0, RoadCrossingFree = 0;
    double BigStraightRoadFree = 0, RoadCrossingFree = 0, TargetFree = 0;

    UpdateCarSensor(rtkTime);

@@ -1269,8 +1317,8 @@
    // 起步后滑
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarOnRedArea = false;
            stopTimepoint = *rtkTime;
            StopCarOnRedArea = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

@@ -1291,14 +1339,12 @@
        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)) {
        if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea && StopOnRedArea(RoadMap, car)) {
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("禁停区停车");
            reportStopCarOnRedArea = true;
        }*/
            StopCarOnRedArea = true;
        }
    } else if (moveDirect == -1) {
        // 持续后滑
        if (occurSlide) {
@@ -1336,12 +1382,12 @@
    if (currExamMapIndex >= 0) {
        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
        // 检测通过路口、人行道等区域时，释放刹车或减速
        ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
        TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
    }
    ExitTarget(RoadMap, car, CarModelList, rtkTime);

    oldid = CrashLineType;

    // 检测压线状态
    DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);

    if (oldid != CrashLineType) {
@@ -1385,15 +1431,13 @@

        RoadCrossingFree = freeSepDis;

//        DEBUG("累计行驶距离 %f, 直道剩余距离 %f, 车道剩余距离 %f", ReadOdo(), BigStraightRoadFree, RoadCrossingFree);
//        DEBUG("车速 %f, 累计行驶距离 %f, 直道剩余距离 %f, 车道剩余距离 %f 特殊区域距离 %f", ConvertMs2KMh(speed), ReadOdo(), BigStraightRoadFree, RoadCrossingFree, TargetFree);
    }

    // 检测压线状态

    // 额外的转向检测
    DetectTurn(car, moveDirect, rtkTime);

    ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);
    ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
}

static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
@@ -1643,39 +1687,62 @@

/**********************************************************
 * 按整个车身是否覆盖计算
 * @param RoadMapList
 * @param RoadMap
 * @param car
 * @return
 */
/*static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car)
{
    bool ret = false;
    for (int i = 0; i < RoadMap.specialAreas.size(); ++i) {
        Polygon redArea;
        redArea.num = 0;

    Polygon carBody;
        if (RoadMap.specialAreas[i].type == GRID_AREA && RoadMap.specialAreas[i].area.size() == 4) {
            redArea.num = 4;
            redArea.point = (PointF *)malloc(4 * sizeof(PointF));

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    for (int i = 0; i < RoadMapList.size(); ++i) {
        if (RoadMapList[i].type == GENERAL_MAP) {

            // 每条红区都检测
            for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
                if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
                    ret = true;
                }
            }
            redArea.point[0] = RoadMap.specialAreas[i].area[0];
            redArea.point[1] = RoadMap.specialAreas[i].area[1];
            redArea.point[2] = RoadMap.specialAreas[i].area[2];
            redArea.point[3] = RoadMap.specialAreas[i].area[3];
        } else if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING) {
            int j = 0;
            for (; j < RoadMap.roads.size(); ++j) {
                if (RoadMap.specialAreas[i].road == RoadMap.roads[j].id) {
            break;
        }
    }

    free(carBody.point);
            if (j < RoadMap.roads.size()) {

    return ret;
}*/
                PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
                                                       RoadMap.specialAreas[i].area[0]);
                PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[j].leftEdge,
                                                       RoadMap.specialAreas[i].area[1]);

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

                redArea.point[0] = RoadMap.specialAreas[i].area[0];
                redArea.point[1] = RoadMap.specialAreas[i].area[1];
                redArea.point[2] = p2;
                redArea.point[3] = p1;
            }
        }

        if (redArea.num > 0) {
            for (int i = 0; i < car->bodyNum; ++i) {
                if (IntersectionOf(car->carXY[car->body[i]], &redArea) == GM_Containment) {
                    free(redArea.point);
                    return true;
                }
            }
            free(redArea.point);
        }
    }

    return false;
}

bool CrashTheLine(Line line, const car_model *car, LIST_CAR_MODEL &CarModelList)
{

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

@@ -66,10 +66,12 @@
    TargetReduceRec2.clear();
}

void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    vector<double> nearbyTarget;        // 学校、公交站、人行道区域的距离

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;
        return 100000;
    // 路口
    for (int i = 0; i < RoadMap.roads[roadIndex].stopLine.size(); ++i) {
        PointF point;
@@ -106,12 +108,19 @@
            continue;

        if (RoadMap.specialAreas[i].type == ZEBRA_CROSSING || RoadMap.specialAreas[i].type == BUS_STATION_AREA) {
            double distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.specialAreas[i].area[0], RoadMap.roads[roadIndex].rightEdge);
            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 (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
            if (distance1 < -1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(0);
            } else if (distance1 > 1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(distance1);
            }

            if (distance1 > 1e-3 && distance1 < LASTEST_BREAK_POINT) {
                if (rec == NOT_ENTER) {
                    SetTargetReduceRec(TargetReduceRec2, key, ENTER_Z);
                }
@@ -119,32 +128,47 @@
                if (dobreak && !(rec & REDUCE_SPEED)) {
                    SetTargetReduceRec(TargetReduceRec2, key, rec | REDUCE_SPEED);
                }
            } else if (distance > LASTEST_BREAK_POINT + 5 && rec != NOT_ENTER) {
            } else if (distance1 > 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 (distance1 < -1e-3 && distance2 > 1e-3) {
                nearbyTarget.push_back(0);
                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("通过学校区域超速");
                    DEBUG("通过学校区域超速 %f kmh", ConvertMs2KMh(speed));
                    AddExamFault(49, rtkTime);
                }
            } else if (rec != NOT_ENTER) {
            } else if (distance1 < -1e-3 && distance2 < -1e-3) {
                if (rec != NOT_ENTER) {
                    RemoveTargetReduceRec(TargetReduceRec2, key);
                }
            } else {
                nearbyTarget.push_back(distance1);
                if (rec != NOT_ENTER) {
                RemoveTargetReduceRec(TargetReduceRec2, key);
            }
        }
    }
}

    if (nearbyTarget.size() > 0) {
        sort(nearbyTarget.begin(), nearbyTarget.end());
        return nearbyTarget[0];
    }
    return 100000;
}

void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime)
{
    RECHECK:

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

@@ -8,7 +8,7 @@
#include "../driver_test.h"

void ResetTarget(road_exam_map &RoadMap);
void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime);
double ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, bool dobreak, double speed, int moveDirect, const struct RtkTime *rtkTime);
void ExitTarget(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, const struct RtkTime *rtkTime);