Alterações para gerar quadrado da sala no mapa como obstáculos.

Author: ruanmed

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -177,25 +177,40 @@ def plot_arrow(x, y, yaw, length=0.5, width=0.1):  # pragma: no cover
               head_length=width, head_width=width)
     plt.plot(x, y)
 
-
-def main(gx=10, gy=10):
+def create_square(dx, dy, px, py):
+    # Cria um vetor descrevendo um quadrado com tamanho dx * dy na posição (px, py)
+    square = []
+    for x in range(dx):
+        square.append([px + x,       py + 0.0])
+        square.append([px + x,       py + dy])
+        square.append([px + x + 0.5, py + 0.0])
+        square.append([px + x + 0.5, py + dy])
+    for y in range(dy):
+        square.append([px + 0.0,    py + y])
+        square.append([px + dx,     py + y])
+        square.append([px + 0.0,    py + y + 0.5])
+        square.append([px + dx,     py + y + 0.5])
+    square.append([px + dx,     py + dy])
+
+    return square
+a = [[1,2]]
+a = create_square(21, 16, 0, 0.0)
+a = a + create_square(2, 5, 4.0, 0.0)
+a = a + create_square(2, 5, 4.0, 11.0)
+a = a + create_square(2, 5, 9.0, 0.0)
+a = a + create_square(2, 5, 9.0, 11.0)
+a = a + create_square(2, 5, 14.0, 0.0)
+# a = a + create_square(2, 5, 9.0, 11.0)
+
+
+def main(gx=2, gy=13):
     print(__file__ + " start!!")
     # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
-    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
+    x = np.array([18, 1.0, math.pi / 8.0, 0.5, 0.5])
     # goal position [x(m), y(m)]
     goal = np.array([gx, gy])
     # obstacles [x(m) y(m), ....]
-    ob = np.array([[-1, -1],
-                   [0, 2],
-                   [4.0, 2.0],
-                   [5.0, 4.0],
-                   [5.0, 5.0],
-                   [5.0, 6.0],
-                   [5.0, 9.0],
-                   [8.0, 9.0],
-                   [7.0, 9.0],
-                   [12.0, 12.0]
-                   ])
+    ob = np.array(a)
 
     # input [forward speed, yawrate]
     u = np.array([0.0, 0.0])
@@ -213,11 +228,11 @@ def main(gx=10, gy=10):
             plt.plot(ptraj[:, 0], ptraj[:, 1], "-g")
             plt.plot(x[0], x[1], "xr")
             plt.plot(goal[0], goal[1], "xb")
-            plt.plot(ob[:, 0], ob[:, 1], "ok")
+            plt.plot(ob[:, 0], ob[:, 1], "ok", marker=".")
             plot_arrow(x[0], x[1], x[2])
             plt.axis("equal")
-            plt.grid(True)
-            plt.pause(0.0001)
+            plt.grid(False)
+            plt.pause(0.001)
 
         # check reaching goal
         dist_to_goal = math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2)  

PathPlanning/PotentialFieldPlanning/potential_field_planning.py

@@ -138,19 +138,54 @@ def draw_heatmap(data):
     data = np.array(data).T
     plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)
 
+def create_square(dx, dy, px, py):
+    # Cria um vetor descrevendo um quadrado com tamanho dx * dy na posição (px, py)
+    ox, oy = [], []
+    for x in range(dx):
+        ox.append(px + x)
+        ox.append(px + x)
+        ox.append(px + x + 0.5)
+        ox.append(px + x + 0.5) 
+        oy.append(py + 0.0)
+        oy.append(py + dy)
+        oy.append(py + 0.0)
+        oy.append(py + dy)
+    for y in range(dy):
+        ox.append(px + 0.0)
+        ox.append(px + dx)
+        ox.append(px + 0.0)
+        ox.append(px + dx) 
+        oy.append(py + y)
+        oy.append(py + y)
+        oy.append(py + y + 0.5)
+        oy.append(py + y + 0.5)
+    ox.append(px + dx) 
+    oy.append(py + dy)
+
+    return ox, oy
 
 def main():
     print("potential_field_planning start")
 
-    sx = 0.0  # start x position [m]
-    sy = 10.0  # start y positon [m]
-    gx = 30.0  # goal x position [m]
-    gy = 30.0  # goal y position [m]
+    sx = 35.0  # start x position [m]
+    sy = 25.0  # start y positon [m]
+    gx = 4.0  # goal x position [m]
+    gy = 4.0  # goal y position [m]
     grid_size = 0.5  # potential grid size [m]
-    robot_radius = 5.0  # robot radius [m]
-
-    ox = [15.0, 5.0, 20.0, 25.0]  # obstacle x position list [m]
-    oy = [25.0, 15.0, 26.0, 25.0]  # obstacle y position list [m]
+    robot_radius = 3.0  # robot radius [m]
+    ox, oy = create_square(41, 31, 0, 0.0)
+    ox1, oy1 = create_square(4, 10, 8.0, 0.0)
+    ox, oy = ox + ox1, oy + oy1
+    ox1, oy1 = create_square(4, 10, 8.0, 20.0)
+    ox, oy = ox + ox1, oy + oy1
+    ox1, oy1 = create_square(4, 10, 18.0, 0.0)
+    ox, oy = ox + ox1, oy + oy1
+    ox1, oy1 = create_square(4, 10, 18.0, 20.0)
+    ox, oy = ox + ox1, oy + oy1
+    ox1, oy1 = create_square(4, 10, 28.0, 0.0)
+    ox, oy = ox + ox1, oy + oy1
+    # ox = [15.0, 5.0, 20.0, 25.0]  # obstacle x position list [m]
+    # oy = [25.0, 15.0, 26.0, 25.0]  # obstacle y position list [m]
 
     if show_animation:
         plt.grid(True)