坐标

yy1717

2020-03-21 1101dc614acb3cd794c13cd68c9a24d7c353cb28

坐标

 lib/src/main/cpp/driver_test.cpp

@@ -277,7 +277,7 @@
    }
}

void AddRoadMapParent(int id, int type, string tts,
void AddRoadMapParent(int id, int type, string tts, int stopFlag,
        vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas,
                      vector<vector<int>> &greenLines,
@@ -290,8 +290,13 @@
    newMap.id = id;
    newMap.type = type;
    newMap.tts = tts;
    newMap.flagStop = stopFlag;

    DEBUG("加入路考地图信息 id = %d type = %d", id, type);

    if (!tts.empty()) {
        DEBUG("TTS 信息 = %s", tts.c_str());
    }

    if ((newMap.redLineNum = redLines.size()) > 0) {
        newMap.redLine = (Polygon *) malloc(sizeof(Polygon) * newMap.redLineNum);
@@ -357,7 +362,7 @@
            newMap.triggerLine[i].line.num = triggerLines[i].size() - 1;
            newMap.triggerLine[i].line.point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].line.num);

            DEBUG("    结点 %d 个", newMap.triggerLine[i].line.num);
            DEBUG("    结点 %d 个, 触发 id = %d", newMap.triggerLine[i].line.num, triggerLines[i][0]);

            newMap.triggerLine[i].triggerMapId = triggerLines[i][0];
            for (int j = 0; j < newMap.triggerLine[i].line.num; ++j) {
@@ -497,7 +502,7 @@
{
    DEBUG("获取模拟路考灯光测试项目 N = %d %d", n, ExamStart);

//    if (ExamStart) return;
    if (ExamStart) return;

    if (DummyLightContent != NULL) {
        free(DummyLightContent);
@@ -538,8 +543,8 @@
    }
    if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
        DEBUG("没有模拟灯光");
        err = true;
        MA_SendExamStatus(0, -3);
//        err = true;
//        MA_SendExamStatus(0, -3);
    }
    if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) {
        DEBUG("没有路考地图");
@@ -559,6 +564,8 @@

            if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                exam_dummy_light = 0;

//                InitRoadExam();         ////////////////////
            }
        }
        MA_SendExamStatus(1, 0);
@@ -733,16 +740,18 @@
        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);
            exam_dummy_light = 1;
            // 汇报灯光考试开始
            DEBUG("灯光考试开始");
        } else if (exam_dummy_light == 1) {
            exam_dummy_light = ExecuteDummyLightExam(rtkTime);
            // 汇报灯光考试结束
            if (exam_dummy_light == 2) {
                DEBUG("灯光考试结束");
                InitRoadExam();
            }
        }
@@ -912,6 +921,7 @@
    tm.DD = RtkBuffer[p1].DD;
    tm.hh = RtkBuffer[p1].hh;
    tm.mm = RtkBuffer[p1].mm;
    tm.ss = RtkBuffer[p1].ss;
    tm.mss = RtkBuffer[p1].dss;

    UpdateCarBodyCoord(&tm, RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
@@ -1210,6 +1220,11 @@
    fault.sn = examFaultIndex++;
    sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
            rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

    if (ExamType != TEST_TYPE_AREA) {
        wrong += 1000;
    }

    fault.wrong_id = wrong;

    DEBUG("考试发生错误 %d %s", wrong, fault.utc);

 lib/src/main/cpp/driver_test.h

@@ -124,7 +124,7 @@
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
void CleanRoadMap(void);
void SetRoadMapPoints(vector<double> &mapPoints);
void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines,
                      vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines,
                      vector<int> area, vector<int> stopLine);


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

@@ -633,7 +633,7 @@
                                tts = s.GetString();
                            }

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

 lib/src/main/cpp/native-lib.cpp

@@ -26,7 +26,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};

const char *VIRTUAL_RTK_IP = "192.168.3.52";
const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9001;

static void SendBootIndicate(union sigval sig);

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

@@ -11,6 +11,7 @@
#include "../test_common/car_sensor.h"
#include "../native-lib.h"
#include "through_something.h"
#include "../master/comm_if.h"

#include <vector>
#include <list>
@@ -21,10 +22,10 @@

using namespace std;

#define TURN_CHECK_CNT          5
#define TURN_CHECK_CNT          4

static const int TURN_THRESHOLD = 10;
static const int TURN_CHECK_INTERVAL = D_SEC(1);
static const int TURN_THRESHOLD = 5;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;

@@ -83,6 +84,8 @@

void InitRoadExam(void)
{
    DEBUG("Start road_exam");

    crashGreenCmpTime.hour = -1;
    occurCrashRedLine = false;
    occurCrashGreenLine = false;
@@ -209,9 +212,9 @@
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
            // 停车超2秒，停在红区
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("中途停车");
            DEBUG("禁停区停车");
            reportStopCarOnRedArea = true;
        }
    } else if (moveDirect == -1) {
@@ -263,6 +266,7 @@
    // 检查是否持续转向
    char turnDirect = CheckCarTurn(CarModelList);
    if (turnDirect == 'L') {
        PlayTTS("左1", 5692);
        if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打左转灯");
@@ -280,6 +284,7 @@
            }
        }
    } else if (turnDirect == 'R') {
        PlayTTS("右1", 5692);
        if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打右转灯");
@@ -304,6 +309,7 @@
    if (CrashRedLine(RoadMapList, car)) {
        if (!occurCrashRedLine) {
            // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
            DEBUG("撞道路边缘线");
            AddExamFault(11, rtkTime);
            occurCrashRedLine = true;
        }
@@ -324,6 +330,7 @@
                        crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);

                if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
                    DEBUG("长时间压虚线");
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶，不合格
                    AddExamFault(12, rtkTime);
@@ -368,6 +375,7 @@
                                                crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10);

                    if (diff < CHANGE_ROAD_MIN_INTERVAL) {
                        DEBUG("===================== 连续变道 ============!!");
                        // 连续变道，不合格
                        AddExamFault(15, rtkTime);
                    }
@@ -378,16 +386,20 @@

                // 检查变道前，是否提前转向灯
                if (inter == 1) {
                    PlayTTS("左2", 5698);
                    // 向左侧变道
                    DEBUG("向左侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    }
                } else {
                    PlayTTS("右2", 5698);
                    // 向右侧变道
                    DEBUG("向右侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    }
@@ -402,10 +414,20 @@
    if (currExamMapIndex == -1) {
        currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
        if (currExamMapIndex != -1) {
            DEBUG("碰撞触发线");

            MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);
            StartThroughExam(currExamMapIndex, RoadMapList);
        }
    } else {
        int prevIdx = currExamMapIndex;

        currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
        CarModelList, speed, moveDirect, rtkTime);

        if (currExamMapIndex == -1) {
            MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
        }
    }
}

@@ -436,6 +458,61 @@
    return ret;
}

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

    if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size())
        return ret;

    Polygon trace;

    trace.num = 5;          // 最近5个轮轨迹
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);

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

    int pn = 0;
    while (iter != CarModelList.end() && pn < trace.num) {
        trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        ++iter;
    }

    // 每条线都检测
    for (int j = 0; j < RoadMapList[index].redLineNum; ++j) {
        Line red_line;

        int kp = 0;

        // 触发线一般应该只有首尾2点（id, p1, p2）
        for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) {
            MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp],
                     &RoadMapList[index].redLine[j].point[k]);

            int pp = 0;
            for (int p = 1; p < pn; ++p) {
                Line trace_line;
                MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);

                if (IntersectionOf(trace_line, red_line) == GM_Intersection) {
                    // 碰到触发线
                    ret = true;
                    goto SEARCH_SON_RED_LINE_END;
                }

                pp = p;
            }

            kp = k;
        }
    }

    SEARCH_SON_RED_LINE_END:
    free(trace.point);

    return ret;
}

static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
    tm.hour = rtkTime->hh;
@@ -446,18 +523,24 @@

static char isTurn(int currYaw, int prevYaw)
{
//    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);

    int deltaAng = 0;

    if (ABS(currYaw - prevYaw) > 180) {
        currYaw = 360 - ABS(currYaw-prevYaw);
        deltaAng = 360 - ABS(currYaw-prevYaw);
    } else {
        currYaw = ABS(currYaw - prevYaw);
        deltaAng = ABS(currYaw - prevYaw);
    }

    if (currYaw >= TURN_THRESHOLD) {
//    DEBUG("角度差值 %d", deltaAng);

    if (deltaAng >= TURN_THRESHOLD) {
        if((( currYaw + 360 - prevYaw) % 360) < 180) {
            DEBUG("右转");
//            DEBUG("右转");
            return 'R';
        } else {
            DEBUG("左转");
//            DEBUG("左转");
            return 'L';
        }
    }
@@ -480,17 +563,22 @@
    char turn[TURN_CHECK_CNT] = {0};
    int checkCnt = 0;

    while (iter != CarModelList.end()) {
//    DEBUG("CheckCarTurn.........");

    while (iter != CarModelList.end() && checkCnt < TURN_CHECK_CNT) {
        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 >= TURN_CHECK_INTERVAL) {
            turn[checkCnt++] = isTurn((int)c1->yaw, (int)c2->yaw);
            c1 = c2;

            if (checkCnt == TURN_CHECK_CNT)
            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw);
//            DEBUG("%c  角度比较 %02d:%02d:%02d.%03d  %02d:%02d:%02d.%03d", turn[checkCnt], 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 (turn[checkCnt] == 0) {
                break;
            }

            c1 = c2;
            checkCnt++;
        }

        ++iter;
@@ -502,8 +590,10 @@
            break;
    }

    if (i == TURN_CHECK_CNT-1)
    if (i == TURN_CHECK_CNT-1) {
        DEBUG("左右转确认 %c", turn[0]);
        return turn[0];
    }

    return 0;
}
@@ -659,15 +749,18 @@
                for (int k = 1; k < RoadMapList[i].triggerLine[j].line.num; ++k) {
                    MakeLine(&trigger_line, &RoadMapList[i].triggerLine[j].line.point[kp], &RoadMapList[i].triggerLine[j].line.point[k]);

                    int pp = 1;
                    for (int p = 2; p < pn; ++p) {
                    int pp = 0;
                    for (int p = 1; p < pn; ++p) {
                        Line trace_line;
                        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);

                        if (IntersectionOf(trace_line, trigger_line) == GM_Intersection &&
                            IntersectionOfLine(trace.point[pp], trace.point[p], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
                            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trace_line) > 0.1) {
                            IntersectionOfLine(RoadMapList[i].triggerLine[j].line.point[kp],
                                    RoadMapList[i].triggerLine[j].line.point[k],
                                    car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
                            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trigger_line) > 0.1) {
                            // 碰到触发线
                            DEBUG("碰撞触发线 引发地图 id = %d", RoadMapList[i].triggerLine[j].triggerMapId);
                            map_idx =  FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList);
                            goto SEARCH_TRIGGER_LINE_END;
                        }

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

@@ -22,5 +22,6 @@
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);
bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList);

#endif //MYAPPLICATION2_ROAD_EXAM_H

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

@@ -17,17 +17,22 @@

static int breakActive;
static int stopActive;
static bool crashRedLine;

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);
    DEBUG("进入路考通过something地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS");
        PlayTTS(RoadMapList[index].tts.c_str(), 0);
    } else {
        DEBUG("没有TTS");
    }
    breakActive = 0;
    stopActive = 0;
    crashRedLine = false;
}

int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
@@ -99,6 +104,7 @@
                default:
                    break;
            }
            stopActive = -1;
        } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
            if (moveDirect == 0) {
                stopActive = 1;
@@ -106,9 +112,21 @@
        }
    }

    if (ExitSonArea(index, RoadMapList, car))
     if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) {
         if (!crashRedLine) {
             DEBUG("不按考试员指令行驶");
             crashRedLine = true;
             // 不按考试员指令行驶
             AddExamFault(3, rtkTime);
         }
     } else {
         crashRedLine = false;
     }

    if (ExitSonArea(index, RoadMapList, car)) {
        DEBUG("离开通过something区域");
        return -1;
    }

    return index;
}