Potential field - potential range and ocillations (AtsushiSakai#345)

* Potential cover start and goal as well

* Oscillation detection and fix potential look if outside range

* Formatting

* cleaner and flexible oscillation detection

Co-authored-by: Adi Vardi <[email protected]>

Author: adivardi

PathPlanning/PotentialFieldPlanning/potential_field_planning.py

@@ -9,22 +9,25 @@
 
 """
 
+from collections import deque
 import numpy as np
 import matplotlib.pyplot as plt
 
 # Parameters
 KP = 5.0  # attractive potential gain
 ETA = 100.0  # repulsive potential gain
 AREA_WIDTH = 30.0  # potential area width [m]
+# the number of previous positions used to check oscillations
+OSCILLATIONS_DETECTION_LENGTH = 3
 
 show_animation = True
 
 
-def calc_potential_field(gx, gy, ox, oy, reso, rr):
-    minx = min(ox) - AREA_WIDTH / 2.0
-    miny = min(oy) - AREA_WIDTH / 2.0
-    maxx = max(ox) + AREA_WIDTH / 2.0
-    maxy = max(oy) + AREA_WIDTH / 2.0
+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
     xw = int(round((maxx - minx) / reso))
     yw = int(round((maxy - miny) / reso))
 
@@ -84,10 +87,26 @@ def get_motion_model():
     return motion
 
 
+def oscillations_detection(previous_ids, ix, iy):
+    previous_ids.append((ix, iy))
+
+    if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):
+        previous_ids.popleft()
+
+    # check if contains any duplicates by copying into a set
+    previous_ids_set = set()
+    for index in previous_ids:
+        if index in previous_ids_set:
+            return True
+        else:
+            previous_ids_set.add(index)
+    return False
+
+
 def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
 
     # calc potential field
-    pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr)
+    pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)
 
     # search path
     d = np.hypot(sx - gx, sy - gy)
@@ -106,14 +125,17 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
 
     rx, ry = [sx], [sy]
     motion = get_motion_model()
+    previous_ids = deque()
+
     while d >= reso:
         minp = float("inf")
         minix, miniy = -1, -1
         for i, _ in enumerate(motion):
             inx = int(ix + motion[i][0])
             iny = int(iy + motion[i][1])
-            if inx >= len(pmap) or iny >= len(pmap[0]):
+            if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:
                 p = float("inf")  # outside area
+                print("outside potential!")
             else:
                 p = pmap[inx][iny]
             if minp > p:
@@ -128,6 +150,10 @@ def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
         rx.append(xp)
         ry.append(yp)
 
+        if (oscillations_detection(previous_ids, ix, iy)):
+            print("Oscillation detected at ({},{})!".format(ix, iy))
+            break
+
         if show_animation:
             plt.plot(ix, iy, ".r")
             plt.pause(0.01)