LanePlan.cpp 12.6 KB
Newer Older
oscar's avatar
oscar committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260

#include "LanePlan.h"

#include <iostream>

#include "Bezier.h"

using namespace jf;

LanePlan::LanePlan(const std::string &cstrProjectPath, const std::string &cstrMapConfigPath) { HdLib::GetInstance()->Init(cstrProjectPath, cstrMapConfigPath); }

bool LanePlan::SetPath(const std::vector<Point> &cvctptWayNode, std::vector<LaneEndpoint> &vctleOutWayPoint, bool bRenowResult /* = true*/) {
    // 刷新现有的寻路结果
    if (true == bRenowResult) {
        m_vctleRoute.clear();
        m_vctlRouteEndLaneId.clear();
    }

    // 尝试寻路
    bool bFindResult = m_rtleRoute.FindRoute(cvctptWayNode, m_vctleRoute, vctleOutWayPoint, false, true);
    if (true == bFindResult) {
        // 将终点的车道ID保存
        m_vctlRouteEndLaneId.push_back(m_vctleRoute[m_vctleRoute.size() - 1].lLaneId);
    }

    // 生成轨迹点
    GenInterpGeometry(cvctptWayNode, vctleOutWayPoint, m_vctleRoute);
    PrintQGIS("FullRoute.csv", m_vctleRoute);

    return bFindResult;
}

void LanePlan::SetUnpassableLane(const std::vector<HdLaneId> &cvctlUnpassableLane) {
    // 手动设置不可通行的车道
    m_vctlUnpassableLane = cvctlUnpassableLane;
}

int LanePlan::GetLaneCnt() {
    // 返回寻路结果中的车道数目
    return m_vctleRoute.size();
}

bool LanePlan::GetLaneEndpoint(int nIndex, LaneEndpoint &leOutEndpoint) {
    // 判断参数是否合法
    if ((nIndex >= 0) && (nIndex < m_vctleRoute.size())) {
        leOutEndpoint = m_vctleRoute[nIndex];
        return true;
    }
    // 如果参数不对
    return false;
}

bool LanePlan::GetNearEndpoints(const Point &cptInPoint, std::vector<Point> &vctptOutPoint, std::vector<LaneEndpoint> &vctleOutLaneEp, std::vector<double> &vctdOutDistance) {
    std::vector<Point> vctptPoint = {};
    std::vector<LaneEndpoint> vctleLaneEp = {};
    std::vector<double> vctdDistance = {};
    // 获取指定点附近的车道集合
    if (true == HdLib::GetInstance()->GetNearEndpoints(cptInPoint, MapConfig::route_rect_lon, MapConfig::route_rect_lat, vctptPoint, vctleLaneEp, vctdDistance)) {
        static const int sc_nMaxResult = 6;
        vctptOutPoint.insert(vctptOutPoint.end(), vctptPoint.begin(), vctptPoint.begin() + std::min(int(vctleLaneEp.size()), sc_nMaxResult));
        vctleOutLaneEp.insert(vctleOutLaneEp.end(), vctleLaneEp.begin(), vctleLaneEp.begin() + std::min(int(vctleLaneEp.size()), sc_nMaxResult));
        vctdOutDistance.insert(vctdOutDistance.end(), vctdDistance.begin(), vctdDistance.begin() + std::min(int(vctleLaneEp.size()), sc_nMaxResult));
        return true;
    }
    return false;
}

bool LanePlan::GetIndex(const std::vector<LaneEndpoint> &cvctleEndpoint, int &nOutIndex, int nPastIndex /* = 0*/, int nFindCnt /* = 65536*/) {
    // 获取所给车道在寻路结果中的索引值
    for (const LaneEndpoint leEndpoint : cvctleEndpoint) {
        if (true == GetIndex(leEndpoint, nOutIndex, nPastIndex, nFindCnt)) {
            return true;
        }
    }
    return false;
}

bool LanePlan::GetIndex(const LaneEndpoint &cleEndpoint, int &nOutIndex, int nPastIndex /* = 0*/, int nFindCnt /* = 65536*/) {
    // 计算到终点的距离
    size_t nEndIndex = std::min(m_vctleRoute.size(), (size_t)(nPastIndex + nFindCnt));
    // 在寻路结果中,从当前节点开始,向后找,指导找到与指定节点相同的节点开始
    for (size_t i = nPastIndex; i < nEndIndex; ++i) {
        LaneEndpoint leCurEp = m_vctleRoute[i];
        if (cleEndpoint.lLinkId == leCurEp.lLinkId) {
            if (((leCurEp.nStartLaneNum >= cleEndpoint.nStartLaneNum) && (leCurEp.nEndLaneNum <= cleEndpoint.nStartLaneNum)) || ((leCurEp.nStartLaneNum <= cleEndpoint.nStartLaneNum) && (leCurEp.nEndLaneNum >= cleEndpoint.nStartLaneNum))) {
                nOutIndex = i;
                return true;
            } else {
                return false;
            }
        }
    }
    return false;
}

int LanePlan::GetPosIdxInLane(const Point &cptInPoint, int nIndex) {
    // 判断参数是否合法
    if (nIndex < 0 || nIndex >= m_vctleRoute.size()) {
        return -1;
    }

    // 获取指定的车道
    const LaneEndpoint &leLaneEp = m_vctleRoute[nIndex];
    std::vector<std::vector<double>> vctvdGeoLink = {};
    HdLib::GetInstance()->ConvertGeoLink(leLaneEp.vctptGeoLink, vctvdGeoLink);
    Point ptGeoPos = {};
    double dGeoDistance = -1;
    int nGeoIndex = 0;
    // 获取指定的点,在车道轨迹点上的索引(取距离最近的点的索引)
    if (false == HdLibMath::GetNearestPointFromArray(cptInPoint, vctvdGeoLink, ptGeoPos, dGeoDistance, nGeoIndex)) {
        return -1;
    }

    return nGeoIndex;
}

void LanePlan::GenInterpGeometry(const std::vector<Point> &cvctptWayNode, std::vector<LaneEndpoint> &vctleOutWayPoint, std::vector<LaneEndpoint> &vctleInOutLanePath) {
    // 处理每一个车道
    int nClosedListCnt = vctleInOutLanePath.size();
    for (size_t i = 0; i < nClosedListCnt; ++i) {
        // 获取该车道的link
        std::vector<std::vector<double>> vctvdGeoLink = {};
        HdLib::GetInstance()->GetLaneGeoLink(vctleInOutLanePath[i].lLinkId, vctleInOutLanePath[i].nEndLaneNum, vctvdGeoLink);
#if true
        // 将double类型的二维数组转换为Point型的一维数组
        HdLib::GetInstance()->ConvertGeoLink(vctvdGeoLink, vctleInOutLanePath[i].vctptGeoLink);

#else
        std::vector<Point> vctptGeoLink = {};
        // 将double类型的二维数组转换为Point型的一维数组
        m_mlMapLib.ConvertGeoLink(vctvdGeoLink, vctptGeoLink);

        int nStatGeoIndex = 0;
        int nEndGeoIndex = vctptGeoLink.size() - 1;
        Point ptGeoPos = {};
        // 第一车道和最后车道单独处理
        if ((i == 0) || (i == nClosedListCnt - 1)) {
            double dGeoDistance = -1;
            int nGeoIndex = 0;
            if (i == 0) {
                // 第一车道,所给点到车道的垂足点到距离车道最近的点的下一点(第二个点),以及从第二个点到最后的点分别处理
                GetNearestDistance(cvctptWayNode.at(0), vctvdGeoLink, ptGeoPos, dGeoDistance, nGeoIndex);
                GetPerpendicularFoot(cvctptWayNode.at(0), vctvdGeoLink, ptGeoPos, dGeoDistance);
                nStatGeoIndex = nGeoIndex == 0 ? nGeoIndex : nGeoIndex - 1;
            }
            if (i == nClosedListCnt - 1) {
                // 第末车道,最初的点到所给点的前一点,以及从前一点到所给点到车道的垂足点分别处理
                GetNearestDistance(cvctptWayNode.at(cvctptWayNode.size() - 1), vctvdGeoLink, ptGeoPos, dGeoDistance, nGeoIndex);
                GetPerpendicularFoot(cvctptWayNode.at(cvctptWayNode.size() - 1), vctvdGeoLink, ptGeoPos, dGeoDistance);
                nEndGeoIndex = nGeoIndex >= vctvdGeoLink.size() - 1 ? nGeoIndex : nGeoIndex + 1;
            }
        }

        if (vctleInOutLanePath[i].nStartLaneNum == vctleInOutLanePath[i].nEndLaneNum) {
            // 针对原来的点,做插值处理(间隔0.5米)
            for (int j = nStatGeoIndex, k = j + 1; k <= nEndGeoIndex; ++j, ++k) {
                std::vector<Point> ret = jf::InterP(vctptGeoLink[j], vctptGeoLink[k], 0.5);
                vctleInOutLanePath[i].vctptGeoLink.insert(vctleInOutLanePath[i].vctptGeoLink.end(), ret.begin(), ret.end());
            }
        } else {
            // 平滑
            std::vector<Point> vctptCurve = {};
            SmoothChangeRoad(vctleInOutLanePath[i].lLinkId, vctleInOutLanePath[i].nStartLaneNum, vctleInOutLanePath[i].nEndLaneNum, vctptCurve);
            vctleInOutLanePath[i].vctptGeoLink = vctptCurve;
        }
#endif
    }
}

bool LanePlan::SmoothChangeRoad(const HdLinkId &clLinkId, int nFromLaneNum, int nToLaneNum, std::vector<Point> &vctptOutCurve) {
    int nChangeRoadNum = abs(nFromLaneNum - nToLaneNum);
    auto func_Calc = [this](const HdLinkId &clLinkId, int nChangeRoadNum, int nFromLane, int nToLane, float fPercent, int nPastIndex, bool bIsFirstSegment, bool bIsLastSegment, std::vector<Point> &vctptOutCurve) -> int {
        std::vector<std::vector<double>> vctvdFromGeoLink = {};
        HdLib::GetInstance()->GetLaneGeoLink(clLinkId, nFromLane, vctvdFromGeoLink);
        int nFromControlIndex = fPercent * vctvdFromGeoLink.size();
        std::vector<std::vector<double>> vctvdToGeoLink = {};
        HdLib::GetInstance()->GetLaneGeoLink(clLinkId, nToLane, vctvdToGeoLink);
        int nToControlIdx = fPercent * vctvdToGeoLink.size();
        // 变道半长度,根据index来的,index每隔0.5米一个
        int nSmoothValue = std::min(std::max(int(vctvdFromGeoLink.size() / float(nChangeRoadNum) / 2), 1), 30);
        // 上一次变道到本nFromLane的index,到本nFromLane到to_lane的起始
        // int nCurveSeg1FromIdx = nPastIndex + (bIsFirstSegment ? 0 : nSmoothValue);
        // int nCurveSeg1ToIdx = nFromControlIndex - nSmoothValue;
        // if (nCurveSeg1FromIdx < nCurveSeg1ToIdx) {
        // vctptOutCurve.insert(vctptOutCurve.end(), vctvdFromGeoLink.begin() + nCurveSeg1FromIdx, vctvdFromGeoLink.begin() + nCurveSeg1ToIdx);
        // }
        // 贝塞尔曲线
        std::vector<Point> vctptCs{Point(vctvdFromGeoLink[std::max(nFromControlIndex - nSmoothValue, 0)][0], vctvdFromGeoLink[std::max(nFromControlIndex - nSmoothValue, 0)][1], vctvdFromGeoLink[std::max(nFromControlIndex - nSmoothValue, 0)][2]),
                                   Point(vctvdFromGeoLink[nFromControlIndex - 0][0], vctvdFromGeoLink[nFromControlIndex - 0][1], vctvdFromGeoLink[nFromControlIndex - 0][2]),
                                   Point(vctvdToGeoLink[nToControlIdx + 0][0], vctvdToGeoLink[nToControlIdx + 0][1], vctvdToGeoLink[nToControlIdx + 0][2]),
                                   Point(vctvdToGeoLink[std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1))][0], vctvdToGeoLink[std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1))][1],
                                         vctvdToGeoLink[std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1))][2])};
        std::vector<Point> vctptBs = Bezier(vctptCs, nSmoothValue * 6);
        vctptOutCurve.insert(vctptOutCurve.end(), vctptBs.begin(), vctptBs.end());

        // 连到终点
        if (bIsLastSegment) {
            vctptOutCurve.insert(vctptOutCurve.end(), vctvdToGeoLink.begin() + std::min(nToControlIdx + nSmoothValue, int(vctvdToGeoLink.size() - 1)), vctvdToGeoLink.end());
        }

        return nToControlIdx;
    };
    // 循环需要并道的所有index
    // 找比例点,贝塞尔曲线控制点在比例点附近
    // 比如变3条车道,比例点是1/6,3/6,5/6
    // 比如变2条车道,比例点是1/4,3/4
    int nToControlIdx = 0;
    // 从右向左变道
    if (nFromLaneNum > nToLaneNum) {
        int nPercentIdx = 1;
        for (int i = nFromLaneNum, j = nFromLaneNum - 1; j >= nToLaneNum; --i, --j) {
            float fPercent = float(nPercentIdx) / (nChangeRoadNum * 2);
            nToControlIdx = func_Calc(clLinkId, nChangeRoadNum, i, j, fPercent, nToControlIdx, i == nFromLaneNum, j == nToLaneNum, vctptOutCurve);
            nPercentIdx += 2;
        }
    }
    // 从左向右变道
    else {
        int nPercentIdx = 1;
        for (int i = nFromLaneNum, j = nFromLaneNum + 1; j <= nToLaneNum; ++i, ++j) {
            float fPercent = float(nPercentIdx) / (nChangeRoadNum * 2);
            nToControlIdx = func_Calc(clLinkId, nChangeRoadNum, i, j, fPercent, nToControlIdx, i == nFromLaneNum, j == nToLaneNum, vctptOutCurve);
            nPercentIdx += 2;
        }
    }

    // 插值
    std::vector<Point> vctptInterP = {};
    for (int i = 0, j = 1; j < vctptOutCurve.size(); ++i, ++j) {
        std::vector<Point> vctptRet = jf::InterP(vctptOutCurve[i], vctptOutCurve[j], 0.5);
        vctptInterP.insert(vctptInterP.end(), vctptRet.begin(), vctptRet.end());
    }
    vctptOutCurve = vctptInterP;

    return true;
}

void LanePlan::PrintQGIS(const std::string &cstrFileName, const std::vector<LaneEndpoint> &cvctleClosedList) {
    static int64_t IndexGlobal;
    std::ofstream ofsQgisResult;
    // 创建csv文件
    ofsQgisResult.open(cstrFileName.c_str(), std::ios::out | std::ios::trunc);
    ofsQgisResult.setf(std::ios::fixed, std::ios::floatfield);
    ofsQgisResult.precision(15);

    // 插入标签
    ofsQgisResult << "lon"
                  << ", "
                  << "lat"
                  << ", "
                  << "num" << std::endl;
    // 写入点
    for (size_t i = 0; i < cvctleClosedList.size(); ++i) {
        for (size_t j = 0; j < cvctleClosedList[i].vctptGeoLink.size(); j++) {
            ofsQgisResult << cvctleClosedList[i].vctptGeoLink[j].dLon << ", " << cvctleClosedList[i].vctptGeoLink[j].dLat << ", " << IndexGlobal << std::endl;
            IndexGlobal++;
        }
    }
    ofsQgisResult.close();
}