Fix potential field planning without obstacles

PathPlanning/PotentialFieldPlanning/potential_field_planning.py

@@ -24,10 +24,12 @@
 
 
 def calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):
-    minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0
-    miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0
-    maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0
-    maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0
+    x_coords = [sx, gx, *ox]
+    y_coords = [sy, gy, *oy]
+    minx = min(x_coords) - AREA_WIDTH / 2.0
+    miny = min(y_coords) - AREA_WIDTH / 2.0
+    maxx = max(x_coords) + AREA_WIDTH / 2.0
+    maxy = max(y_coords) + AREA_WIDTH / 2.0
     xw = int(round((maxx - minx) / reso))
     yw = int(round((maxy - miny) / reso))
 
@@ -52,6 +54,9 @@ def calc_attractive_potential(x, y, gx, gy):
 
 
 def calc_repulsive_potential(x, y, ox, oy, rr):
+    if len(ox) == 0:
+        return 0.0
+
     # search nearest obstacle
     minid = -1
     dmin = float("inf")

tests/test_potential_field_planning.py

@@ -1,4 +1,5 @@
 import conftest  # Add root path to sys.path
+import numpy as np
 from PathPlanning.PotentialFieldPlanning import potential_field_planning as m
 
 
@@ -7,5 +8,28 @@ def test1():
     m.main()
 
 
+def test_obstacle_free_potential_field():
+    # A free map has only attractive potential; there is no nearest obstacle.
+    gx, gy, sx, sy = 5.0, 5.0, 0.0, 0.0
+    assert m.calc_repulsive_potential(1.0, 2.0, [], [], 1.0) == 0.0
+
+    field, minx, miny = m.calc_potential_field(
+        gx, gy, [], [], 1.0, 1.0, sx, sy
+    )
+    assert np.isfinite(field).all()
+    np.testing.assert_allclose(
+        field[0][0], m.calc_attractive_potential(minx, miny, gx, gy)
+    )
+
+
+def test_obstacle_free_planning():
+    m.show_animation = False
+    sx, sy, gx, gy = 0.0, 0.0, 5.0, 5.0
+    rx, ry = m.potential_field_planning(sx, sy, gx, gy, [], [], 1.0, 1.0)
+
+    np.testing.assert_allclose([rx[0], ry[0]], [sx, sy])
+    assert np.hypot(rx[-1] - gx, ry[-1] - gy) < 1.0
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)