def create_map_from_world(self):
"""
Creates the occupancy grid (self.map) as a boolean numpy array. True is occupied, False is unoccupied.
This function is called during initialization of the object.
"""
bounds = self.world.world['bounds']['extents']
voxel_dimensions_metric = []
voxel_dimensions_indices = []
for i in range(3):
voxel_dimensions_metric.append(abs(bounds[1+i*2]-bounds[i*2]))
voxel_dimensions_indices.append(int(np.ceil(voxel_dimensions_metric[i]/self.resolution[i])))
# initialize the map with the correct dimensions as unoccupied
self.map = np.zeros(voxel_dimensions_indices, dtype=bool)
self.origin = np.array([bounds[0], bounds[2], bounds[4]])
# Create Rectangle objects from the obstacles to use for computing the configuration space
obstacle_rects = []
if 'blocks' in self.world.world:
for block in self.world.world['blocks']:
extent = block['extents']
obstacle_rects.append(Rectangle([extent[1], extent[3], extent[5]], [extent[0], extent[2], extent[4]]))
it = np.nditer(self.map, flags=['multi_index'])
# Iterate through every voxel in the map and check if it is too close to an obstacle. If so, mark occupied
while not it.finished:
metric_loc = self.index_to_metric_negative_corner(it.multi_index)
voxel_rectangle = Rectangle(metric_loc+self.resolution, metric_loc)
for obstacle in obstacle_rects:
rect_distance = voxel_rectangle.min_distance_rectangle(obstacle)
if rect_distance <= self.margin:
self.map[it.multi_index] = True
it.iternext()
def _init_map_from_world(self):
"""
Creates the occupancy grid (self.map) as a boolean numpy array. True is
occupied, False is unoccupied. This function is called during
initialization of the object.
"""
# Initialize the occupancy map, marking all free.
bounds = self.world.world['bounds']['extents']
voxel_dimensions_metric = []
voxel_dimensions_indices = []
for i in range(3):
voxel_dimensions_metric.append(
abs(bounds[1 + i * 2] - bounds[i * 2]))
voxel_dimensions_indices.append(
int(np.ceil(voxel_dimensions_metric[i] / self.resolution[i])))
self.map = np.zeros(voxel_dimensions_indices, dtype=bool)
self.origin = np.array(bounds[0::2])
# Iterate through each block obstacle.
for block in self.world.world.get('blocks', []):
extent = block['extents']
block_rect = Rectangle([extent[1], extent[3], extent[5]],
[extent[0], extent[2], extent[4]])
# Get index range that is definitely occupied by this block.
(inner_min_index,
inner_max_index) = self._metric_block_to_index_range(
extent, outer_bound=False)
a, b = inner_min_index, inner_max_index
self.map[a[0]:(b[0] + 1), a[1]:(b[1] + 1), a[2]:(b[2] + 1)] = True
# Get index range that is definitely not occupied by this block.
outer_extent = extent + self.margin * np.array(
[-1, 1, -1, 1, -1, 1])
(outer_min_index,
outer_max_index) = self._metric_block_to_index_range(
outer_extent, outer_bound=True)
# Iterate over uncertain voxels with rect-rect distance check.
for i in range(outer_min_index[0], outer_max_index[0] + 1):
for j in range(outer_min_index[1], outer_max_index[1] + 1):
for k in range(outer_min_index[2], outer_max_index[2] + 1):
# If map is not already occupied, check for collision.
if not self.map[i, j, k]:
metric_loc = self.index_to_metric_negative_corner(
(i, j, k))
voxel_rect = Rectangle(
metric_loc + self.resolution, metric_loc)
rect_distance = voxel_rect.min_distance_rectangle(
block_rect)
self.map[i, j, k] = rect_distance <= self.margin
