<filename>part2/python/occupancy_grid.py
import numpy as np
import matplotlib.pyplot as plt
import scipy.signal
import argparse
import yaml
import os
import re

# Constants for Indexing
X = 0
Y = 1
YAW = 2

# Constants for Occupancy grid

FREE = 0
UNKNOWN = 1
OCCUPIED = 2


ROBOT_RADIUS = 0.21 / 2.

# Defines an occupancy grid.
class OccupancyGrid(object):
  def __init__(self, values, origin, resolution):
    self._original_values = values.copy()
    self._values = values.copy()
    self._values_trajectory = values.copy()
    # Inflate obstacles (using a convolution).
    inflated_grid = np.zeros_like(values)
    inflated_grid[values == OCCUPIED] = 1.
    w = 2 * int(ROBOT_RADIUS / resolution) + 1
    inflated_grid = scipy.signal.convolve2d(inflated_grid, np.ones((w, w)), mode='same')
    self._values[inflated_grid > 0.] = OCCUPIED
    self._origin = np.array(origin[:2], dtype=np.float32)
    self._origin -= resolution / 2.
    assert origin[YAW] == 0.
    self._resolution = resolution

  @property
  def values(self):
    return self._values

  @property
  def values_trajectory(self):
    return self._values_trajectory

  @property
  def resolution(self):
    return self._resolution

  @property
  def origin(self):
    return self._origin

  def draw(self):
    plt.imshow(self._original_values.T, interpolation='none', origin='lower',
               extent=[self._origin[X],
                       self._origin[X] + self._values.shape[0] * self._resolution,
                       self._origin[Y],
                       self._origin[Y] + self._values.shape[1] * self._resolution])
    plt.set_cmap('gray_r')

  def draw_trajectory(self):
    plt.imshow(self._values_trajectory.T, interpolation='none', origin='lower',
               extent=[self._origin[X],
                       self._origin[X] + self._values.shape[0] * self._resolution,
                       self._origin[Y],
                       self._origin[Y] + self._values.shape[1] * self._resolution])
    plt.set_cmap('RdGy')

  def get_index(self, position):
    idx = ((position - self._origin) / self._resolution).astype(np.int32)
    if len(idx.shape) == 2:
      idx[:, 0] = np.clip(idx[:, 0], 0, self._values.shape[0] - 1)
      idx[:, 1] = np.clip(idx[:, 1], 0, self._values.shape[1] - 1)
      return (idx[:, 0], idx[:, 1])
    idx[0] = np.clip(idx[0], 0, self._values.shape[0] - 1)
    idx[1] = np.clip(idx[1], 0, self._values.shape[1] - 1)
    return tuple(idx)

  def get_position(self, i, j):
    return np.array([i, j], dtype=np.float32) * self._resolution + self._origin

  def is_occupied(self, position):
    return self._values[self.get_index(position)] == OCCUPIED

  def is_free(self, position):
    return self._values[self.get_index(position)] == FREE

def read_pgm(filename, byteorder='>'):
  """Read PGM file."""
  with open(filename, 'rb') as fp:
    buf = fp.read()
  try:
    header, width, height, maxval = re.search(
        b'(^P5\s(?:\s*#.*[\r\n])*'
        b'(\d+)\s(?:\s*#.*[\r\n])*'
        b'(\d+)\s(?:\s*#.*[\r\n])*'
        b'(\d+)\s(?:\s*#.*[\r\n]\s)*)', buf).groups()
  except AttributeError:
    raise ValueError('Invalid PGM file: "{}"'.format(filename))
  maxval = int(maxval)
  height = int(height)
  width = int(width)
  img = np.frombuffer(buf,
                      dtype='u1' if maxval < 256 else byteorder + 'u2',
                      count=width * height,
                      offset=len(header)).reshape((height, width))
  return img.astype(np.float32) / 255.

parser = argparse.ArgumentParser(description='Uses cpp to reach the goal.')
parser.add_argument('--map', action='store', default='map', help='Which map to use.')
args, unknown = parser.parse_known_args()

# Load map.
with open(args.map + '.yaml') as fp:
    data = yaml.safe_load(fp)
img = read_pgm(os.path.join(os.path.dirname(args.map), data['image']))
occupancy_grid = np.empty_like(img, dtype=np.int8)
occupancy_grid[:] = UNKNOWN
occupancy_grid[img < .1] = OCCUPIED
occupancy_grid[img > .9] = FREE
# Transpose (undo ROS processing).
occupancy_grid = occupancy_grid.T
# Invert Y-axis.
occupancy_grid = occupancy_grid[:, ::-1]
occupancy_grid = OccupancyGrid(occupancy_grid, data['origin'], data['resolution'])

<filename>part2/python/occupancy_grid.py
import numpy as np
import matplotlib.pyplot as plt
import scipy.signal
import argparse
import yaml
import os
import re

# Constants for Indexing
X = 0
Y = 1
YAW = 2

# Constants for Occupancy grid

FREE = 0
UNKNOWN = 1
OCCUPIED = 2


ROBOT_RADIUS = 0.21 / 2.

# Defines an occupancy grid.
class OccupancyGrid(object):
  def __init__(self, values, origin, resolution):
    self._original_values = values.copy()
    self._values = values.copy()
    self._values_trajectory = values.copy()
    # Inflate obstacles (using a convolution).
    inflated_grid = np.zeros_like(values)
    inflated_grid[values == OCCUPIED] = 1.
    w = 2 * int(ROBOT_RADIUS / resolution) + 1
    inflated_grid = scipy.signal.convolve2d(inflated_grid, np.ones((w, w)), mode='same')
    self._values[inflated_grid > 0.] = OCCUPIED
    self._origin = np.array(origin[:2], dtype=np.float32)
    self._origin -= resolution / 2.
    assert origin[YAW] == 0.
    self._resolution = resolution

  @property
  def values(self):
    return self._values

  @property
  def values_trajectory(self):
    return self._values_trajectory

  @property
  def resolution(self):
    return self._resolution

  @property
  def origin(self):
    return self._origin

  def draw(self):
    plt.imshow(self._original_values.T, interpolation='none', origin='lower',
               extent=[self._origin[X],
                       self._origin[X] + self._values.shape[0] * self._resolution,
                       self._origin[Y],
                       self._origin[Y] + self._values.shape[1] * self._resolution])
    plt.set_cmap('gray_r')

  def draw_trajectory(self):
    plt.imshow(self._values_trajectory.T, interpolation='none', origin='lower',
               extent=[self._origin[X],
                       self._origin[X] + self._values.shape[0] * self._resolution,
                       self._origin[Y],
                       self._origin[Y] + self._values.shape[1] * self._resolution])
    plt.set_cmap('RdGy')

  def get_index(self, position):
    idx = ((position - self._origin) / self._resolution).astype(np.int32)
    if len(idx.shape) == 2:
      idx[:, 0] = np.clip(idx[:, 0], 0, self._values.shape[0] - 1)
      idx[:, 1] = np.clip(idx[:, 1], 0, self._values.shape[1] - 1)
      return (idx[:, 0], idx[:, 1])
    idx[0] = np.clip(idx[0], 0, self._values.shape[0] - 1)
    idx[1] = np.clip(idx[1], 0, self._values.shape[1] - 1)
    return tuple(idx)

  def get_position(self, i, j):
    return np.array([i, j], dtype=np.float32) * self._resolution + self._origin

  def is_occupied(self, position):
    return self._values[self.get_index(position)] == OCCUPIED

  def is_free(self, position):
    return self._values[self.get_index(position)] == FREE

def read_pgm(filename, byteorder='>'):
  """Read PGM file."""
  with open(filename, 'rb') as fp:
    buf = fp.read()
  try:
    header, width, height, maxval = re.search(
        b'(^P5\s(?:\s*#.*[\r\n])*'
        b'(\d+)\s(?:\s*#.*[\r\n])*'
        b'(\d+)\s(?:\s*#.*[\r\n])*'
        b'(\d+)\s(?:\s*#.*[\r\n]\s)*)', buf).groups()
  except AttributeError:
    raise ValueError('Invalid PGM file: "{}"'.format(filename))
  maxval = int(maxval)
  height = int(height)
  width = int(width)
  img = np.frombuffer(buf,
                      dtype='u1' if maxval < 256 else byteorder + 'u2',
                      count=width * height,
                      offset=len(header)).reshape((height, width))
  return img.astype(np.float32) / 255.

parser = argparse.ArgumentParser(description='Uses cpp to reach the goal.')
parser.add_argument('--map', action='store', default='map', help='Which map to use.')
args, unknown = parser.parse_known_args()

# Load map.
with open(args.map + '.yaml') as fp:
    data = yaml.safe_load(fp)
img = read_pgm(os.path.join(os.path.dirname(args.map), data['image']))
occupancy_grid = np.empty_like(img, dtype=np.int8)
occupancy_grid[:] = UNKNOWN
occupancy_grid[img < .1] = OCCUPIED
occupancy_grid[img > .9] = FREE
# Transpose (undo ROS processing).
occupancy_grid = occupancy_grid.T
# Invert Y-axis.
occupancy_grid = occupancy_grid[:, ::-1]
occupancy_grid = OccupancyGrid(occupancy_grid, data['origin'], data['resolution'])

# Constants for Indexing # Constants for Occupancy grid # Defines an occupancy grid. # Inflate obstacles (using a convolution). Read PGM file. #.*[\r\n])*' #.*[\r\n])*' #.*[\r\n])*' #.*[\r\n]\s)*)', buf).groups() # Load map. # Transpose (undo ROS processing). # Invert Y-axis.