def updateIndividualByBSO(robot_amount, robotIndex, allFrontiers,
map_grid_matrix, robotlocation, centerfrontiers,
total_weights):
prob_one_cluster = 0.5 # 0.8
frontiers_1 = allFrontiers[robotIndex]
# print("robotIndex", robotIndex)
# replace cluster center by at randomly generated center TODO
# if random.random() < 0.2:
# cenIdx = math.ceil(random.random() * (len(allFrontiers) -1) )
# centerfrontiers[cenIdx] =
for i in range(len(frontiers_1)):
r_1 = random.random()
indi_temp = Frontier(0, 0, 0)
if r_1 < prob_one_cluster: # update from self cluster
if random.random() < 0.4:
indi_temp = centerfrontiers[robotIndex]
else:
# math.ceil(random.random() * (len(frontiers)-1))
indi_1 = resampling(frontiers_1, total_weights[robotIndex])
indi_temp = frontiers_1[indi_1]
indi_temp.weight = calculateDistance(map_grid_matrix, indi_temp.x,
indi_temp.y, robotlocation)
if (frontiers_1[i].weight < indi_temp.weight):
frontiers_1[i] = indi_temp
calculate_frontiers(frontiers_1, robotIndex, centerfrontiers,
total_weights)
else: # update throught other cluster
cluster_2 = robotIndex
while cluster_2 != robotIndex:
cluster_2 = math.ceil(random.random() * (robot_amount - 1))
# print("cluster_2:", cluster_2)
frontiers_2 = allFrontiers[cluster_2]
# math.ceil(random.random() * (len(frontiers_2)-1))
indi_2 = resampling(frontiers_2, total_weights[cluster_2])
# math.ceil(random.random() * (len(frontiers)-1))
indi_1 = resampling(frontiers_1, total_weights[robotIndex])
if random.random() < 0.5:
# indi_temp = pick_from_two_cluster(
# centerfrontiers[robotIndex], centerfrontiers[cluster_2], robotlocation)
indi_temp = pick_from_two_cluster(centerfrontiers[robotIndex],
centerfrontiers[cluster_2],
robotlocation,
map_grid_matrix)
else:
# indi_temp = pick_from_two_cluster(
# frontiers_1[indi_1], frontiers_2[indi_2], robotlocation)
indi_temp = pick_from_two_cluster(frontiers_1[indi_1],
frontiers_2[indi_2],
robotlocation,
map_grid_matrix)
# print("robotlocation", robotlocation.x, robotlocation.y)
indi_temp.weight = calculateDistance(map_grid_matrix, indi_temp.x,
indi_temp.y, robotlocation)
# if(frontiers_1[i].weight < indi_temp.weight):
frontiers_1[i] = indi_temp
calculate_frontiers(frontiers_1, robotIndex, centerfrontiers,
total_weights)
return frontiers_1
def findFrontiers(map_grid_matrix, robotlocation):
frontiers = []
for x in range(len(map_grid_matrix)):
for y in range(len(map_grid_matrix[0])):
if map_grid_matrix[x][y] == EXPLORATED_BOUND:
m_frontier = Frontier(x, y, calculateDistance(map_grid_matrix, x, y, robotlocation))
frontiers.append(m_frontier)
# print("frontiers:", len(frontiers))
frontiers = frontierFilter(frontiers)
# print("newfrontiers:", len(frontiers))
return frontiers
def calculate_frontiers(frontiers, index, centerfrontiers, total_weights):
if len(centerfrontiers) <= index:
centerfrontiers.append(Frontier(0, 0, 0))
if len(total_weights) <= index:
total_weights.append(0)
total_weight = 0
centerfrontier = frontiers[0]
for i in range(len(frontiers)):
total_weight += frontiers[i].weight
if centerfrontiers[index].weight < frontiers[i].weight:
centerfrontier = frontiers[i]
centerfrontiers[index] = centerfrontier
total_weights[index] = total_weight
def pick_from_two_cluster(frontier_W_1, frontier_W_2, robotlocation_W,
map_grid_matrix):
reject_distance = 5
# calculate the distance betweem two frontiers
ff_distance = math.hypot((frontier_W_1.x - frontier_W_2.x),
(frontier_W_1.y - frontier_W_2.y))
# print("ff_distance:", ff_distance)
# determine the search distance
if ff_distance < reject_distance:
# y(x) = 2Lr/(x+Lr)
ff_distance = (2 * reject_distance**2) / (ff_distance +
reject_distance)
# search better frontier candidates, based on the distance
candidate_frontiers = []
for i in range(int(frontier_W_1.x - ff_distance),
int(frontier_W_1.x + ff_distance)):
# out of range
if i < 0 or i > 19:
continue
for j in range(int(frontier_W_1.y - ff_distance),
int(frontier_W_1.y + ff_distance)):
# out of range
if i < 0 or j > 39:
continue
if (map_grid_matrix[i][j] == EXPLORATED_BOUND):
# g(a), this function can be changed by user
weight = math.hypot((i - frontier_W_2.x),
(j - frontier_W_2.y))
frontier = Frontier(i, j, weight)
candidate_frontiers.append(frontier)
# select the best candidate
# candidate_frontiers.sort(key=lambda x:x[0], reverse=True)
indi_temp_W = candidate_frontiers.pop()
for i in range(len(candidate_frontiers)):
# print("candidate_frontiers[i].weight:", candidate_frontiers[i].weight)
if indi_temp_W.weight < candidate_frontiers[i].weight:
indi_temp_W = candidate_frontiers[i]
# print("max.weight:", indi_temp_W.weight)
return indi_temp_W
else:
ff_distance = ff_distance
return frontier_W_1