def main():
# get command line arguments
slam_data_filepath = sys.argv[1]
smarthome_data_filepath = sys.argv[2]
sensor_map_filepath = sys.argv[3]
config_filepath = sys.argv[4]
# load the config
config = Config(config_filepath)
### get all the things to plot
# get slam map
slam_map = SlamMap(slam_data_filepath, config)
# get heatmap
heatmap = StaticHeatmap(sensor_map_filepath, smarthome_data_filepath, config)
heatmap.set_offset(slam_map.origin[0], slam_map.origin[1])
# get path map
path_map = PathMap(heatmap)
# get the weighted average
weighted_average = WeightedAverage(heatmap)
# get the reachability map
reachability_map = ReachabilityMap(slam_map)
### plot all the things to plot
# plot the path map
path_array = path_map.get_as_array()
#plt.imshow(np.transpose(path_array), cmap='hot', interpolation='nearest')
# plot the heatmap
heatmap = heatmap.get_heatmap_array()
plt.imshow(np.transpose(heatmap), cmap=get_custom_colormap_blue(), interpolation='nearest')
# plot the slam map
plt.imshow(np.transpose(slam_map.map), cmap=get_custom_colormap_green(), interpolation='nearest')
# plot the weighted average
average_point = weighted_average.get_weighted_average_point()
#plt.plot(average_point[0], average_point[1], 'go')
# plot the reachability map
#plt.imshow(np.transpose(reachability_map.map), cmap=get_custom_colormap_blue(), interpolation='nearest')
# flip y axis
axis = plt.gca()
axis.set_ylim(axis.get_ylim()[::-1])
plt.show()
class BasePlacer:
""" Finds where the robot should be placed based on our metrics
Loads up the heatmap, slam map, path map, and weighted average.
Then uses each of these metrics to find the best point by finding
a weighted value for every point on the map.
"""
def __init__(self, slam_data_filepath, sensor_list_filepath,
smarthome_data_filepath, config):
# variables
self.map = None
self.slam_weight = 1.0
self.wa_weight = 0.1
self.path_weight = 1500.0
# load slam map
self.slam_map = slam_map = SlamMap(slam_data_filepath, config)
# load reachability map
self.reachability_map = reachability_map = ReachabilityMap(slam_map)
# load heatmap
self.static_heatmap = static_heatmap = StaticHeatmap(
sensor_list_filepath, smarthome_data_filepath, config)
static_heatmap.set_offset(slam_map.origin[0], slam_map.origin[1])
# load path map
self.path_map_array = PathMap(self.static_heatmap).get_as_array()
# load weighted average
self.weighted_average = WeightedAverage(static_heatmap)
self.average_point = self.weighted_average.get_weighted_average_point()
self._build_map()
def _build_map(self):
# find the value of each point
placement_map = np.zeros_like(self.static_heatmap.get_heatmap_array())
for i in range(placement_map.shape[0]):
for j in range(placement_map.shape[1]):
# find the value of a given location
placement_map[i, j] = self.find_value(i, j)
# rescale to between zero and 1
# note that all values in the map are less than zero
amin = np.amin(placement_map)
placement_map = np.subtract(placement_map, amin)
amax = np.amax(placement_map)
placement_map = np.true_divide(placement_map, amax)
# apply reachability mask
placement_map = self.apply_reachability_mask(placement_map)
self.map = placement_map
def apply_reachability_mask(self, placement_map):
reachability_map = self.reachability_map.map
masked_map = np.zeros_like(reachability_map, dtype="float")
for i in range(reachability_map.shape[0]):
for j in range(reachability_map.shape[1]):
# skip values outside the placement map
if i >= placement_map.shape[0] or j >= placement_map.shape[1]:
continue
if reachability_map[i, j] > 0:
masked_map[i, j] = placement_map[i, j]
return masked_map
def find_value(self, i, j):
"""
find the placement value at the given array location
"""
# get weighted average value
p1 = (i, j)
p2 = self.average_point
wa_value = -(get_point_distance(p1, p2)**2) * self.wa_weight
# get path map value
path_value = -self.path_map_array[i, j] * self.path_weight
return wa_value + path_value
def get_best_point(self):
""" get the location of the best point
returns:
tuple (x, y) - indecies of the best scoring map location
"""
max_value = 0.0 # minimum value of the map is 0.0
best_point = (-1, -1)
for i in range(self.map.shape[0]):
for j in range(self.map.shape[1]):
if self.map[i, j] > max_value:
max_value = self.map[i, j]
best_point = (i, j)
return best_point
def display_as_heatmap(self):
best_point = self.get_best_point()
# display
plt.imshow(np.transpose(self.map), cmap='hot', interpolation='nearest')
plt.plot(best_point[0], best_point[1], 'go')
axis = plt.gca()
axis.set_ylim(axis.get_ylim()[::-1])
plt.show()
def display_top(self, top_percent=0.1):
top_map = np.copy(self.map)
minimum = np.amin(top_map)
maximum = np.amax(top_map)
value_range = maximum - minimum
cutoff = maximum - value_range * top_percent
# clip any values below the desired percentage
full = np.full_like(top_map, cutoff)
top_map = np.maximum(top_map, full)
top_map = np.subtract(top_map, full)
# scale to between 0 and 1
amax = np.amax(top_map)
top_map = np.true_divide(top_map, amax)
plt.imshow(np.transpose(top_map), cmap='hot', interpolation='nearest')
plt.imshow(np.transpose(self.slam_map.map),
cmap=get_custom_colormap_green(),
interpolation='nearest')
axis = plt.gca()
axis.set_ylim(axis.get_ylim()[::-1])
plt.show()