def show_multiple(costmaps,
                  original_costmaps,
                  workspace,
                  show_result,
                  weights=None,
                  centers=None,
                  starts=None,
                  targets=None,
                  paths=None,
                  ex_paths=None,
                  title=None,
                  directory=None):
    """ Show multiple maps with demonstrations and example paths
        plot all demonstrations and example paths of one iteration in one map
    """
    pixel_map = workspace.pixel_map(original_costmaps[0].shape[0])
    cols = max(math.ceil(math.sqrt(len(costmaps))), len(original_costmaps) + 1)
    rows = math.ceil(len(costmaps) / cols) + 1
    viewer = render.WorkspaceDrawer(workspace,
                                    wait_for_keyboard=True,
                                    rows=rows,
                                    cols=cols,
                                    scale=rows / (rows * cols))
    for i in range(cols):
        viewer.set_drawing_axis(i)
        if i < len(original_costmaps):
            viewer.draw_ws_img(original_costmaps[i], interpolate="none")
            # viewer._ax.set_title('Training Costmap', size=32 / cols)
        viewer.remove_axis()

    for i in range(cols, rows * cols):
        viewer.set_drawing_axis(i)
        if i < len(costmaps) + cols:
            viewer.draw_ws_img(costmaps[i - cols], interpolate="none")
            # viewer._ax.set_title('Learned Costmap: \n {}.'.format(i + 1 - cols),
            #                     size=32 / cols)
            if weights is not None:
                show_weights(viewer, weights[i - cols], centers)
            if paths is not None:
                show_paths(paths, pixel_map, starts, targets, viewer)
            if ex_paths is not None:
                show_paths(ex_paths[i - cols], pixel_map, starts, targets,
                           viewer)
        viewer.remove_axis()

    # viewer._colorbar.ax.tick_params(labelsize=FONTSIZE)
    viewer._fig.tight_layout()

    if title is not None:
        viewer.set_title('\n'.join(wrap(title, 60)), fontsize=32)
    if show_result == 'SHOW':
        viewer.show_once()
    elif show_result == 'SAVE':
        viewer.save_figure(directory)
def show_predictions(costmap,
                     original_costmap,
                     workspace,
                     show_result,
                     starts=None,
                     targets=None,
                     paths=None,
                     ex_paths=None,
                     title=None,
                     directory=None):
    """ Show multiple maps with demonstrations and example paths """
    pixel_map = workspace.pixel_map(original_costmap.shape[0])
    cols = min(5, len(ex_paths))
    rows = math.ceil(len(ex_paths) / 5) + 1
    viewer = render.WorkspaceDrawer(workspace,
                                    wait_for_keyboard=True,
                                    rows=rows,
                                    cols=cols,
                                    scale=rows / (rows * cols))
    viewer.draw_ws_img(original_costmap, interpolate="none")
    # viewer._ax.set_title('Training Costmap', size=32 / cols)
    for i in range(cols):
        viewer.set_drawing_axis(i)
        viewer.remove_axis()

    for i in range(cols, rows * cols):
        viewer.set_drawing_axis(i)
        if i < len(ex_paths) + cols:
            viewer.draw_ws_img(costmap, interpolate="none")

            if paths is not None:
                show_paths([paths[i - cols]], pixel_map, [starts[i - cols]],
                           [targets[i - cols]], viewer)
            if ex_paths is not None:
                show_paths([ex_paths[i - cols]], pixel_map, [starts[i - cols]],
                           [targets[i - cols]], viewer)
        viewer.remove_axis()
    viewer._fig.tight_layout()

    if title is not None:
        viewer.set_title('\n'.join(wrap(title, 60)), fontsize=32)
    #viewer._colorbar.ax.tick_params(labelsize=FONTSIZE)
    if show_result == 'SHOW':
        viewer.show_once()
    elif show_result == 'SAVE':
        viewer.save_figure(directory + '.pdf')
def show(costmap,
         workspace,
         show_result,
         starts=None,
         targets=None,
         paths=None,
         ex_paths=None,
         weights=None,
         d=None,
         predecessors=None,
         title=None,
         directory=None,
         centers=None):
    """ Show a single map with demonstrations and example paths
    """
    viewer = render.WorkspaceDrawer(workspace, wait_for_keyboard=True)
    viewer.draw_ws_img(costmap, interpolate="none")
    viewer._colorbar.ax.tick_params(labelsize=FONTSIZE)
    pixel_map = workspace.pixel_map(costmap.shape[0])

    if paths is not None:
        show_paths(paths, pixel_map, starts, targets, viewer)

    if ex_paths is not None:
        show_paths(ex_paths, pixel_map, starts, targets, viewer)

    if weights is not None:
        show_weights(viewer, weights, centers)

    if d is not None:
        show_D(d, pixel_map, viewer)

    if predecessors is not None:
        show_policy(pixel_map, predecessors, viewer)

    viewer.remove_axis()

    if title is not None:
        viewer.set_title('\n'.join(wrap(title, 60)))

    if show_result == 'SHOW':
        viewer.show_once()
    elif show_result == 'SAVE':
        viewer.save_figure(directory)
    for i in range(10):
        s_w = sample_collision_free(workspace)
        t_w = sample_collision_free(workspace)
        s = pixel_map.world_to_grid(s_w)
        t = pixel_map.world_to_grid(t_w)
        try:
            print("planning...")
            time_0 = time.time()
            path = converter.dijkstra_on_map(costmap, s[0], s[1], t[0], t[1])
        except Exception as e:
            continue
        paths.append(path)
        starts.append(s_w)
        targets.append(t_w)
        print("took t : {} sec.".format(time.time() - time_0))

    if show_result:
        viewer = render.WorkspaceDrawer(workspace, wait_for_keyboard=True)
        viewer.draw_ws_img(costmap, interpolate="none")
        for s_w, t_w, path in zip(starts, targets, paths):
            trajectory = [None] * len(path)
            for i, p in enumerate(path):
                trajectory[i] = pixel_map.grid_to_world(np.array(p))
            c = cmap(np.random.rand())
            viewer.draw_ws_line(trajectory, color=c)
            viewer.draw_ws_point(s_w, color=c)
            viewer.draw_ws_point(t_w, color=c)
        viewer.show_once()

print(path)
def show_esf_maps(costmaps,
                  original_costmap,
                  workspace,
                  show_result,
                  esf,
                  loss_aug_maps,
                  loss_aug_esf,
                  weights=None,
                  centers=None,
                  starts=None,
                  targets=None,
                  paths=None,
                  ex_paths=None,
                  title=None,
                  directory=None):
    """ show the costmap with its log augmentation as well as the corresponding
        expected state frequencies
    """
    pixel_map = workspace.pixel_map(original_costmap.shape[0])
    cols = 4
    rows = len(costmaps) + 1
    viewer = render.WorkspaceDrawer(workspace,
                                    wait_for_keyboard=True,
                                    rows=rows,
                                    cols=cols,
                                    scale=rows / (rows * cols))
    viewer.draw_ws_img(original_costmap, interpolate="none")
    viewer._ax.set_title('Training Costmap', size=32 / cols)
    for i in range(cols):
        viewer.set_drawing_axis(i)
        viewer.remove_axis()

    for i in range(1, rows):
        viewer.set_drawing_axis(i * cols)
        viewer.remove_axis()
        viewer.draw_ws_img(costmaps[i - 1], interpolate="none")
        viewer._ax.set_title('learned costmap: \n {}.'.format(i),
                             size=32 / cols)
        if weights is not None and centers is not None:
            show_weights(viewer, weights[i - 1], centers)
        viewer.set_drawing_axis(i * cols + 1)
        viewer.remove_axis()
        viewer.draw_ws_img(loss_aug_maps[i - 1], interpolate="none")
        viewer._ax.set_title('loss augmented costmap: \n {}.'.format(i),
                             size=32 / cols)
        viewer.set_drawing_axis(i * cols + 2)
        viewer.remove_axis()
        viewer.draw_ws_img(esf[i - 1], interpolate="none")
        viewer._ax.set_title('occupancy: \n {}.'.format(i), size=32 / cols)
        if paths is not None:
            show_paths(paths, pixel_map, starts, targets, viewer)
        if ex_paths is not None:
            show_paths(ex_paths[i - 1], pixel_map, starts, targets, viewer)
        if weights is not None and centers is not None:
            show_weights(viewer, weights[i - 1], centers)
        viewer.set_drawing_axis(i * cols + 3)
        viewer.remove_axis()
        viewer.draw_ws_img(loss_aug_esf[i - 1], interpolate="none")
        viewer._ax.set_title('loss augmented occupancy: \n {}.'.format(i),
                             size=32 / cols)
        if paths is not None:
            show_paths(paths, pixel_map, starts, targets, viewer)
        if ex_paths is not None:
            show_paths(ex_paths[i - 1], pixel_map, starts, targets, viewer)
    viewer._fig.tight_layout()

    if title is not None:
        viewer.set_title('\n'.join(wrap(title, 60)), fontsize=32)

    if show_result == 'SHOW':
        viewer.show_once()
    elif show_result == 'SAVE':
        viewer.save_figure(directory)
def show_iteration(costmaps,
                   original_costmaps,
                   workspace,
                   show_result,
                   weights=None,
                   starts=None,
                   targets=None,
                   paths=None,
                   centers=None,
                   ex_paths=None,
                   title=None,
                   directory=None):
    """ Show multiple maps with demonstrations and example paths
        One row for each iteration
        One column for each demonstrations with its corresponding example path
    """
    pixel_map = workspace.pixel_map(costmaps[0].shape[0])
    rows = len(costmaps) + 1
    cols = max(len(paths), len(original_costmaps))
    viewer = render.WorkspaceDrawer(workspace,
                                    wait_for_keyboard=True,
                                    rows=rows,
                                    cols=cols,
                                    scale=rows / (rows * cols))

    for i in range(cols):
        viewer.set_drawing_axis(i)
        if i < len(original_costmaps):
            viewer.draw_ws_img(original_costmaps[i], interpolate="none")
            viewer._ax.set_title('Training Costmap', size=32 / cols)
        viewer.remove_axis()

    for i in range(rows - 1):
        for j in range(cols):
            viewer.set_drawing_axis((i + 1) * cols + j)
            viewer.draw_ws_img(costmaps[i], interpolate="none")

            if weights is not None:
                show_weights(viewer, weights[i], centers)

            if paths is not None:
                print(len(paths))
                print(len(starts))
                show_paths([paths[j]], pixel_map, [starts[j]], [targets[j]],
                           viewer)
            if ex_paths is not None:
                show_paths([ex_paths[i][j]], pixel_map, [starts[j]],
                           [targets[j]], viewer)
            viewer._ax.set_title('Learned Costmap: \n {}. Iteration \n'
                                 '{}. path'.format((i + 1), j + 1),
                                 size=32 / cols)
            viewer.remove_axis()
    viewer._fig.tight_layout()

    if title is not None:
        viewer._fig.suptitle(title, fontsize=15)

    if show_result == 'SHOW':
        viewer.show_once()
    elif show_result == 'SAVE':
        viewer.save_figure(directory)