world_generator.py 文件源码

def create_world_from_grid(self, grid, im_size, start, goal):
        # Create gazebo world out of the grid
        scale = 0.75

        # Add start box
        self.add_target_box_green(start[0], start[1])

        # Add goal box
        self.add_target_box_red(goal[0], goal[1])

        # Build walls around the field
        wall_width = 0.5
        self.add_wall(scale*(im_size[0]-1)/2.0, 0, 0, scale*(im_size[0]-1), wall_width)
        self.add_wall(0, scale*(im_size[1]-1)/2.0, pi / 2.0, scale*(im_size[0]-1), wall_width)
        self.add_wall(scale*(im_size[0]-1), scale*(im_size[1]-1)/2.0, - pi / 2.0, scale*(im_size[0]-1), wall_width)
        self.add_wall(scale*(im_size[0]-1)/2.0, scale*(im_size[1]-1), pi, scale*(im_size[0]-1), wall_width)

        # Add asphalt
        self.add_tarmac(scale*(im_size[0]-1)/2.0, scale*(im_size[1]-1)/2.0, 0, scale*(im_size[0]-1), scale*(im_size[1]-1))

        # Add cones wherever there should be obstacles
        i = 1
        j = 1
        obstacle_indices = np.where(grid != 1)
        unraveled_indices = np.unravel_index(obstacle_indices, im_size, order='C')
        for x in grid:
            if (grid[j+i*im_size[0]] != 1):
                self.add_cone(scale*j, scale*i)
                self.write()
            j += 1
            if (j % (im_size[1]-1)) == 0:
                j = 1
                i +=1
                if (i == im_size[0]-1):
                    break