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

include/MapInterface.hpp

@@ -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

 
 #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

@@ -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);