坐标

yy1717

2020-08-19 712eb83d17ffc6e259af6c53ba63bbd58cd1242b

坐标

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

@@ -23,6 +23,7 @@
#include <map>
#include <string>
#include <cstdlib>
#include <algorithm>

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

@@ -72,6 +73,13 @@
    int gain;
    struct RtkTime time;
} change_lane_t;

typedef struct {
    int road_index;
    int stop_line_index;
    int sep;
    double distance;
} road_end_point_t;

static const int INVALID_ROAD = -1;

@@ -168,6 +176,7 @@
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);

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

static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);

@@ -175,7 +184,7 @@

static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);

void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);
static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car);

void InitRoadExam(road_exam_map &RoadMap)
{
@@ -1098,33 +1107,118 @@
    }
}

static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
/*********************************************************
 * 计算当前所在分道线的剩余长度
 * @param RoadMap
 * @param lane
 * @param car
 * @return
 */
static double SeparateLength(road_exam_map &RoadMap, lane_t lane, const car_model *car)
{
    for (int i = 0; i < road.stopLine.size(); ++i) {
        PointF point;
    double distance = 0;

    if (lane.road < 0 || lane.total == 0 || lane.sep < 0)
        return distance;

    int n = RoadMap.roads[lane.road].separate[lane.sep].lines[0].size();
    int m = RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points.size();

    return CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]],
            RoadMap.roads[lane.road].separate[lane.sep].lines[0][n-1].points[m-1],
            RoadMap.roads[lane.road].rightEdge);
}

/***********************************************************
 * 前向最近的路口停止线或导向线起点，一定距离内做出路口提示，或项目自动触发
 * @param roadIndex
 * @param RoadMap
 * @param car
 */
static road_end_point_t NearbyRoadEndPoint(int roadIndex, road_exam_map &RoadMap, const car_model *car)
{
    road_end_point_t rec;

    rec.road_index = roadIndex;
    rec.stop_line_index = 0;
    rec.distance = 0;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return rec;

    // 最近的路口距离
    int stop_line_index;
    PointF nearbyStopPoint;

    NearbyCrossingGuide(stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
    nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
    nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;

    double dis1 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], nearbyStopPoint, RoadMap.roads[roadIndex].rightEdge);

    struct ep_ {
        int sep;
        double distance;
    } ep;

        point.X = road.stopLine[i].line.X1;
        point.Y = road.stopLine[i].line.Y1;
    vector<struct  ep_> dset;

        distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, road.rightEdge);
    ep.sep = -1;
    ep.distance = dis1;
    dset.push_back(ep);

        if (distance > 10 && distance < 50) {
            // 提示路口怎么走
            if (GetCrossingStatus(roadIndex, i) == CROSSING_NOT_HINT) {
                if (!road.stopLine[i].tts.empty()) {
                    DEBUG("路口提示 %s", road.stopLine[i].tts.c_str());
                    PlayTTS(road.stopLine[i].tts.c_str());
    // 最近的分道线距离，且不超过最近路口
    for (int i = 0; i < RoadMap.roads[roadIndex].separate.size(); ++i) {
        for (int j = 0;  j < RoadMap.roads[roadIndex].separate[i].lines.size(); ++j) {
            for (int k = 0; k < RoadMap.roads[roadIndex].separate[i].lines[j].size(); ++k) {
                if (RoadMap.roads[roadIndex].separate[i].lines[j][k].character != LINE_DOTTED &&
                        RoadMap.roads[roadIndex].separate[i].lines[j][k].points.size() > 0) {
                    double dis2 = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], RoadMap.roads[roadIndex].separate[i].lines[j][k].points[0], RoadMap.roads[roadIndex].rightEdge);

                    if (dis2 < -1e-3) {
                        continue;
                    }else if (dis2 < dis1) {
                        // 找出最短的点
                        ep.sep = i;
                        ep.distance = dis2;
                        dset.push_back(ep);
                        continue;
                    }
                    goto FIND_END_POINT;
                }
                ChangeCrossingStatus(roadIndex, i, CROSSING_HAS_HINT);
            }
        } else if (distance > 55 && GetCrossingStatus(roadIndex, i) != CROSSING_NOT_HINT) {
            ChangeCrossingStatus(roadIndex, i, CROSSING_NOT_HINT);
        }
    }

    FIND_END_POINT:
    sort(dset.begin(), dset.end(), [=](struct ep_ x, struct ep_ y)->bool {return x.distance < y.distance;});

    rec.stop_line_index = stop_line_index;
    rec.distance = dset[0].distance;
    rec.sep = dset[0].sep;

    return rec;
}

static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;

    if (distance > 5 && distance < 70) {
        // 提示路口怎么走
        if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
            if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
                DEBUG("路口提示 %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
                PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
            }
            ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
        }
    } else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
        ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);
    }
}

static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
{
    int guide = 0, stopLine = 0;
    double distance;
@@ -1173,6 +1267,9 @@
    stopLineIndex = stopLine;
    return guide;
}

static uint8_t itemExec[4] = {0};
static double odo;

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -1372,8 +1469,6 @@

    if (currExamMapIndex >= 0) {
        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
        HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);

        // 检测通过路口、人行道等区域时，释放刹车或减速
        ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
    }
@@ -1393,13 +1488,13 @@

//    DEBUG("Lane信息 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
    if (Lane.guide > 0 && currExamMapIndex >= 0) {
        int stop_line;
        int stop_line_index;

        if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
            if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) {
        if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
            if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                DEBUG("不按规定车道标向行驶");
                AddExamFault(9, rtkTime);
                SetErrorLaneRpt(currExamMapIndex, stop_line, true);
                SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
            }
        }
    }
@@ -1408,8 +1503,92 @@
        DEBUG("导向类型切换 %d", Lane.guide);
    }

    if (currExamMapIndex >= 0) {
        AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
    if (currExamMapIndex >= 0 && Lane.guide == 0) {
        double BigStraightRoad = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);

        road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);

        HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);

        double freeSepDis = SeparateLength(RoadMap, Lane, car);

        // 剩余距离就是导向线起点的距离
        if (Lane.sep >= 0 && Lane.sep == ep.sep) {
            freeSepDis = ep.distance;
        }

        DEBUG("直道剩余距离 %f, 车道剩余距离 %f", BigStraightRoad, freeSepDis);

        if (startCar == START_CAR_DONE) {
            if (itemExec[0] == 1 || itemExec[1] == 1 || itemExec[2] == 1 || itemExec[3] == 1) {

                DEBUG("项目执行距离<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);

                if (ReadOdo() - odo > 120) {
                    odo = ReadOdo();
                    if (itemExec[0] == 1) {
                        itemExec[0] = 2;
                    }
                    if (itemExec[1] == 1) {
                        itemExec[1] = 2;
                    }
                    if (itemExec[2] == 1) {
                        itemExec[2] = 2;
                    }
                    if (itemExec[3] == 1) {
                        itemExec[3] = 2;
                    }
                }
                goto BIG_DOG;
            }

            if (itemExec[0] == 2 || itemExec[1] == 2 || itemExec[2] == 2 || itemExec[3] == 2) {
                DEBUG("项目休息距离<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);

                if (ReadOdo() - odo > 100) {
                    if (itemExec[0] == 2) {
                        itemExec[0] = 3;
                    }
                    if (itemExec[1] == 2) {
                        itemExec[1] = 3;
                    }
                    if (itemExec[2] == 2) {
                        itemExec[2] = 3;
                    }
                    if (itemExec[3] == 2) {
                        itemExec[3] = 3;
                    }
                }
                goto BIG_DOG;
            }


            if (BigStraightRoad > 170 && ep.distance > 170) {
                if (itemExec[0] == 0) {
                    PlayTTS("二狗直线行驶");
                    itemExec[0] = 1;
                    odo = ReadOdo();
                }
            } else if (BigStraightRoad > 150 && ep.distance > 150) {
                if (itemExec[3] == 0) {
                    PlayTTS("二狗加减档");
                    itemExec[3] = 1;
                    odo = ReadOdo();
                }
            } else if (freeSepDis > 150) {
                if (itemExec[2] == 0) {
                    PlayTTS("二狗变道");
                    itemExec[2] = 1;
                    odo = ReadOdo();
                } else if (itemExec[1] == 0) {
                    PlayTTS("二狗超车");
                    itemExec[1] = 1;
                    odo = ReadOdo();
                }
            }

            BIG_DOG:;
        }
    }

    // 检测压线状态
@@ -1430,9 +1609,8 @@
            stopCar = STOP_CAR_DONE;
    }


    // 执行某个项目
    if (currRoadItem != NULL) {
    /*if (currRoadItem != NULL) {
        if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
            if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
                DEBUG("变道距离超标");
@@ -1482,7 +1660,7 @@
                StartDriveStraightExam(currRoadItem->tts);
            }
        }
    }
    }*/
}

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -1625,61 +1803,6 @@
    prevTurnWise = angle;
}

static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
{
    // 最近2秒内，每0.5秒的角度差大于5度，且方向相同，连续4次；或突现超30度的转向；认为转向。
    if (CarModelList.size() < 1)
        return false;

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

    car_model *c1 = *iter, *c2;

    ++iter;

    char turn[TURN_CHECK_CNT] = {0};
    int checkCnt = 0;

//    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) {
            int ang = 0;
            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
//            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] == 'N') {
                break;
            } else if (ang >= 30) {
                DEBUG("左右转确认 %c", turn[checkCnt]);
                return turn[checkCnt];
            }

            c1 = c2;
            checkCnt++;
        }

        ++iter;
    }

    int i = 0;
    for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
        if (turn[i] != turn[i+1])
            break;
    }

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

    return 0;
}

/**********************************************************
 * 按整个车身是否覆盖计算
 * @param RoadMapList
@@ -1738,37 +1861,6 @@
    return false;
}


/***************************************************
 * 接近路口时，提示下一步怎么走
 * @param road
 * @param car
 * @param CarModelList
 */
/*static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    // 计算车前进轨迹延长线
    double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);

    PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
    Line extLine;

    MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);

    if (IntersectionOf(extLine, road.stopLine) == GM_Intersection &&
        IntersectionOfLine(extPoint, road.stopLine) == -1) {
            if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) {
                // 接近路口后，要检查车辆是否进入错误车道
                DEBUG("接近路口");
                road.arrivedTail = true;
                if (!road.tts.empty())
                    PlayTTS(road.tts.c_str());
            }
    } else if (road.arrivedTail) {
        road.arrivedTail = false;
    }
}*/

static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
@@ -1809,21 +1901,22 @@
    return true;
}

void AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
{
    double distance = 0;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;
        return 0;

    for (int i = 0; i < RoadMap.roads[roadIndex].rightEdge.size(); ++i) {
        for (int j = 1; j < RoadMap.roads[roadIndex].rightEdge[i].points.size(); ++j) {
            PointF point = RoadMap.roads[roadIndex].rightEdge[i].points[j];
            distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
            distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].leftEdge);
            if (distance > 1e-3) {
                DEBUG("直道剩余距离 %f", distance);
                return;
                return distance;
            }
        }
    }

    return 0;
}