def warehouse_test():
def load_map_from_pic(path):
image = Image.open(path)
image = image.convert('L')
image = image.convert('1')
data = np.asarray(image)
data = 1 * data
data[0, :] = 0
data[-1, :] = 0
data[:, 0] = 0
data[:, -1] = 0
data = data + 1 - data * 2
return data
warehouse_map = map2D(load_map_from_pic("D:/repos/pythonPathSmooth/img/warehouse2_original.bmp"))
warehouse_balloon = map2D(load_map_from_pic("D:/repos/pythonPathSmooth/img/warehouse2_balloon.bmp"))
astar_solver = aStar(warehouse_map)
start_p = np.array([20, 20])
end_p = np.array([950, 850])
print("start astar searching on original map")
path = np.array(astar_solver.path_plan(start_p, end_p, visual=False, draw_count=100000))
path_y = path.T[0]
path_x = path.T[1]
plt.imshow(astar_solver.map2d.render_image())
plt.plot(path_x, path_y)
plt.show()
def no_astar():
test_2d_map = map2D(ware_house())
# plt_img = test_2d_map.render_image()
# plt.imshow(plt_img)
# plt.show()
test_2d_map_ball = map2D(ballooned_warehouse())
# plt_img = test_2d_map_ball.render_image()
# plt.imshow(plt_img)
# plt.show()
path = np.array([[20, 20], [25, 26], [26, 746], [120, 839], [160, 840],
[388, 1069], [389, 1159], [470, 1239], [710, 1240],
[710, 1241], [711, 1592], [737, 1619], [738, 1708],
[770, 1739], [960, 1740], [980, 1761], [980, 1980]])
path_out = [[20, 20], [25, 26], [26, 746], [120, 839], [160, 840],
[388, 1069], [389, 1159], [470, 1239], [710,
1240], [710, 1241],
[711, 1592], [737, 1619], [738, 1708], [770, 1739],
[960, 1740], [980, 1761], [980, 1980]]
path_y = np.array(path).T[0]
path_x = np.array(path).T[1]
print("calculate start.")
safety_ellipse = safetyEllipse(test_2d_map, 50)
cons_list, ellipse_list = safety_ellipse.ellipse_generate(path)
print("calculate finish, start rendering.")
img_result = safety_ellipse.map2d.render_image()
plt.imshow(img_result)
# plt.plot(path_x, path_y)
# for elli_stack in ellipse_list:
# for ellipse in elli_stack:
# single_ellipse = safety_ellipse.ellipse_mesh(ellipse)
# plt.plot(single_ellipse[0], single_ellipse[1])
plt.ylim(0, 1000)
plt.xlim(0, 2000)
plt.gca().invert_yaxis()
# plt.savefig('warehouse_no_ellipse.png', dpi=400)
plt.show()
plt.imshow(img_result)
for elli_stack in ellipse_list:
for ellipse in elli_stack:
single_ellipse = safety_ellipse.ellipse_mesh(ellipse)
plt.plot(single_ellipse[0], single_ellipse[1], linewidth=0.3)
path_y = np.array(path).T[0]
path_x = np.array(path).T[1]
plt.ylim(0, 1000)
plt.xlim(0, 2000)
plt.plot(path_x, path_y, linewidth=0.5)
plt.gca().invert_yaxis()
plt.savefig('warehouse_ellipse.png', dpi=400)
plt.show()
def main():
test_2d_map = map2D(test_map2)
# plt_img = test_2d_map.render_image()
# plt.imshow(plt_img)
# plt.show()
path = [
np.array([20, 20]),
np.array([350, 50]),
np.array([400, 400]),
np.array([500, 550])
]
path_y = np.array(path).T[0]
path_x = np.array(path).T[1]
safety_ellipse = safetyEllipse(test_2d_map, 50)
cons_list, ellipse_list = safety_ellipse.ellipse_generate(path)
img_result = safety_ellipse.map2d.render_image()
plt.imshow(img_result)
plt.plot(path_x, path_y)
for elli_stack in ellipse_list:
for ellipse in elli_stack:
single_ellipse = safety_ellipse.ellipse_mesh(ellipse)
plt.plot(single_ellipse[0], single_ellipse[1])
plt.plot(path_x, path_y)
plt.ylim(0, 600)
plt.xlim(0, 600)
plt.gca().invert_yaxis()
plt.show()
def main():
test_2d_map = map2D(test_map2)
plt_img = test_2d_map.render_image()
plt.imshow(plt_img)
plt.show()
astar_solver = aStar(test_2d_map)
start_p = np.array([20, 20])
end_p = np.array([570, 570])
path = np.array(astar_solver.path_plan(start_p, end_p, visual=False))
filter_path = np.array(astar_solver.line_fitter(path))
path_y = filter_path.T[0]
path_x = filter_path.T[1]
img_result = astar_solver.map2d.render_image()
plt.imshow(img_result)
plt.plot(path_x, path_y)
plt.show()
print(filter_path)
data = 1 * data
data[0, :] = 0
data[-1, :] = 0
data[:, 0] = 0
data[:, -1] = 0
data = data + 1 - data * 2
print(data)
plt.imshow(data)
# plt.savefig("warehouse2.png", dpi=1600)
plt.show()
return data
def shrinkMap(shrink_param=5):
path = r"./img/warehouser2_balloon.bmp"
image = cv2.imread(path)
image = cv2.dilate(image, np.ones((5, 5)), iterations=shrink_param)
cv2.imwrite("./img/warehouse2_shrink.bmp", image)
b, g, r = cv2.split(image)
image = cv2.merge([r, g, b])
image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
plt.imshow(image)
plt.show()
if __name__ == '__main__':
warehouse_map = map2D(load_map_from_pic("./img/warehouse2_original.bmp"))
warehouse_balloon = map2D(
load_map_from_pic("./img/warehouse2_balloon.bmp"))
shrinkMap()
def main():
test_2d_map = map2D(ware_house())
# plt_img = test_2d_map.render_image()
# plt.imshow(plt_img)
# plt.show()
test_2d_map_ball = map2D(ballooned_warehouse())
# plt_img = test_2d_map_ball.render_image()
# plt.imshow(plt_img)
# plt.show()
# init astar
astar_solver = aStar(test_2d_map_ball)
start_p = np.array([20, 20])
end_p = np.array([980, 1980])
print("start astar searching")
path = np.array(
astar_solver.path_plan(start_p, end_p, visual=False,
draw_count=100000))
# find the filtered path
filter_path = np.array(astar_solver.line_fitter(path))
filter_path = np.array([[20, 20], [25, 26], [26, 746], [120, 839],
[160, 840], [388, 1069], [389, 1159], [470, 1239],
[710, 1240], [710, 1241], [711, 1592], [737, 1619],
[738, 1708], [770, 1739], [960, 1740], [980, 1761],
[980, 1980]])
# render the path and the final result
path_y = filter_path.T[0]
path_x = filter_path.T[1]
sio.savemat("astar_warehouse_ballooned.mat",
{'astar_result_ballooned': test_2d_map_ball.color_stat})
img_result = astar_solver.map2d.render_image()
plt.imshow(img_result)
plt.plot(path_x, path_y)
plt.savefig('warehouse_astar.png', dpi=400)
plt.show()
# print final result
print("Filtered path: {}".format(filter_path))
path = filter_path
path_y = np.array(path).T[0]
path_x = np.array(path).T[1]
safety_ellipse = safetyEllipse(test_2d_map, 50)
cons_list, ellipse_list = safety_ellipse.ellipse_generate(path)
print("calculate finish, start rendering.")
img_result = safety_ellipse.map2d.render_image()
plt.imshow(img_result)
# for elli_stack in ellipse_list:
# for ellipse in elli_stack:
# single_ellipse = safety_ellipse.ellipse_mesh(ellipse)
# plt.plot(single_ellipse[0], single_ellipse[1])
plt.plot(path_x, path_y)
plt.ylim(0, 1000)
plt.xlim(0, 2000)
plt.gca().invert_yaxis()
plt.savefig('warehouse_ellipse.png', dpi=400)
plt.show()
plt.imshow(img_result)
for elli_stack in ellipse_list:
for ellipse in elli_stack:
single_ellipse = safety_ellipse.ellipse_mesh(ellipse)
plt.plot(single_ellipse[0], single_ellipse[1])
plt.plot(path_x, path_y)
plt.ylim(0, 1000)
plt.xlim(0, 2000)
plt.gca().invert_yaxis()
plt.savefig('warehouse_ellipse_no_ellipse.png', dpi=400)
plt.show()