提交接口更新

python_so/MapInterface.cpp

@@ -18,15 +18,35 @@ bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, int &nOutL
     long lRoadId;
     std::vector<long> vctlPreRoadId = {};
     std::vector<long> vctlNxtRoadId = {};
+    int isInCross = 0;
+    return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot, isInCross);
+}
 
-    return GetMapData(ptInLoc, dCarAngle, lRoadId, vctlPreRoadId, vctlNxtRoadId, nOutLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot);
+//  The function will return YES if the point x,y is inside the polygon, or
+//  NO if it is not.  If the point is exactly on the edge of the polygon,
+//  then the function may return YES or NO.
+bool IsPointInPolygon(std::vector<std::vector<float>>& poly, Point& pt)
+{
+    int i, j;
+    bool c = false;
+    for (i = 0, j = poly.size() - 1; i < poly.size(); j = i++)
+    {
+        if ((((poly[i][1] <= pt.dLat) && (pt.dLat < poly[j][1])) ||
+            ((poly[j][1] <= pt.dLat) && (pt.dLat < poly[i][1])))
+            && (pt.dLon < (poly[j][0] - poly[i][0]) * (pt.dLat - poly[i][1]) / (poly[j][1] - poly[i][1]) + poly[i][0]))
+        {
+            c = !c;
+        }
+    }
+    return c;
 }
 
 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 &isInCross) {
     std::vector<HdLinkId> vcthlLinkIds = {};
     std::vector<HdLaneId> vcthlLaneIds = {};
     // Locate lane failed
+    isInCross = 0;
     if (false == m_mlMapLib.GetLocateLanes(ptInLoc, vcthlLinkIds, vcthlLaneIds)) {
         nOutLaneCnt = 0;
         nOutLaneNum = 0;
@@ -39,7 +59,37 @@ bool MapInterface::GetMapData(const Point &ptInLoc, double dCarAngle, long &lOut
         //std::cout << "MapInterface::GetMapData(), Locate lane failed." << std::endl;
         return false;
     }
-
+    std::vector<HdLaneId> vcthlLinkNearby = {};
+    if (false == m_mlMapLib.GetDataNearby(ptInLoc, 10.0f, 10.0f, vcthlLinkNearby))
+    {
+        nOutLaneCnt = 0;
+        nOutLaneNum = 0;
+        nOutLaneType = LaneType::NotInvestigated;
+        nOutLeftEdgeCrossType = EdgeCrossType::NotInvestigated;
+        nOutRightEdgeCrossType = EdgeCrossType::NotInvestigated;
+        nOutSpeedLimit = 0;
+        dOutLaneAngle = 0.0;
+        ptOutFoot = ptInLoc;
+        return false;
+    }
+    for (int i = 0; i < vcthlLinkNearby.size(); i++)
+    {
+        const HdLinkId& hlLinkId = vcthlLinkNearby[i];
+        const HdLink* cphlLink = m_mlMapLib.GetLinkData(hlLinkId);
+        if (cphlLink)
+        {
+            for (int j = 0; j < cphlLink->vctcrCrossing; j++)
+            {
+                if (IsPointInPolygon(cphlLink->vctcrCrossing[j]->vctvdGeoArea, ptInLoc))
+                {
+                    isInCross = 1;
+                    break;
+                }
+            }
+        }
+        if (isInCross == 1)
+            break;
+    }
     int nIndex = 0;
     double dMinAngleDiff = std::numeric_limits<double>::max();
     auto func_GetAngleDiff = [](double dAngle1, double dAngle2) { return std::min(std::abs(dAngle1 - dAngle2), 360 - std::abs((dAngle1 - dAngle2))); };

python_so/MapInterface.hpp

@@ -49,7 +49,7 @@ class MapInterface {
     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& isInCross);
 
    private:
     MapLib m_mlMapLib;

python_so/jfxmap_python.cpp

@@ -45,7 +45,8 @@ MapInfo* GetMapData(double lat, double lon, double carAngle)
 		int nOutSpeedLimit;
 		double dOutLaneAngle;
 		jf::Point ptOutFoot;
-		if (g_jfMap.GetMapData(ptInLoc, carAngle, lOutRaodId, vctlOutPreRoadId, vctlOutNxtRoadId, nLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot))
+		int isInCross = 0;
+		if (g_jfMap.GetMapData(ptInLoc, carAngle, lOutRaodId, vctlOutPreRoadId, vctlOutNxtRoadId, nLaneCnt, nOutLaneNum, nOutLaneType, nOutLeftEdgeCrossType, nOutRightEdgeCrossType, nOutSpeedLimit, dOutLaneAngle, ptOutFoot, isInCross))
 		{
 			g_info.isInMap = 1;
 			g_info.lOutRaodId = lOutRaodId;
@@ -60,6 +61,7 @@ MapInfo* GetMapData(double lat, double lon, double carAngle)
 			g_info.dOutLaneAngle = dOutLaneAngle;
 			g_info.ptOutFoot_lat = ptOutFoot.dLat;
 			g_info.ptOutFoot_lon = ptOutFoot.dLon;
+			g_info.isInCross = isInCross;
 		}
 	}
 	return &g_info;

python_so/jfxmap_python.h

@@ -23,6 +23,7 @@ struct MapInfo
     double dOutLaneAngle;
     double ptOutFoot_lat;
     double ptOutFoot_lon;
+    int isInCross;
 };
 
 int add(int i, int j);
@@ -49,7 +50,8 @@ PYBIND11_MODULE(libjfxmap_python, m)
         .def_readwrite("nOutSpeedLimit", &MapInfo::nOutSpeedLimit)
         .def_readwrite("dOutLaneAngle", &MapInfo::dOutLaneAngle)
         .def_readwrite("ptOutFoot_lat", &MapInfo::ptOutFoot_lat)
-        .def_readwrite("ptOutFoot_lon", &MapInfo::ptOutFoot_lon);
+        .def_readwrite("ptOutFoot_lon", &MapInfo::ptOutFoot_lon)
+        .def_readwrite("isInCross", &MapInfo::isInCross);
     // expose add function, and add keyword arguments and default arguments
     m.def("add", &add, "A function which adds two numbers", py::arg("i") = 1, py::arg("j") = 2);
     m.def("InitMap", &InitMap, "A function which init map", py::return_value_policy::copy);