def create_dataset(list):
filter = gflags.FLAGS.filter_dat
if not filter:
to_remove = []
for s1 in list:
for s2 in list:
if s1 != s2 and s1[-1] != 'C' and \
utils.eucl_dist((float(s1[1]), float(s1[2])), (float(s2[1]), float(s2[2]))) <= 1.75 and \
utils.eucl_dist((float(s1[3]), float(s1[4])), (float(s2[3]), float(s2[4]))) <= 1.75:
if s1 not in to_remove:
to_remove.append(s1)
print len(list)
print len(to_remove)
data = [x for x in list if x not in to_remove]
else:
data = list
dataset = []
for s in data:
new_data = SignalData()
new_data.timestep = float(s[0])
new_data.my_pos.pose.position.x = float(s[1])
new_data.my_pos.pose.position.y = float(s[2])
new_data.teammate_pos.pose.position.x = float(s[3])
new_data.teammate_pos.pose.position.y = float(s[4])
new_data.signal_strength = float(s[5])
dataset.append(new_data)
return dataset
def create_dataset(data_list, set):
if set == "pre_processing":
to_remove = []
for d1 in data_list:
for d2 in data_list:
if d1 != d2 and d1[-1] != 'C' and \
utils.eucl_dist((float(d1[1]), float(d1[2])), (float(d2[1]), float(d2[2]))) <= 2.0 and \
utils.eucl_dist((float(d1[3]), float(d1[4])), (float(d2[3]), float(d2[4]))) <= 2.0:
to_remove.append(d1)
break
data = [x for x in data_list if x not in to_remove]
else:
data = data_list
dataset = []
for s in data:
new_data = SignalData()
new_data.timestep = float(s[0])
new_data.my_pos.pose.position.x = float(s[1])
new_data.my_pos.pose.position.y = float(s[2])
new_data.teammate_pos.pose.position.x = float(s[3])
new_data.teammate_pos.pose.position.y = float(s[4])
new_data.signal_strength = float(s[5])
dataset.append(new_data)
return dataset
def pose_callback(self, msg):
self.x = self.robots_pos[self.robot_id][0]
self.y = self.robots_pos[self.robot_id][1]
if self.last_x is not None:
self.traveled_dist += utils.eucl_dist((self.x, self.y),(self.last_x, self.last_y))
self.last_x = self.x
self.last_y = self.y
def assignLabels(pixel, K, centroids):
min = 1000000
index = -1
for i in range(0, K, 1):
temp = utils.eucl_dist(pixel, centroids[i])
if temp < min:
min = temp
index = i
return index
def create_test(im_array, data_list, resize_factor=0.1):
XTest = []
YTest = []
dimX = np.size(im_array, 1) * resize_factor
dimY = np.size(im_array, 0) * resize_factor
test_set = []
for d1 in data_list:
for d2 in data_list:
if d1 != d2 and d1[-1] != 'C' and \
utils.eucl_dist((float(d1[1]), float(d1[2])), (float(d2[1]), float(d2[2]))) <= 2.0 and \
utils.eucl_dist((float(d1[3]), float(d1[4])), (float(d2[3]), float(d2[4]))) <= 2.0:
test_set.append(d1)
break
print "Test Set length: " + str(len(test_set))
for s in test_set:
XTest.append((float(s[1]), float(s[2]), float(s[3]), float(s[4])))
YTest.append(float(s[5]))
return dimX, dimY, XTest, YTest
def grid_points_selection():
global start
global goal_config
points = []
for i in xrange(0, len(G_E.vs)):
points.append(
G_E.vs[i]
) #creating a list of vertices of the graph (for legibility)
#removing points that are too close to each other or too close to an obstacle
too_close = []
for p1 in points:
if p1 in too_close: continue
x1 = p1['x_coord']
y1 = p1['y_coord']
if not all_free(int(y1), int(x1), I, J, wall_dist):
too_close.append(p1)
else:
for p2 in points:
if p1 == p2 or p2 in too_close: continue
x2 = p2['x_coord']
y2 = p2['y_coord']
if eucl_dist((x1, y1), (x2, y2)) < coeff * sel_grid_size:
too_close.append(p2)
points = [x for x in points if x not in too_close]
for point in points:
x1 = point['x_coord']
y1 = point['y_coord']
vertex_id = get_closest_vertex(x1, y1)
vertex_id = check_vertex_dist_from_obstacle(vertex_id)
if vertex_id and vertex_id not in goal_config:
goal_config.append(vertex_id)
write_exp_file()
plot_plan(goal_config)
print 'Done. Number of points: ' + str(len(goal_config))
def check_vertex_dist_from_obstacle(vertex_id):
#fixing the vertices that are too close to an obstacle
if env_name == 'open' and (G_E.vs[vertex_id]['x_coord'] < 60
or G_E.vs[vertex_id]['x_coord'] > 700):
return None #bad discretization of the environment
if not all_free(int(G_E.vs[vertex_id]['y_coord']),
int(G_E.vs[vertex_id]['x_coord']), I, J, wall_dist):
for vertex in G_E.vs:
if vertex == vertex_id: continue
x1 = G_E.vs[vertex_id]['x_coord']
y1 = G_E.vs[vertex_id]['y_coord']
x2 = vertex['x_coord']
y2 = vertex['y_coord']
if eucl_dist((x1, y1), (x2, y2)) <= sel_grid_size and all_free(
int(y2), int(x2), I, J, wall_dist):
vertex_id = get_closest_vertex(x2, y2)
return vertex_id
return None #if no fixing vertex is found, no vertex will be added in the goal set
return vertex_id
def get_average_variance_comm_region(self, center, env):
"""Calculate average of variance centered in center over comm_range.
center (tuple of float): x, y position, in meters.
env (Environment): environment used.
"""
"""minX = max(0, center[0] - self.comm_range)
maxX = min(self.dimX, center[0] + self.comm_range)
minY = max(0, center[1] - self.comm_range)
maxY = min(self.dimY, center[1] + self.comm_range)
X = np.arange(minX, maxX, 2.0)
Y = np.arange(minY, maxY, 2.0)"""
data = []
for x, y in env.free_positions:
if eucl_dist(center, (x, y)) <= self.comm_range:
data.append((center[0], center[1], x, y))
data.append((x, y, center[0], center[1]))
"""
for x in X:
for y in Y:
# Only for free cell and within communication range
# calculate variance.
ii, jj, = grid_map_cells[conv_to_hash(x, y)]
if(env.all_free(ii, jj, env.I, env.J) and
eucl_dist(center, (x,y)) <= self.comm_range):
data.append((center[0], center[1], x, y))
data.append((x, y, center[0], center[1]))
"""
variances = map(lambda x: x[1], self.predict(data))
return np.mean(variances)
def create_test_set(im_array,
comm_model,
test_set_size,
normalized,
resize_factor=0.1):
def all_free(ii, jj, I, J, border=None):
if (im_array[ii][jj] == 0): return False
if border is None: return True
for k in xrange(ii - border, ii + border + 1):
if k < 0 or k >= I: return False
for w in xrange(jj - border, jj + border + 1):
if w < 0 or w >= J: return False
if (im_array[k][w] == 0): return False
return True
XTest = []
YTest = []
dimX = np.size(im_array, 1) * resize_factor
dimY = np.size(im_array, 0) * resize_factor
I = np.size(im_array, 0)
J = np.size(im_array, 1)
items = 0
while items < test_set_size:
while True:
x1 = dimX * random.random()
y1 = dimY * random.random()
i1 = I - int(y1 / resize_factor)
j1 = int(x1 / resize_factor)
if i1 >= I or j1 >= J: continue
if all_free(i1, j1, I, J, environment.WALL_DIST):
break
while True:
x2 = dimX * random.random()
y2 = dimY * random.random()
if (utils.eucl_dist((x1, y1), (x2, y2)) < comm_model.MIN_DIST
or utils.eucl_dist((x1, y1), (x2, y2)) >
comm_model.COMM_RANGE): #gflags.FLAGS.comm_range_exp):
continue
i2 = I - int(y2 / resize_factor)
j2 = int(x2 / resize_factor)
if i2 >= I or j2 >= J: continue
if all_free(i2, j2, I, J, environment.WALL_DIST):
break
num_obstacles = numObstaclesBetweenRobots(im_array, I, (x1, y1),
(x2, y2), resize_factor)
signal_strength = (
comm_model.REF_SIGNAL - 10 * comm_model.PATH_LOSS * math.log10(
utils.eucl_dist((x1, y1), (x2, y2)) / comm_model.REF_DIST) -
min(comm_model.MAX_WALL, num_obstacles) * comm_model.WALL_ATT)
noise = np.random.normal(0, comm_model.VAR_NOISE)
signal_strength += noise
#otherwise the robot has not measured it
if signal_strength < comm_model.CUTOFF_SAFE: continue
XTest.append((x1, y1, x2, y2))
YTest.append(signal_strength)
items += 1
if normalized:
print "Normalizing the test set..."
YTest = [YTest]
YTest_norm = preprocessing.normalize(YTest)
YTest = YTest_norm[0]
return dimX, dimY, XTest, YTest
def voronoi_points_selection():
global start
global goal_config
graph = environment_discretization()
graph_points = []
for vertex in graph:
x = vertex['x_coord']
y = vertex['y_coord']
graph_points.append((x, y))
vertices = []
for vertex in G_E.vs:
x = vertex['x_coord']
y = vertex['y_coord']
vertices.append((x, y))
#Calculating Voronoi skeleton by keeping max_min distance points from the obstacles
too_close = []
for p1 in vertices:
if p1 in too_close: continue
min_dist = eucl_dist((p1[0], p1[1]),
(graph_points[0][0], graph_points[0][1]))
min_coords = graph_points[0]
for p2 in graph_points:
dist = eucl_dist((p1[0], p1[1]), (p2[0], p2[1]))
if dist < min_dist:
min_dist = dist
min_coords = p2
for p3 in vertices:
if p1 == p3 or p3 in too_close: continue
if eucl_dist((p3[0], p3[1]),
(min_coords[0], min_coords[1])) < min_dist:
too_close.append(p3)
points = [x for x in vertices if x not in too_close]
# removing points that are too close to an obstacle
to_remove = []
for point in points:
if not all_free(int(point[1]), int(point[0]), I, J, wall_dist):
to_remove.append(point)
points = [x for x in points if x not in to_remove]
# removing points that too close to each other
cluster = []
for p1 in points:
if p1 in cluster: continue
for p2 in points:
if p1 == p2: continue
if eucl_dist((p1[0], p1[1]),
(p2[0], p2[1])) < coeff * sel_grid_size:
if p2 not in too_close:
cluster.append(p2)
points = [x for x in points if x not in cluster]
for point in points:
vertex_id = get_closest_vertex(point[0], point[1])
vertex_id = check_vertex_dist_from_obstacle(vertex_id)
if vertex_id and vertex_id not in goal_config:
goal_config.append(vertex_id)
write_exp_file()
plot_plan(goal_config)
print 'Done. Number of points: ' + str(len(goal_config))
