diff --git a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py b/PathPlanning/PotentialFieldPlanning/potential_field_planning.py index 603a9d16cf..c8cafc1288 100644 --- a/PathPlanning/PotentialFieldPlanning/potential_field_planning.py +++ b/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") diff --git a/tests/test_potential_field_planning.py b/tests/test_potential_field_planning.py index ce178d793d..d8d376a12f 100644 --- a/tests/test_potential_field_planning.py +++ b/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__)