Skip to content

T
thirdparty_map
Commit 4294e10b authored by oscar's avatar oscar
Browse files
Options

更新接口逻辑

parent 7945b821
Hide whitespace changes
Inline Side-by-side
Showing with 185 additions and 28 deletions
main.cpp
View file @ 4294e10b
......@@ -9,8 +9,8 @@ int main(int argc, char* argv[])
{
printf("hello world\n");
std::string dir = "";
std::string cfg = "";
std::string dir = "/home/oscar/ros/git/catkin_ws_xishan/src/jfx_traffic_events/";
std::string cfg = "/thirdparty_map/maps/dqSample12091.xodr";
jf::MapInterface mapInstance(dir, cfg);
int nLaneCnt = 0/*所在道路的车道总数*/, nOutLaneNum = 0;//当前所在位置的车道序号，从左至右，从1开始
......@@ -20,9 +20,9 @@ int main(int argc, char* argv[])
int nOutSpeedLimit = 0;//车道最高限速值，单位是千米每小时
double dOutLaneAngle = 0.0;//所在车道，车应该的朝向角度，北为0，顺时针旋转360，0-360
const jf::Point ptInLoc = { 106.474419689, 29.67213045, 0.0 };
const jf::Point ptInLoc = { 3926.381879, 11180.870534, 0.0 };
jf::Point ptOutFoot;//车道中心线做垂线的点
double dCarAngle = 0;
double dCarAngle = 30.0f;
long lOutRaodId;
std::vector<long> vctlOutPreRoadId;
std::vector<long> vctlOutNxtRoadId;
......
src/MapInterface.cpp
View file @ 4294e10b
#include "MapInterface.hpp"
#include "OdrManager.h"
using namespace OpenDrive;
OpenDrive::OdrManager g_map;
int g_load = 0;
namespace jf {
MapInterface::MapInterface(const std::string& strPrjPath, const std::string strCfgPath)
{
}
MapInterface::~MapInterface()
{
}
bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType, EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle,
Point &ptOutFoot)
{
long lRoadId;
std::vector<long> vctlPreRoadId = {};
std::vector<long> vctlNxtRoadId = {};
return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot);
}
bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, long &lOutRaodId, std::vector<long> &vctlOutPreRoadId, std::vector<long> &vctlOutNxtRoadId, int &nOutLaneCnt, int &nOutLaneNum, LaneType &nOutLaneType,
EdgeCrossType &nOutLeftEdgeCrossType, EdgeCrossType &nOutRightEdgeCrossType, int &nOutSpeedLimit, double &dOutLaneAngle, Point &ptOutFoot)
{
return true;
}
MapInterface::MapInterface(const std::string& strPrjPath, const std::string strCfgPath)
{
std::string filename = strPrjPath + strCfgPath;
bool bRtn = g_map.LoadFile(filename.c_str());
if (bRtn)
g_load = 1;
std::cout << "load map file = " << filename << " ret = " << bRtn << std::endl;
}
MapInterface::~MapInterface()
{
}
bool MapInterface::GetMapData(const Point& ptInLoc, double dCarAngle, int& nOutLaneCnt, int& nOutLaneNum, LaneType& nOutLaneType, EdgeCrossType& nOutLeftEdgeCrossType, EdgeCrossType& nOutRightEdgeCrossType, int& nOutSpeedLimit, double& dOutLaneAngle,
Point& ptOutFoot)
{
long lRoadId;
std::vector<long> vctlPreRoadId = {};
std::vector<long> vctlNxtRoadId = {};
return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot);
}
bool MapInterface::GetMapData(const Point& ptInLoc, double dCarAngle, long& lOutRaodId, std::vector<long>& vctlOutPreRoadId, std::vector<long>& vctlOutNxtRoadId, int& nOutLaneCnt, int& nOutLaneNum, LaneType& nOutLaneType,
EdgeCrossType& nOutLeftEdgeCrossType, EdgeCrossType& nOutRightEdgeCrossType, int& nOutSpeedLimit, double& dOutLaneAngle, Point& ptOutFoot)
{
if (g_load == 0)
return false;
std::vector<OdrInfo> odr = g_map.GetOdrInfoByXY(ptInLoc.dLon, ptInLoc.dLat);
printf("odr size = %d\n", odr.size());
for (auto iter : odr)
{
printf("roadID = %s,sectionID = %s,laneID = %s, _s = %f. _t = %f \n", iter._roadID, iter._sectionID, iter._laneID, iter._s, iter._t);
}
if (odr.size() == 0)
return false;
std::string roadId = odr[0]._roadID;
std::string sectionId = odr[0]._sectionID;
std::string laneId = odr[0]._laneID;
lOutRaodId = std::stol(roadId);
int _sectionId = std::stol(sectionId);
int _laneId = std::stol(laneId);
std::cout << "lOutRaodId = " << lOutRaodId << ",_sectionId = " << _sectionId << ", _laneId = " << _laneId << std::endl;
char** preRoadId = nullptr;
int preSize = 0;
bool retPre = g_map.GetPreRoadID(roadId.c_str(), preRoadId, preSize);
if (retPre)
{
std::cout << "vctlOutPreRoadId size = " << preSize << std::endl;
for (int i = 0; i < preSize; i++)
{
std::string prdId = preRoadId[i];
long preIdx = std::stol(prdId);
vctlOutPreRoadId.push_back(preIdx);
std::cout << "vctlOutPreRoadId i = " << i << ", roadId = " << preIdx << std::endl;
}
}
char** nextRoadId = nullptr;
int nextSize = 0;
bool retNext = g_map.GetSucRoadID(roadId.c_str(), nextRoadId, nextSize);
if (retNext)
{
std::cout << "vctlOutNxtRoadId size = " << nextSize << std::endl;
for (int i = 0; i < nextSize; i++)
{
std::string nextId = nextRoadId[i];
long nextIdx = std::stol(nextId);
vctlOutNxtRoadId.push_back(nextIdx);
std::cout << "vctlOutNxtRoadId i = " << i << ", roadId = " << nextIdx << std::endl;
}
}
char** allLaneIds = nullptr;
int allLaneSize = 0;
bool retALane = g_map.GetAllDrivingLaneIDS(roadId.c_str(), sectionId.c_str(), allLaneIds, allLaneSize);
if (retALane)
{
std::cout << "GetAllDrivingLaneIDS size = " << allLaneSize << std::endl;
for (int i = 0; i < allLaneSize; i++)
{
std::cout << "GetAllDrivingLaneIDS i = " << i << ", laneId = " << allLaneIds[i] << std::endl;
}
}
nOutLaneCnt = allLaneSize;
nOutLaneNum = abs(_laneId);
std::string typeString = g_map.GetLaneTypeString(roadId.c_str(), sectionId.c_str(), laneId.c_str());
std::cout << "GetLaneTypeString type = " << typeString << std::endl;
// "Driving" "Biking" "Shoulder" "SideWalk" "Parking" "Restricted" "Border" "none"
if ("Driving" == typeString)
{
nOutLaneType = LaneType::NormalLane;
}
else if ("Biking" == typeString)
{
nOutLaneType = LaneType::NonMotorizedLane;
}
else if ("Shoulder" == typeString)
{
nOutLaneType = LaneType::NormalLane;
}
else if ("SideWalk" == typeString)
{
nOutLaneType = LaneType::Sidewalk;
}
else if ("Parking" == typeString)
{
nOutLaneType = LaneType::StopLane;
}
else if ("Restricted" == typeString)
{
nOutLaneType = LaneType::NormalLane;
}
else if ("Border" == typeString)
{
nOutLaneType = LaneType::NormalLane;
}
else
{
nOutLaneType = LaneType::Ohters;
}
std::cout << "nOutLaneType = " << static_cast<int>(nOutLaneType) << std::endl;
bool isJunction = g_map.bJunctionRoad(roadId.c_str());
std::cout << "bJunctionRoad = " << isJunction << std::endl;
if (isJunction)
{
nOutLaneType = LaneType::JunctionLane;
}
NearestRoadMark mark = g_map.GetNeartestLaneRoadMarkByXY(ptInLoc.dLon, ptInLoc.dLat);
if (mark._leftRoadMark)
{
std::cout << "_leftRoadMark _type = " << mark._leftRoadMark->_type << std::endl;
if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID || mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_SOLID)
nOutLeftEdgeCrossType = EdgeCrossType::Unable;
else if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN)
nOutLeftEdgeCrossType = EdgeCrossType::BidirecAble;
else if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_BROKEN)
nOutLeftEdgeCrossType = EdgeCrossType::LeftAble;
else if (mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN_SOLID)
nOutLeftEdgeCrossType = EdgeCrossType::RightAble;
else
nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
std::cout << "nOutLeftEdgeCrossType = " << static_cast<int>(nOutLeftEdgeCrossType) << std::endl;
}
if (mark._rightRoadMark)
{
std::cout << "_rightRoadMark _type = " << mark._rightRoadMark->_type << std::endl;
if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID || mark._leftRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_SOLID)
nOutRightEdgeCrossType = EdgeCrossType::Unable;
else if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN)
nOutRightEdgeCrossType = EdgeCrossType::BidirecAble;
else if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_SOLID_BROKEN)
nOutRightEdgeCrossType = EdgeCrossType::LeftAble;
else if (mark._rightRoadMark->_type == ODR_ROAD_MARK_TYPE_BROKEN_SOLID)
nOutRightEdgeCrossType = EdgeCrossType::RightAble;
else
nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
std::cout << "nOutRightEdgeCrossType = " << static_cast<int>(nOutRightEdgeCrossType) << std::endl;
}
nOutSpeedLimit = 0;
double angle = g_map.GetHdgByXY(3926.381879, 11180.870534, dCarAngle);
std::cout << "GetHdgByXY comValue = " << dCarAngle << ",return = " << angle << std::endl;
dOutLaneAngle = angle;
// angle = g_map.GetHdgByXY(ptInLoc.dLon,ptInLoc.dLat,90.0);
// std::cout << "GetHdgByXY comValue = " << 90 << ",return = " << angle << std::endl;
// angle = g_map.GetHdgByXY(ptInLoc.dLon,ptInLoc.dLat,180.0);
// std::cout << "GetHdgByXY comValue = " << 180 << ",return = " << angle << std::endl;
return true;
}
} // namespace jf
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment