路考

yy1717

2020-03-20 3a48a0de38910517352557510882f2ff4d8436ae

路考

 lib/src/main/cpp/CMakeLists.txt

@@ -39,6 +39,7 @@
        test_items2/common_check.cpp
        test_items2/dummy_light.cpp
        test_items2/road_exam.cpp
        test_items2/through_something.cpp

        rtk_module/rtk.cpp
        rtk_module/virtual_rtk.cpp

 lib/src/main/cpp/driver_test.cpp

@@ -252,6 +252,10 @@
            }
            free(map.triggerLine);
        }

        if (map.area.point != NULL) {
            free(map.area.point);
        }
    }

    RoadMapList.clear();
@@ -272,8 +276,13 @@
    }
}

void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines)
void AddRoadMapParent(int id, int type, string tts,
        vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas,
                      vector<vector<int>> &greenLines,
                      vector<vector<int>> &triggerLines,
                      vector<int> area,
                      vector<int> stopLine)
{
    struct road_exam_map newMap;

@@ -343,6 +352,25 @@
    } else {
        newMap.triggerLine = NULL;
    };

    if (area.size() > 0) {
        newMap.area.num = area.size();
        newMap.area.point = (PointF *) malloc(sizeof(PointF) * newMap.area.num);

        for (int i = 0; i < newMap.area.num; ++i) {
            newMap.area.point[i] = RoadMapPoints.point[area[i]];
        }
    } else {
        newMap.area.point = NULL;
        newMap.area.num = 0;
    }

    if (stopLine.size() == 4) {
        newMap.stopLine.X1 = stopLine[0];
        newMap.stopLine.Y1 = stopLine[1];
        newMap.stopLine.X2 = stopLine[2];
        newMap.stopLine.Y2 = stopLine[3];
    }

    RoadMapList.push_back(newMap);
}
@@ -681,7 +709,7 @@
        if (ExamType == TEST_TYPE_AREA)
            AddExamFault(4, rtkTime);
    }

DEBUG("22222222222222222222");
    if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
        if (exam_dummy_light == 0) {
            StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
@@ -886,7 +914,6 @@
    CarModelList.push_front(newModel);

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

        if (ptr->body != NULL)

 lib/src/main/cpp/driver_test.h

@@ -107,8 +107,8 @@
    int triggerLineNum;
    trigger_line_t *triggerLine;

    Line startLine;
    Line endLine;
    Polygon area;           // 子项目的区域
    Line stopLine;         // 诸如人行道、路口等考点的停止线
    int flagStop;           // 到达开始线前，是否需要停车
};

@@ -125,7 +125,8 @@
void CleanRoadMap(void);
void SetRoadMapPoints(vector<double> &mapPoints);
void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines);
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines,
                      vector<int> area, vector<int> stopLine);

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,

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

@@ -492,11 +492,15 @@
            if (!doc.HasParseError()) {
                CleanRoadMap();

                DEBUG("开始解析路考地图");

                vector<double> mapPoints;
                mapPoints.clear();

                if (doc.HasMember("points")) {
                    const Value &s = doc["points"];

                    DEBUG("得到所有点");
                    // X-Y坐标集合
                    for (Value::ConstValueIterator itr2 = s.Begin();
                             itr2 != s.End(); ++itr2) {
@@ -510,6 +514,8 @@
                    const Value &a = doc["maps"];

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

                        DEBUG("得到各子地图");
                        if (itr->IsObject()) {
                            // a Map
                            int id, type;
@@ -520,7 +526,11 @@
                            vector<vector<int>> greenLines;
                            vector<vector<int>> triggerLines;
                            vector<vector<int>> redAreas;
                            vector<int> area;
                            vector<int> stopLine;

                            stopLine.clear();
                            area.clear();
                            tts.clear();
                            redLines.clear();
                            greenLines.clear();
@@ -557,8 +567,8 @@
                                }
                            }

                            if (itr->HasMember("trigger_line")) {
                                const Value &s = (*itr)["trigger_line"];
                            if (itr->HasMember("all_trigger_line")) {
                                const Value &s = (*itr)["all_trigger_line"];

                                for (Value::ConstValueIterator itrLine = s.Begin();
                                     itrLine != s.End(); ++itrLine) {
@@ -585,6 +595,24 @@
                                }
                            }

                            if (itr->HasMember("area")) {
                                const Value &s = (*itr)["area"];

                                for (Value::ConstValueIterator itrPoint = s.Begin();
                                     itrPoint != s.End(); ++itrPoint) {
                                    area.push_back((*itrPoint).GetInt());
                                }
                            }

                            if (itr->HasMember("stop_line")) {
                                const Value &s = (*itr)["stop_line"];

                                for (Value::ConstValueIterator itrPoint = s.Begin();
                                     itrPoint != s.End(); ++itrPoint) {
                                    stopLine.push_back((*itrPoint).GetInt());
                                }
                            }

                            if (itr->HasMember("id")) {
                                const Value &s = (*itr)["id"];
                                id = s.GetInt();
@@ -605,7 +633,7 @@
                                tts = s.GetString();
                            }

                            AddRoadMapParent(id, type, tts, redLines, redAreas, greenLines, triggerLines);
                            AddRoadMapParent(id, type, tts, redLines, redAreas, greenLines, triggerLines, area, stopLine);
                        }
                    }
                }

 lib/src/main/cpp/test_common/car_sensor.h

@@ -19,6 +19,9 @@
    MAIN_BEAM_LAMP,
    SEATBELT,
    ENGINE_START,
    BREAK,
    HAND_BREAK,
    SECOND_BREAK,
    CAR_STATUS_END              //////////////
};

@@ -26,6 +29,7 @@
    OFF_LIGHT = 0,
    EJECT_SEATBELT = 0,
    ENGINE_START_INACTIVE = 0,
    BREAK_INACTIVE = 0,
    HAZARD_LIGHTS,
    LEFT_TURN_LIGHT,
    RIGHT_TURN_LIGHT,
@@ -42,7 +46,8 @@
    GEAR_2,
    GEAR_3,
    GEAR_4,
    GEAR_5
    GEAR_5,
    BREAK_ACTIVE
};

void CarSensorInit(void);

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

@@ -10,6 +10,7 @@
#include "../defs.h"
#include "../test_common/car_sensor.h"
#include "../native-lib.h"
#include "through_something.h"

#include <vector>
#include <list>
@@ -400,14 +401,38 @@
    if (currExamMapIndex == -1) {
        currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
        if (currExamMapIndex != -1) {
            DEBUG("进入路考子地图 index = %d id = %d item = %d", currExamMapIndex, RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type);
            if (!RoadMapList[currExamMapIndex].tts.empty()) {
                PlayTTS(RoadMapList[currExamMapIndex].tts.c_str(), 0);
            }
            StartThroughExam(currExamMapIndex, RoadMapList);
        }
    } else {

    }
}

bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car)
{
    bool ret = false;

    if (index < 0 || index > RoadMapList.size())
        return true;

    if (RoadMapList[index].area.point != NULL) {
        // 需要车身全部离开
        Polygon carBody;

        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]];
        }

        if (IntersectionOf(&carBody, &RoadMapList[index].area) == GM_None) {
            ret = true;
        }

        free(carBody.point);
    }

    return ret;
}

static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -494,7 +519,7 @@
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

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

            // 每条线都检测
            for (int j = 0; j < RoadMapList[i].redLineNum; ++j) {
@@ -536,7 +561,7 @@
    }

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

            // 每条红区都检测
            for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
@@ -566,7 +591,7 @@
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

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

            // 每条线都检测
            for (int j = 0; j < RoadMapList[i].greenLineNum; ++j) {
@@ -621,7 +646,7 @@
    }

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

            // 每条线都检测
            for (int j = 0; j < RoadMapList[i].triggerLineNum; ++j) {

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

@@ -7,7 +7,20 @@

#include "../driver_test.h"

#define GENERAL_MAP                     100
#define DRIVE_STRAIGHT_MAP              101
#define OP_GEAER_MAP                    102
#define STOP_CAR_MAP                    103
#define THROUGH_INTERSECTION_MAP        104
#define TURN_LEFT_MAP                   105
#define TURN_RIGHT_MAP                  106
#define THROUGH_ZEBRA_CROSSING_MAP      107
#define THROUGH_SCHOOL_MAP              108
#define THROUGH_BUS_STATION_MAP         109
#define TURN_AROUND_MAP                 110

void InitRoadExam(void);
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car);

#endif //MYAPPLICATION2_ROAD_EXAM_H

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

New file
@@ -0,0 +1,114 @@
//
// Created by YY on 2020/3/20.
//

#include "through_something.h"
#include "../driver_test.h"
#include "../Geometry.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../test_common/car_sensor.h"
#include "road_exam.h"

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

static const double LASTEST_BREAK_POINT = 30.0;
static const double DISTANCE_STOP_CAR_TO_STOP_LINE = 5.0;

static int breakActive;
static int stopActive;

void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
{
    if (index == -1)
        return;
    DEBUG("进入路考子地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
    if (!RoadMapList[index].tts.empty()) {
        PlayTTS(RoadMapList[index].tts.c_str(), 0);
    }
    breakActive = 0;
    stopActive = 0;
}

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;

    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 >= LASTEST_BREAK_POINT) {
            if (ReadCarStatus(BREAK) == BREAK_ACTIVE) {
                breakActive = 1;
            }
        } else {
            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:
                    AddExamFault(49, rtkTime);
                    break;
                case THROUGH_BUS_STATION_MAP:
                    AddExamFault(50, rtkTime);
                    break;
                default:
                    break;
            }
        }
    }

    // 有停止标记的，是否停车（停止线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;
            }
        } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
            if (moveDirect == 0) {
                stopActive = 1;
            }
        }
    }

    if (ExitSonArea(index, RoadMapList, car))
        return -1;

    return index;
}


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

New file
@@ -0,0 +1,14 @@
//
// Created by YY on 2020/3/20.
//

#ifndef MYAPPLICATION2_THROUGH_SOMETHING_H
#define MYAPPLICATION2_THROUGH_SOMETHING_H

#include "../driver_test.h"

void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList);
int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
                       LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_THROUGH_SOMETHING_H