#include "MapInfo.hpp" namespace jf { MapInfo::MapInfo(){} MapInfo::~MapInfo() {} /** * 获取车辆实际行驶的link和lane * * @param pointIn 车辆GPS坐标点 * @param Agdir 车辆航向角 * @param hdLinkId * @param hdLaneId * @return 主要排除问题:(1)坐标点位于多个link的交叉区域的时候判定车辆真正行驶的link * (2)车辆拐弯的时候暂时偏离实际想走的车道 */ bool MapInfo::locateLinkandLane(Point pointIn, float Agdir, HdLinkId &hdLinkId, HdLaneId &hdLaneId) { static HdLinkId s_liLastLinkId = {}; static HdLaneId s_liLastLaneId = {}; //记录车辆上一个周期经过的link和lane std::vector<HdLinkId> vctliLocateLinkIds = {}; std::vector<HdLaneId> vctliLocateLaneIds = {}; if (false == m_spHdLib->GetLocateLanes(pointIn, vctliLocateLinkIds, vctliLocateLaneIds)) { //定位失败,那么使用上次的结果 if (s_liLastLaneId.lValue == 0) { // 上次结果不可用 return false; } else { // 上次结果可以用 vctliLocateLinkIds.push_back(s_liLastLinkId); vctliLocateLaneIds.push_back(s_liLastLaneId); } } if (vctliLocateLaneIds.size() >= 2) { // 首先,选择航向角匹配的车道 std::vector<int> vctnAngleMatchedLanes = {}; for (int i = 0; i < vctliLocateLaneIds.size(); ++i) { double dAzimuth = 0.0; m_spHdLib->GetLaneAzimuth(vctliLocateLinkIds.at(i), vctliLocateLaneIds.at(i), pointIn, dAzimuth); double dAngleSub = std::min(std::abs(dAzimuth - Agdir), std::abs(360 - (dAzimuth - Agdir))); if (dAngleSub < 15) { vctnAngleMatchedLanes.push_back(i); } } // 多个车道符合条件或者没有符合条件的车道 if (vctnAngleMatchedLanes.size() >= 2 || vctnAngleMatchedLanes.size() == 0) { // 应该尝试使用INS的高程作为判断,但INS的高程并不是很精确,所以不使用 hdLinkId = s_liLastLinkId; hdLaneId = s_liLastLaneId; } else { hdLinkId = vctliLocateLinkIds.at(vctnAngleMatchedLanes.at(0)); hdLaneId = vctliLocateLaneIds.at(vctnAngleMatchedLanes.at(0)); } } else { // // 即使只有一条车道符合条件,也可能存在坐标点与实际车道不符合的情况 // double dAzimuth = 0.0; // m_spHdLib->GetLaneAzimuth(vctliLocateLinkIds.at(0), vctliLocateLaneIds.at(0), pointIn, dAzimuth); // double dAngleSub = std::min(std::abs(dAzimuth - Agdir), std::abs(360 - (dAzimuth - Agdir))); // if (dAngleSub < 15) { hdLinkId = vctliLocateLinkIds.at(0); hdLaneId = vctliLocateLaneIds.at(0); // } // // 角度不匹配,使用上次结果 // else { // hdLinkId = s_liLastLinkId; // hdLaneId = s_liLastLaneId; // } } s_liLastLinkId = hdLinkId; s_liLastLaneId = hdLaneId; return true; } /** * 输出自车当前车道的道路信息 * * 信息包括: * (1)当前车道的限速信息 * (2)左右相邻车道的车到边界线ID * (3)是否能变道到左右相邻车道 * @return */ bool MapInfo::getCurrentLocationInfo(Point pointIn, float Agdir, HdLinkId &hdLinkId, HdLaneId &hdLaneId, int& speedLimitInfo, int64_t& LeftLaneEdgeId1, int64_t& LeftLaneEdgeId2, bool& LeftLane_Crossable, int64_t& RightLaneEdgeId1, int64_t& RightLaneEdgeId2, bool& RightLane_Crossable){ const HdLink *linkPtr = m_spHdLib->GetLinkData(hdLinkId); int CurrentLaneNum; for(HdLane hdLane :linkPtr->vcthlLane){ if(hdLane.liId.lValue == hdLaneId.lValue){ speedLimitInfo = hdLane.nMaxSpeed; //限速信息输出 CurrentLaneNum = hdLane.nSeqNum; break; } } for (const HdLane hdLane : linkPtr->vcthlLane) { if(CurrentLaneNum - 1 > 0){ if (hdLane.nSeqNum == CurrentLaneNum-1) { LeftLaneEdgeId1 = hdLane.lLeftEdgeId; LeftLaneEdgeId2 = hdLane.lRightEdgeId; LeftLane_Crossable = m_spHdLib->IsReachable(linkPtr, hdLaneId,hdLane.liId); } }else if(CurrentLaneNum + 1 <= linkPtr->vcthlLane.size()){ if(hdLane.nSeqNum == CurrentLaneNum + 1){ RightLaneEdgeId1 = hdLane.lLeftEdgeId; RightLaneEdgeId2 = hdLane.lRightEdgeId; RightLane_Crossable = m_spHdLib->IsReachable(linkPtr, hdLaneId,hdLane.liId); } } } return true; } /** * 计算需要输出车道边界线的Link * * std::deque<HdLinkId> hdLinkIdQueue; //最终需要维护的数据结构1 * std::map<HdLinkId, std::map<int, Point>> hdLinkIdQueueTable; //最终需要维护的数据结构2 * @return */ bool MapInfo::CreateMapInfo_First( const HdLinkId &hdLinkId, const HdLaneId &hdLaneId, const Point pointIn, std::deque<HdLinkId>& hdLinkIdQueue, std::map<HdLinkId, std::map<int, Point>>& hdLinkIdQueueTable) { double BehindDistanceTarget = 10.0; //车辆后方10m double FontDistanceTarget = 200; //车辆前方100m const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkId);//自车所在的Link static std::queue<HdLinkId> hdLinkIdLastQueue; //自辆经过的上一个Link和本周期的link队列 Point footPointOfCar; //自车垂足点 double distanceTmp; int indexOfCarPoint; //自车所在的Index HdLibMath::GetPerpendicularFoot(pointIn, hdLinkPtr->vctvdGeoLink, footPointOfCar, distanceTmp, indexOfCarPoint); /** * 生成车前的信息 */ auto createFont = [this](Point pointInGeoLink, const std::vector<std::vector<double>> &vctvdGeoLink, int indexOfPoint, double &distanceTarget, std::map<int, Point> &resultGeoMap) { double dRealLength = 0; //本周期累加距离总和 static double dRealLengthLast = 0; //上一周期累加距离总和 for (int i = indexOfPoint; i < vctvdGeoLink.size(); i++) { if (i == indexOfPoint) { dRealLength += PointsDis(Point(vctvdGeoLink[i]), pointInGeoLink); } else { dRealLength += PointsDis(Point(vctvdGeoLink[i]), Point(vctvdGeoLink[i - 1])); } if (dRealLength >= distanceTarget) { //如果累加距离超过了目标距离 std::vector<Point> vctPointTmp = jf::InterP(vctvdGeoLink[i - 1], vctvdGeoLink[i],distanceTarget - dRealLengthLast); resultGeoMap[i] = vctPointTmp[1]; return true; } dRealLengthLast = dRealLength; } distanceTarget = distanceTarget - dRealLength; return false; }; /** * 生成车后的信息 */ auto createBehind = [this](Point pointInGeoLink, const std::vector<std::vector<double>> &vctvdGeoLink, int indexOfPoint, double &distanceTarget, std::map<int, Point> &resultGeoMap) { double dRealLength = 0; //本周期累加距离总和 static double dRealLengthLast = 0; //上一周期累加距离总和 for (int i = indexOfPoint - 1; i >= 0; i--) { if (i == indexOfPoint - 1) { dRealLength += PointsDis(Point(vctvdGeoLink[i]), pointInGeoLink); } else { dRealLength += PointsDis(Point(vctvdGeoLink[i]), Point(vctvdGeoLink[i + 1])); } if (dRealLength >= distanceTarget) { //如果累加距离超过了目标距离 std::vector<Point> vctPointTmp = jf::InterP(vctvdGeoLink[i + 1], vctvdGeoLink[i], distanceTarget - dRealLengthLast); resultGeoMap[i] = vctPointTmp[1]; distanceTarget = 0; return true; } dRealLengthLast = dRealLength; } distanceTarget = distanceTarget - dRealLength; return false; }; /** * 递归寻找前方具有联通关系的Link */ auto findFontLinksFunc = [this, &createFont, &hdLinkIdQueue, &hdLinkIdQueueTable](HdLinkId hdLinkId1, double FontDistanceTarget) { std::function<void(HdLinkId, double)> func; func = [this, &createFont, &hdLinkIdQueue, &hdLinkIdQueueTable, &func](HdLinkId hdLinkId1, double FontDistanceTarget) { const HdLink *hdLinkPtrTmp = m_spHdLib->GetLinkData(hdLinkId1); for (HdLinkId hdLinkIdTmp: hdLinkPtrTmp->vctliNxtLinkId) { std::map<int, Point> resultGeoMap_FontTmp; double FontDistanceTargetTmp1 = FontDistanceTarget; const HdLink *hdLinkPtrTmp = m_spHdLib->GetLinkData(hdLinkIdTmp); if (!createFont(hdLinkPtrTmp->vctvdGeoLink[0], hdLinkPtrTmp->vctvdGeoLink, 1, FontDistanceTargetTmp1, resultGeoMap_FontTmp)) { hdLinkIdQueue.push_front(hdLinkPtrTmp->liId); func(hdLinkPtrTmp->liId, FontDistanceTargetTmp1); } else { hdLinkIdQueue.push_front(hdLinkPtrTmp->liId); hdLinkIdQueueTable[hdLinkPtrTmp->liId] = resultGeoMap_FontTmp; } } }; return func(hdLinkId1, FontDistanceTarget); }; /******车辆后方*******/ std::map<int, Point> resultGeoMap_behind; double BehindDistanceTargetTmp = BehindDistanceTarget; if (createBehind(footPointOfCar, hdLinkPtr->vctvdGeoLink, indexOfCarPoint, BehindDistanceTargetTmp, resultGeoMap_behind)) { //计算自车link hdLinkIdQueue.push_back(hdLinkPtr->liId); hdLinkIdQueueTable[hdLinkPtr->liId] = resultGeoMap_behind; } else {//计算以前走过的link hdLinkIdQueue.push_back(hdLinkPtr->liId); HdLinkId hdLinkIdLast; if (hdLinkIdLastQueue.size() < 2) { //如果没有以前的linkId记录 if (!hdLinkPtr->vctliPreLinkId.empty()) { hdLinkIdLast = hdLinkPtr->vctliPreLinkId[0]; } } else { //如果有以前的linkId记录 hdLinkIdLast = hdLinkIdLastQueue.front(); } if (hdLinkIdLast.lValue != 0) { const HdLink *hdLinkLastPtr = m_spHdLib->GetLinkData(hdLinkIdLast); if (createBehind(hdLinkLastPtr->vctvdGeoLink[hdLinkLastPtr->vctvdGeoLink.size() - 1], hdLinkLastPtr->vctvdGeoLink, hdLinkLastPtr->vctvdGeoLink.size() - 1, BehindDistanceTargetTmp, resultGeoMap_behind)) { hdLinkIdQueue.push_back(hdLinkLastPtr->liId); hdLinkIdQueueTable[hdLinkLastPtr->liId] = resultGeoMap_behind; } else { hdLinkIdQueue.push_back(hdLinkLastPtr->liId); } } } if (hdLinkIdLastQueue.empty()) { hdLinkIdLastQueue.push(hdLinkPtr->liId); } else if (hdLinkIdLastQueue.size() == 1) { if (hdLinkIdLastQueue.back().lValue != hdLinkPtr->liId.lValue) { hdLinkIdLastQueue.push(hdLinkPtr->liId); } } else { if (hdLinkIdLastQueue.back().lValue != hdLinkPtr->liId.lValue) { hdLinkIdLastQueue.push(hdLinkPtr->liId); } hdLinkIdLastQueue.pop(); } /******车辆前方*******/ std::map<int, Point> resultGeoMap_Font; double FontDistanceTargetTmp = FontDistanceTarget; if (createFont(footPointOfCar, hdLinkPtr->vctvdGeoLink, indexOfCarPoint, FontDistanceTargetTmp, resultGeoMap_Font)) { //计算自车link hdLinkIdQueueTable[hdLinkPtr->liId].insert(resultGeoMap_Font.begin(), resultGeoMap_Font.end()); } else {//计算前方具有联通关系的link findFontLinksFunc(hdLinkPtr->liId, FontDistanceTargetTmp); } return true; } /** * 计算需要输出的地图信息 * @return */ bool MapInfo::CreateMapInfo_Second(const HdLinkId &hdLinkId, const HdLaneId &hdLaneId, const Point pointIn, std::deque<HdLinkId> &hdLinkIdQueue, std::map<HdLinkId, std::map<int, Point>> &hdLinkIdQueueTable, std::vector<HdEdge>& vctheEdgeResult) { //(1)队列尾部第一个元素 特殊处理:从Index开始向数组尾部方向输出边界线 if((hdLinkIdQueueTable.count(hdLinkIdQueue.back())!=0)&& hdLinkIdQueueTable[hdLinkIdQueue.back()].size()==1){ Point pointTmp; for(auto it:hdLinkIdQueueTable[hdLinkIdQueue.back()]){ pointTmp = it.second; } const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back()); for(HdEdge hdEdge : hdLinkPtr->vctheEdge){ Point footPoint; double dis; int footIndex; HdLibMath::GetPerpendicularFoot(pointTmp,hdEdge.vctvdGeoLink,footPoint,dis,footIndex); int targetEdgePointSize = hdEdge.vctvdGeoLink.size()-footIndex; //删除数组中前面的元素,一直到元素个数到达目标个数。 for(std::vector<std::vector<double>>::iterator it = hdEdge.vctvdGeoLink.begin();hdEdge.vctvdGeoLink.size()>targetEdgePointSize;){ it = hdEdge.vctvdGeoLink.erase(it); } std::vector<double> Vectpoint = {footPoint.dLon,footPoint.dLat,footPoint.dAlt}; hdEdge.vctvdGeoLink.insert(hdEdge.vctvdGeoLink.begin(),Vectpoint); vctheEdgeResult.push_back(hdEdge); } hdLinkIdQueue.pop_back(); } //(2)遍历队列中所有link while(!hdLinkIdQueue.empty()){ /**table里面没有记录信息,则表示link的全部的边界线都要**/ if(hdLinkIdQueueTable.find(hdLinkIdQueue.back())==hdLinkIdQueueTable.end()){ const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back()); for(HdEdge hdEdge : hdLinkPtr->vctheEdge){ vctheEdgeResult.push_back(hdEdge); } hdLinkIdQueue.pop_back(); } /**table里面记录一条数据,则从Index开始向数组头部方向输出边界线**/ else if(hdLinkIdQueueTable[hdLinkIdQueue.back()].size()==1){ Point pointTmp; for(auto it:hdLinkIdQueueTable[hdLinkIdQueue.back()]){ pointTmp = it.second; } const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back()); for(HdEdge hdEdge : hdLinkPtr->vctheEdge){ Point footPoint; double dis; int footIndex; HdLibMath::GetPerpendicularFoot(pointTmp,hdEdge.vctvdGeoLink,footPoint,dis,footIndex); int targetEdgePointSize = footIndex; //删除数组中后面的元素,一直到元素个数到达目标个数。 while( hdEdge.vctvdGeoLink.size()>targetEdgePointSize){ hdEdge.vctvdGeoLink.pop_back(); } std::vector<double> Vectpoint = {footPoint.dLon,footPoint.dLat,footPoint.dAlt}; hdEdge.vctvdGeoLink.push_back(Vectpoint); vctheEdgeResult.push_back(hdEdge); } hdLinkIdQueue.pop_back(); } /**table里面记录两条数据,则从小Index向大Index方向输出边界线**/ else{ std::vector<Point> VctpointTmp; for(auto it:hdLinkIdQueueTable[hdLinkIdQueue.back()]){ VctpointTmp.push_back(it.second); } const HdLink *hdLinkPtr = m_spHdLib->GetLinkData(hdLinkIdQueue.back()); for(HdEdge hdEdge : hdLinkPtr->vctheEdge){ Point footPoint1,footPoint2; double dis1,dis2; int footIndex1,footIndex2; HdLibMath::GetPerpendicularFoot(VctpointTmp[0],hdEdge.vctvdGeoLink,footPoint1,dis1,footIndex1); HdLibMath::GetPerpendicularFoot(VctpointTmp[1],hdEdge.vctvdGeoLink,footPoint2,dis2,footIndex2); if(footIndex1>=footIndex2){ /**首先删除尾部元素*/ int targetEdgePointSize = footIndex1; //删除数组中后面的元素,一直到元素个数到达目标个数。 while( hdEdge.vctvdGeoLink.size()>targetEdgePointSize){ hdEdge.vctvdGeoLink.pop_back(); } std::vector<double> Vectpoint = {footPoint1.dLon,footPoint1.dLat,footPoint1.dAlt}; hdEdge.vctvdGeoLink.push_back(Vectpoint); /***删除头部元素****/ int targetEdgePointSize2 = footIndex2; for(std::vector<std::vector<double>>::iterator it = hdEdge.vctvdGeoLink.begin();targetEdgePointSize2 > 0;){ it = hdEdge.vctvdGeoLink.erase(it); targetEdgePointSize2--; } std::vector<double> Vectpoint2 = {footPoint2.dLon,footPoint2.dLat,footPoint2.dAlt}; hdEdge.vctvdGeoLink.insert(hdEdge.vctvdGeoLink.begin(),Vectpoint2); vctheEdgeResult.push_back(hdEdge); }else{ /**首先删除尾部元素*/ int targetEdgePointSize = footIndex2; //删除数组中后面的元素,一直到元素个数到达目标个数。 while( hdEdge.vctvdGeoLink.size()>targetEdgePointSize){ hdEdge.vctvdGeoLink.pop_back(); } std::vector<double> Vectpoint = {footPoint2.dLon,footPoint2.dLat,footPoint2.dAlt}; hdEdge.vctvdGeoLink.push_back(Vectpoint); /***删除头部元素****/ int targetEdgePointSize2 = footIndex1; for(std::vector<std::vector<double>>::iterator it = hdEdge.vctvdGeoLink.begin();targetEdgePointSize2 > 0;){ it = hdEdge.vctvdGeoLink.erase(it); targetEdgePointSize2--; } std::vector<double> Vectpoint2 = {footPoint1.dLon,footPoint1.dLat,footPoint1.dAlt}; hdEdge.vctvdGeoLink.insert(hdEdge.vctvdGeoLink.begin(),Vectpoint2); vctheEdgeResult.push_back(hdEdge); } } hdLinkIdQueue.pop_back(); } } return true; } } // namespace jf