Question

我是ROS编程的新手，我正在尝试编写一种导航算法作为其资格的一部分（该算法的主要步骤是）： 1.通过调用static_map服务加载占用栅格地图。 2.切换到一个网格，其中每个单元等于机器人D的大小。为避免机器人与障碍物碰撞，如果给定网格单元中的一个子单元被占用，则将整个单元标记为已占用。 3.查找从机器人初始位置可以到达的所有空闲单元，并忽略所有其他单元。 4.从开始位置到结束位置打印网格单元格（在D尺寸网格中）的路径。您可以使用任何所需的搜索算法找到该路径。

输入该算法以.pgm文件（作为启动文件中的参数），机械手直径大小（作为启动文件中的参数）以及机器人的开始和结束位置的形式给出环境图。启动文件中的参数）。为了进行测试，您可以使用turtlebot_gazebo / maps中的map park.pgm地图。算法您不必自己编写，也可以使用外部代码。

输出文件您的应用程序应生成两个输出文件： 1. new_grid.txt-新的粗网格 2. path.txt –从起始位置到目标的路径。

假设您可以假设以下有关环境和机器人运动的信息： •近似的细胞分解–环境可以分解分为一组均匀的网格单元，每个单元的尺寸为D（机器人的直径）。 •机器人只能在四个罗盘方向上移动-东，西，南和北。 •机器人具有完美的定位。 •机械手具有圆形形状，其直径等于单元大小D。 •起始位置不会在障碍物上，目标可以在障碍物上。在这种情况下，最终目标应该是通往目标的最后一步，而不是障碍。

我目前正在执行第3步，当时一位朋友告诉我，为了获得机器人的初始位置或我必须使用map.info.origin或其附近的任何位置，任何人都可以详细说明如何将地图中的机器人位置正确地转换为其在网格中的位置，反之亦然？

我也很想获得关于在不使用原始信息的情况下创建占用网格的能力的确认，因为我不知道它的存在，我希望我做对了。这是我的代码：

def create_occupancy_grid(my_map):
    global grid
    grid = [[None] * my_map.info.width for i in xrange(my_map.info.height)]
    for i in xrange(my_map.info.height):
        for j in xrange(my_map.info.width):
            if my_map.data[i * my_map.info.width + j] == 0:
                grid[i][j] = False
            else:
                grid[i][j] = True

'''Returns the resize factor of the grid according to the argument robot
 diameter (D) and the map's resolution.'''
def get_resize_factor(robot_diameter, my_map):
    return int( ceil( robot_diameter / my_map.info.resolution))

'''Returns the diameter (D) cell sized grid dimension sizes, the height and the
 width, according to the argument resize factor. '''
def get_diameter_grid_dimension_sizes(resize_factor, my_map):
    diameter_grid_height = int( floor( my_map.info.height / resize_factor))
    diameter_grid_width = int( floor( my_map.info.width / resize_factor))
    return diameter_grid_height, diameter_grid_width

'''Create a new, obstacle free, diameter (D) cell sized grid'''
def create_diameter_cell_sized_grid(diameter_grid_height, diameter_grid_width):
    global diameter_grid
    diameter_grid = [[False] * diameter_grid_width for i in xrange(diameter_grid_height)]

'''Fill the obstacle free, diameter (D) cell sized grid with obstacles
according to the original grid and the resize factor.'''
def fill_obstacle_free_diameter_grid_with_obstacles(resize_factor, my_map):
    global grid
    global diameter_grid
    for row in xrange(my_map.info.height):
        for column in xrange(my_map.info.width):
            if grid[row][column]:
                diameter_grid_row = int(floor(row / resize_factor))
                diameter_grid_column = int(floor(column / resize_factor))
                if not diameter_grid[diameter_grid_row][diameter_grid_column]:
                    diameter_grid[diameter_grid_row][diameter_grid_column] = True

'''Create a resized grid, that is resized according to the resize factor and
diameter. '''
def create_resized_grid(robot_diameter, my_map):
    resize_factor = get_resize_factor(robot_diameter, my_map)
    diameter_grid_height, diameter_grid_width = \
        get_diameter_grid_dimension_sizes(resize_factor, my_map)
    create_diameter_cell_sized_grid(diameter_grid_height, diameter_grid_width)
    fill_obstacle_free_diameter_grid_with_obstacles(resize_factor, my_map)

谢谢！

如何将地图文件中的位置转换为ROS中的占用网格中的位置？

0 个答案: