fix bugs in check a position inside the polygon (AtsushiSakai#701)

* fix bugs in check a position inside the polygon

It is possible that the position `(iox, ioy)` to be checked with `iox == min_x`. This will cause a incorrect inside check.

* add a boundary condition for gridmap

Author: jsbyysheng

Mapping/grid_map_lib/grid_map_lib.py

@@ -194,7 +194,7 @@ def check_inside_polygon(iox, ioy, x, y):
                 min_x, max_x = x[i2], x[i1]
             else:
                 min_x, max_x = x[i1], x[i2]
-            if not min_x < iox < max_x:
+            if not min_x <= iox < max_x:
                 continue
 
             tmp1 = (y[i2] - y[i1]) / (x[i2] - x[i1])
@@ -226,8 +226,8 @@ def plot_grid_map(self, ax=None):
 
 
 def test_polygon_set():
-    ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
-    oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
+    ox = [0.0, 4.35, 20.0, 50.0, 100.0, 130.0, 40.0]
+    oy = [0.0, -4.15, -20.0, 0.0, 30.0, 60.0, 80.0]
 
     grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
 

tests/test_grid_map_lib.py

@@ -14,8 +14,8 @@ def test_position_set():
 
 
 def test_polygon_set():
-    ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
-    oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
+    ox = [0.0, 4.35, 20.0, 50.0, 100.0, 130.0, 40.0]
+    oy = [0.0, -4.15, -20.0, 0.0, 30.0, 60.0, 80.0]
 
     grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)