def get_map_coords(wx, wy, costmap):
    """Given coordinates in the world, figure out the indices in the costmap
    that correspond to them. Make sure that the frame_id of the coordinates are
    the same as that of the costmap!"""
    ox, oy, res = (costmap.info.origin.position.x,
                   costmap.info.origin.position.y,
                   costmap.info.resolution)
    size_x, size_y = costmap.info.width, costmap.info.height

    # Sanity check the inputs
    assert not (wx < ox or wy < oy), ("World coordinates {} not in map with origin {}"
                                      .format((wx, wy,), (ox, oy,)))

    # Calculate the map coordinate
    mx = (wx - ox) / res
    my = (wy - oy) / res

    # Sanity check the outputs
    assert (mx < size_x and my < size_y), ("Calculated coords {} larger than map size {}"
                                           .format((mx, my,), (size_x, size_y,)))

    # Return
    return (int(mx), int(my),)