keep implementing potentialfieldplanning

PathPlanning/PotentialFieldPlanning/potential_field_planning.py

@@ -7,16 +7,97 @@ author: Atsushi Sakai (@Atsushi_twi)
 
 """
 
-#  import numpy as np
+import numpy as np
 import matplotlib.pyplot as plt
 
 
+def calc_potential_field(gx, gy, reso):
+    minx = -10.0
+    miny = -10.0
+    maxx = 50.0
+    maxy = 50.0
+    xw = round(maxx - minx)
+    yw = round(maxy - miny)
+
+    pmap = [[0.0 for i in range(xw)] for i in range(yw)]
+
+    for ix in range(xw):
+        x = ix * reso + minx
+        for iy in range(yw):
+            y = iy * reso + miny
+
+            ug = np.hypot(x - gx, y - gy)
+            uo = 0.0
+            uf = ug + uo
+
+            pmap[ix][iy] = uf
+
+    #  print(pmap)
+
+    return pmap, minx, miny
+
+
+def get_motion_model():
+    # dx, dy
+    motion = [[1, 0],
+              [0, 1],
+              [-1, 0],
+              [0, -1],
+              [-1, -1],
+              [-1, 1],
+              [1, -1],
+              [1, 1]]
+
+    return motion
+
+
+def potential_field_planning(sx, sy, gx, gy, reso):
+
+    # calc potential field
+    pmap, minx, miny = calc_potential_field(gx, gy, reso)
+
+    # search path
+    d = np.hypot(sx - gx, sy - gy)
+    ix = round((sx - minx) / reso)
+    iy = round((sy - miny) / reso)
+    rx, ry = [sx], [sy]
+    motion = get_motion_model()
+    while d >= reso:
+        #  print(ix, iy)
+        #  input()
+        minp = float("inf")
+        minix, miniy = -1, -1
+        for i in range(len(motion)):
+            inx = ix + motion[i][0]
+            iny = iy + motion[i][1]
+            p = pmap[inx][iny]
+            if minp > p:
+                minp = p
+                minix = inx
+                miniy = iny
+        ix = minix
+        iy = miniy
+        xp = ix * reso + minx
+        yp = iy * reso + miny
+        d = np.hypot(gx - xp, gy - yp)
+        #  print(d, xp, yp)
+        rx.append(xp)
+        ry.append(yp)
+
+    return rx, ry
+
+
 def main():
 
     sx = 0.0
     sy = 10.0
     gx = 30.0
     gy = 30.0
+    grid_size = 2.0  # [m]
+
+    rx, ry = potential_field_planning(sx, sy, gx, gy, grid_size)
+
+    plt.plot(rx, ry, "-r")
 
     plt.plot(sx, sy, "*r")
     plt.plot(gx, gy, "*r")