Skip to content

T
thirdparty_map
Commit 9d75ba3e authored by oscar's avatar oscar
Browse files
Options

提交四维地图接口的更新修改

parent a61eb045
Hide whitespace changes
Inline Side-by-side
Showing with 97 additions and 27 deletions
include/MapInterface.hpp
View file @ 9d75ba3e
......@@ -74,7 +74,7 @@ namespace jf {
bool GetMapData(const Point& ptInLoc, double dCarAngle, int& nOutLaneCnt, int& nOutLaneNum, LaneType& nOutLaneType, EdgeCrossType& nOutLeftEdgeCrossType, EdgeCrossType& nOutRightEdgeCrossType, int& nOutSpeedLimit, double& dOutLaneAngle,
Point& ptOutFoot);
bool GetMapData(const Point& ptInLoc, double dCarAngle, long& lOutRaodId, std::vector<long>& vctlOutPreRoadId, std::vector<long>& vctlOutNxtRoadId, int& nLaneCnt, int& nOutLaneNum, LaneType& nOutLaneType, EdgeCrossType& nOutLeftEdgeCrossType,
EdgeCrossType& nOutRightEdgeCrossType, int& nOutSpeedLimit, double& dOutLaneAngle, Point& ptOutFoot);
EdgeCrossType& nOutRightEdgeCrossType, int& nOutSpeedLimit, double& dOutLaneAngle, Point& ptOutFoot,int& id);
bool GetMapIDS(const Point& ptInLoc,double dCarAngle, int& id);
};
......
src/MapInterface.cpp
View file @ 9d75ba3e
#include "MapInterface.hpp"
#include "OdrManager.h"
#include "Component.h"
using namespace OpenDrive;
......@@ -27,15 +28,18 @@ namespace jf {
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);
int id;
return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot,id);
}
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)
EdgeCrossType& nOutLeftEdgeCrossType, EdgeCrossType& nOutRightEdgeCrossType, int& nOutSpeedLimit, double& dOutLaneAngle, Point& ptOutFoot,int& id)
{
if (g_load == 0)
return false;
uint64_t begin = GetCurTime();
uint64_t start = GetCurTime();
uint64_t end1,end2,end3,end4,end5;
#ifdef _DEBUG_API_
std::vector<OdrInfo> odr = g_map.GetOdrInfoByXY(ptInLoc.dLon, ptInLoc.dLat);
printf("odr size = %d\n", odr.size());
......@@ -46,39 +50,68 @@ namespace jf {
#else
std::vector<OdrInfo> odr = g_map.GetOdrInfo(ptInLoc.dLon, ptInLoc.dLat);
#endif
end1 = GetCurTime() - begin;
begin = GetCurTime();
if (odr.size() == 0)
{
printf("not 2 in map lon = %.8f, lat = %.8f\n", ptInLoc.dLon, ptInLoc.dLat);
return false;
}
std::string roadId;
std::string sectionId;
std::string laneId;
#ifdef _DEBUG_API_
double angle = g_map.GetHdgByXY(3926.381879, 11180.870534, dCarAngle, roadId);
std::cout << "GetHdgByXY comValue = " << dCarAngle << ",return = " << angle << ",roadId = " << roadId << std::endl;
#else
double carAngle = (90 - dCarAngle)/180*3.1415926;
double angle = g_map.GetHdg(ptInLoc.dLon, ptInLoc.dLat, dCarAngle, roadId);
//double angle = g_map.GetHdg(ptInLoc.dLon, ptInLoc.dLat, dCarAngle, roadId);
long int roadLaneId = 0;
int rett = g_map.GetHdg(odr,carAngle,roadLaneId);
if(rett)
id = roadLaneId;
#endif
dOutLaneAngle = -(angle*180/3.1415926 - 90);
if(dOutLaneAngle < 0)
dOutLaneAngle += 360;
std::string sectionId;
std::string laneId;
for (auto its : odr)
double angle = 0;
double min_angle = 3.14159265;
for (auto its : odr)
{
std::string id = its._roadID;
if (id == roadId)
{
sectionId = its._sectionID;
laneId = its._laneID;
break;
}
//std::string id = its._roadID;
double roadAngle = 0;
roadAngle = g_map.GetDirection(its._roadID,its._s);
printf("GetDirection roadAngle = %f, carAngle = %f\n",roadAngle,carAngle);
double tmp = roadAngle - carAngle;
if(tmp < -3.14159265)
tmp += 2*3.14159265;
if(tmp > 3.14159265)
tmp -= 2*3.14159265;
double deta = abs(tmp);
if(deta < min_angle)
{
min_angle = deta;
angle = roadAngle;
roadId = its._roadID;
sectionId = its._sectionID;
laneId = its._laneID;
}
// if (id == roadId)
//{
// sectionId = its._sectionID;
// laneId = its._laneID;
// break;
//}
}
dOutLaneAngle = -(angle*180/3.1415926 - 90);
if(dOutLaneAngle < 0)
dOutLaneAngle += 360;
end2 = GetCurTime() - begin;
begin = GetCurTime();
if(roadId == "")
return false;
lOutRaodId = std::stol(roadId);
int _sectionId = sectionId == "" ? 0 : std::stol(sectionId);
int _laneId = laneId == "" ? 0: std::stol(laneId);
//printf("GetHdg laneId = %d, dCarAngle = %f, carAngle = %f, angle = %f, dOutLandAngle = %f,ptInLoc.dLon = %.8f, ptInLoc.dLat = %.8f\n",_laneId,dCarAngle,carAngle,angle,dOutLaneAngle,ptInLoc.dLon, ptInLoc.dLat);
printf("GetHdg laneId = %d, dCarAngle = %f, carAngle = %f, angle = %f, dOutLandAngle = %f,ptInLoc.dLon = %.8f, ptInLoc.dLat = %.8f\n",_laneId,dCarAngle,carAngle,angle,dOutLaneAngle,ptInLoc.dLon, ptInLoc.dLat);
#ifdef _DEBUG_API_
std::cout << "lOutRaodId = " << lOutRaodId << ",_sectionId = " << _sectionId << ", _laneId = " << _laneId << std::endl;
#endif
......@@ -133,7 +166,8 @@ namespace jf {
}
nOutLaneCnt = allLaneSize;
nOutLaneNum = abs(_laneId);
end3 = GetCurTime() - begin;
begin = GetCurTime();
std::string typeString = g_map.GetLaneTypeString(roadId.c_str(), sectionId.c_str(), laneId.c_str());
#ifdef _DEBUG_API_
std::cout << "GetLaneTypeString type = " << typeString << std::endl;
......@@ -175,7 +209,6 @@ namespace jf {
#ifdef _DEBUG_API_
std::cout << "nOutLaneType = " << static_cast<int>(nOutLaneType) << std::endl;
#endif
bool isJunction = g_map.bJunctionRoad(roadId.c_str());
#ifdef _DEBUG_API_
std::cout << "bJunctionRoad = " << isJunction << std::endl;
......@@ -185,10 +218,13 @@ namespace jf {
std::cout << "bJunctionRoad is true" << std::endl;
nOutLaneType = LaneType::JunctionLane;
}
end4 = GetCurTime() - begin;
begin = GetCurTime();
#ifdef _DEBUG_API_
NearestRoadMark mark = g_map.GetNeartestLaneRoadMarkByXY(ptInLoc.dLon, ptInLoc.dLat);
#else
NearestRoadMark mark = g_map.GetNeartestLaneRoadMark(ptInLoc.dLon, ptInLoc.dLat);
//NearestRoadMark mark = g_map.GetNeartestLaneRoadMark(ptInLoc.dLon, ptInLoc.dLat);
NearestRoadMark mark = g_map.GetNeartestLaneRoadMarkByID(roadId.c_str(), sectionId.c_str(), laneId.c_str());
#endif
if (mark._leftRoadMark)
{
......@@ -203,7 +239,8 @@ namespace jf {
else
nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
#ifdef _DEBUG_API_
std::cout << "_leftRoadMark _type = " << mark._leftRoadMark->_type << std::endl;
std::cout << "_leftRoadMark _type = " << mark._leftRoadMark->_type << std::endl;
std::cout << "nOutLeftEdgeCrossType = " << static_cast<int>(nOutLeftEdgeCrossType) << std::endl;
#endif
}
......@@ -224,7 +261,38 @@ namespace jf {
std::cout << "nOutRightEdgeCrossType = " << static_cast<int>(nOutRightEdgeCrossType) << std::endl;
#endif
}
end5 = GetCurTime() - begin;
begin = GetCurTime();
uint64_t ddd = begin - start;
static int count_t = 0;
static int count_1 = 0;
static int count_5 = 0;
static int count_30 = 0;
static int count_100 = 0;
static int count_more = 0;
count_t++;
if(ddd <= 1000)
count_1++;
else if( ddd < 5000)
count_5++;
else if( ddd < 30000)
count_30++;
else if(ddd < 100000)
count_100++;
else
count_more++;
if(count_t >= 1000)
{
printf("GetMapDataTime frequency totel = %d, <1 = %d, 1-5 = %d, 5-30 = %d, 30-100 = %d, >100 = %d \n",count_t,count_1,count_5,count_30,count_100,count_more);
count_t = 0;
count_1 = 0;
count_5 = 0;
count_30 = 0;
count_100 = 0;
count_more = 0;
}
if(ddd > 0)
printf("GetMapDataTime lon=%.8f,lat=%.8f,carAngle=%f,toteltime=%llu, GetOdrInfo=%llu,GetHdg=%llu,GetPreRoadID&GetSucRoadID&GetAllDrivingLaneIDS=%llu,GetLaneTypeString&bJunctionRoad=%llu,GetNeartestLaneRoadMark=%llu\n",ptInLoc.dLon, ptInLoc.dLat,carAngle,begin - start,end1,end2,end3,end4,end5);
nOutSpeedLimit = 0;
// angle = g_map.GetHdgByXY(ptInLoc.dLon,ptInLoc.dLat,90.0);
// std::cout << "GetHdgByXY comValue = " << 90 << ",return = " << angle << std::endl;
......@@ -233,7 +301,7 @@ namespace jf {
return true;
}
bool MapInterface::GetMapIDS(const Point& ptInLoc, double dCarAngle,int& id)
/* bool MapInterface::GetMapIDS(const Point& ptInLoc, double dCarAngle,int& id)
{
std::vector<OdrInfo> odr = g_map.GetOdrInfo(ptInLoc.dLon, ptInLoc.dLat);
if (odr.size() == 0)
......@@ -265,6 +333,6 @@ namespace jf {
// printf("GetMapIDS result = %d, nSize = %d, id = %d,ptInLoc.dLon = %.8f, ptInLoc.dLat = %.8f\n",result,nSize,id,ptInLoc.dLon, ptInLoc.dLat);
return true;
}
*/
} // namespace jf
src/SDK/arm/include/OdrManager.h
View file @ 9d75ba3e
......@@ -53,6 +53,7 @@ namespace OpenDrive
NearestRoadMark GetNeartestLaneRoadMark(const double & longitude, const double &latitude);
NearestRoadMark GetNeartestLaneRoadMarkByXY(const double &x, const double &y);
NearestRoadMark GetNeartestLaneRoadMarkByID(const char* roadID, const char* sectionID, const char* laneID);
bool GetEdgePolygon(const double &longitude, const double &latitude, const double &radius, PosXY**& pos, int &nSize, int *&length);
bool GetEdgePolygonByXY(const double &x, const double &y, const double &radius, PosXY**& pos, int &nSize, int *&length);
//const RoadProperty GetLaneProperty(const vector<Position> &vecPosition) const;
......@@ -107,7 +108,8 @@ namespace OpenDrive
bool GetHdgByXY(const double &x, const double &y, const double &comValue, long int &roadIDlaneID);
bool GetAllJunctionRoadIDS(const double & longitude, const double &latitude, char**&vecRoad, int &nSize);
bool GetAllJunctionRoadIDSByXY(const double & x, const double &y, char**&vecRoad, int &nSize);
double GetDirection(const char* roadID,const double &s);
double GetDirection(const char* roadID,const double &s);
bool GetHdg(const vector<OdrInfo> &vecOdr, const double &comValue, long int &roadIDlaneID);
~OdrManager();
private:
vector<vector<glm::dvec3>> GetPrePolygon(const double &s, Road* road);
......
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