Skip to content

Commit 93bfc03

Browse files
committed
Alterações para gerar quadrado da sala no mapa como obstáculos.
1 parent b762fd0 commit 93bfc03

File tree

2 files changed

+75
-25
lines changed

2 files changed

+75
-25
lines changed

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

Lines changed: 32 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -177,25 +177,40 @@ def plot_arrow(x, y, yaw, length=0.5, width=0.1): # pragma: no cover
177177
head_length=width, head_width=width)
178178
plt.plot(x, y)
179179

180-
181-
def main(gx=10, gy=10):
180+
def create_square(dx, dy, px, py):
181+
# Cria um vetor descrevendo um quadrado com tamanho dx * dy na posição (px, py)
182+
square = []
183+
for x in range(dx):
184+
square.append([px + x, py + 0.0])
185+
square.append([px + x, py + dy])
186+
square.append([px + x + 0.5, py + 0.0])
187+
square.append([px + x + 0.5, py + dy])
188+
for y in range(dy):
189+
square.append([px + 0.0, py + y])
190+
square.append([px + dx, py + y])
191+
square.append([px + 0.0, py + y + 0.5])
192+
square.append([px + dx, py + y + 0.5])
193+
square.append([px + dx, py + dy])
194+
195+
return square
196+
a = [[1,2]]
197+
a = create_square(21, 16, 0, 0.0)
198+
a = a + create_square(2, 5, 4.0, 0.0)
199+
a = a + create_square(2, 5, 4.0, 11.0)
200+
a = a + create_square(2, 5, 9.0, 0.0)
201+
a = a + create_square(2, 5, 9.0, 11.0)
202+
a = a + create_square(2, 5, 14.0, 0.0)
203+
# a = a + create_square(2, 5, 9.0, 11.0)
204+
205+
206+
def main(gx=2, gy=13):
182207
print(__file__ + " start!!")
183208
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
184-
x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
209+
x = np.array([18, 1.0, math.pi / 8.0, 0.5, 0.5])
185210
# goal position [x(m), y(m)]
186211
goal = np.array([gx, gy])
187212
# obstacles [x(m) y(m), ....]
188-
ob = np.array([[-1, -1],
189-
[0, 2],
190-
[4.0, 2.0],
191-
[5.0, 4.0],
192-
[5.0, 5.0],
193-
[5.0, 6.0],
194-
[5.0, 9.0],
195-
[8.0, 9.0],
196-
[7.0, 9.0],
197-
[12.0, 12.0]
198-
])
213+
ob = np.array(a)
199214

200215
# input [forward speed, yawrate]
201216
u = np.array([0.0, 0.0])
@@ -213,11 +228,11 @@ def main(gx=10, gy=10):
213228
plt.plot(ptraj[:, 0], ptraj[:, 1], "-g")
214229
plt.plot(x[0], x[1], "xr")
215230
plt.plot(goal[0], goal[1], "xb")
216-
plt.plot(ob[:, 0], ob[:, 1], "ok")
231+
plt.plot(ob[:, 0], ob[:, 1], "ok", marker=".")
217232
plot_arrow(x[0], x[1], x[2])
218233
plt.axis("equal")
219-
plt.grid(True)
220-
plt.pause(0.0001)
234+
plt.grid(False)
235+
plt.pause(0.001)
221236

222237
# check reaching goal
223238
dist_to_goal = math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2)

PathPlanning/PotentialFieldPlanning/potential_field_planning.py

Lines changed: 43 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -138,19 +138,54 @@ def draw_heatmap(data):
138138
data = np.array(data).T
139139
plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)
140140

141+
def create_square(dx, dy, px, py):
142+
# Cria um vetor descrevendo um quadrado com tamanho dx * dy na posição (px, py)
143+
ox, oy = [], []
144+
for x in range(dx):
145+
ox.append(px + x)
146+
ox.append(px + x)
147+
ox.append(px + x + 0.5)
148+
ox.append(px + x + 0.5)
149+
oy.append(py + 0.0)
150+
oy.append(py + dy)
151+
oy.append(py + 0.0)
152+
oy.append(py + dy)
153+
for y in range(dy):
154+
ox.append(px + 0.0)
155+
ox.append(px + dx)
156+
ox.append(px + 0.0)
157+
ox.append(px + dx)
158+
oy.append(py + y)
159+
oy.append(py + y)
160+
oy.append(py + y + 0.5)
161+
oy.append(py + y + 0.5)
162+
ox.append(px + dx)
163+
oy.append(py + dy)
164+
165+
return ox, oy
141166

142167
def main():
143168
print("potential_field_planning start")
144169

145-
sx = 0.0 # start x position [m]
146-
sy = 10.0 # start y positon [m]
147-
gx = 30.0 # goal x position [m]
148-
gy = 30.0 # goal y position [m]
170+
sx = 35.0 # start x position [m]
171+
sy = 25.0 # start y positon [m]
172+
gx = 4.0 # goal x position [m]
173+
gy = 4.0 # goal y position [m]
149174
grid_size = 0.5 # potential grid size [m]
150-
robot_radius = 5.0 # robot radius [m]
151-
152-
ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m]
153-
oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m]
175+
robot_radius = 3.0 # robot radius [m]
176+
ox, oy = create_square(41, 31, 0, 0.0)
177+
ox1, oy1 = create_square(4, 10, 8.0, 0.0)
178+
ox, oy = ox + ox1, oy + oy1
179+
ox1, oy1 = create_square(4, 10, 8.0, 20.0)
180+
ox, oy = ox + ox1, oy + oy1
181+
ox1, oy1 = create_square(4, 10, 18.0, 0.0)
182+
ox, oy = ox + ox1, oy + oy1
183+
ox1, oy1 = create_square(4, 10, 18.0, 20.0)
184+
ox, oy = ox + ox1, oy + oy1
185+
ox1, oy1 = create_square(4, 10, 28.0, 0.0)
186+
ox, oy = ox + ox1, oy + oy1
187+
# ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m]
188+
# oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m]
154189

155190
if show_animation:
156191
plt.grid(True)

0 commit comments

Comments
 (0)