def plot_costmaps():
workspace = sample_circle_workspaces(nb_circles=4)
grid_sparse = workspace.box.stacked_meshgrid(24)
grid_dense = workspace.box.stacked_meshgrid(100)
extent = workspace.box.extent_data()
sdf = SignedDistanceWorkspaceMap(workspace)
viewer = WorkspaceDrawer(workspace,
wait_for_keyboard=True,
rows=1,
cols=2,
scale=1.)
viewer.set_drawing_axis(0)
viewer.set_workspace(workspace)
viewer.draw_ws_img(sdf(grid_dense).T)
viewer.draw_ws_obstacles()
viewer.set_drawing_axis(1)
viewer.set_workspace(workspace)
viewer.draw_ws_img(sdf(grid_sparse).T)
viewer.draw_ws_obstacles()
viewer.show_once()
diagonal = workspace.box.diag()
max_h = diagonal * boxes_height
min_h = diagonal * boxes_height * .5
max_w = diagonal * boxes_width
min_w = diagonal * boxes_width * .5
workspace.obstacles = []
while len(workspace.obstacles) < 5:
origin = workspace.box.sample_uniform()
h = (max_h - min_h) * np.random.rand() + min_h
w = (max_w - min_w) * np.random.rand() + min_w
if workspace.min_dist(origin)[0] < np.linalg.norm([h, w]):
continue
dimensions = np.array([w, h])
theta = np.pi * np.random.rand() - np.pi
orientation = rotation_matrix_2d_radian(theta)
workspace.obstacles.append(OrientedBox(origin, dimensions, orientation))
# Compute Occupancy map
matrix = occupancy_map(150, workspace)
# Compute Signed distance field
# meshgrid = workspace.box.stacked_meshgrid(150)
# matrix = SignedDistanceWorkspaceMap(workspace)(meshgrid).T
# Setup viewer
viewer = WorkspaceDrawer(workspace, wait_for_keyboard=True)
viewer.draw_ws_img(matrix)
viewer.draw_ws_obstacles()
viewer.show_once()