def reinsert(self, old_paths, new_paths, population):
# 父子融合
tmp_paths = old_paths
tmp_paths.extend(new_paths)
# 优胜劣汰
quick_sort(tmp_paths)
return tmp_paths[:population]
def initialsolution(self, occ_grid, pri_grid, privacy_sum, obstacle_num,
starting_point, end_point, Tbudget, Kca, grid_x,
grid_y, grid_z):
# 初始化变量
objectives = [end_point]
Kca = Kca
T_budget = Tbudget
occ_grid = occ_grid
pri_grid = pri_grid
privacy_sum = privacy_sum
obstacle_num = obstacle_num
starting_point = starting_point
end_point = end_point
alg = gA.GeneticAlgorithm(self.population)
print(
'\033[94m Generating random initial solutions for global planning... \033[0m'
)
paths = alg.init_population(starting_point, objectives, Kca, grid_x,
grid_y, grid_z)
print('\033[94m Solutions generated.\033[0m')
for i in range(len(paths)):
# print("The value of i in ga_class: ", i)
paths[i] = alg.smooth(paths[i])
print('\033[94m Smooth method finished.\033[0m')
for p in range(len(paths)):
paths[p].fitness = alg.get_fitness(paths[p], occ_grid, pri_grid,
starting_point, end_point,
privacy_sum, obstacle_num)
max_p = max(paths, key=lambda x: x.fitness)
# print(max_p)
max_f = -5
max_path = copy.deepcopy(paths[0])
delete_idx = []
max_flag = 1
for i in range(self.generation):
print("The generation num: ", i)
quick_sort(paths)
for m in range(len(paths)):
# print (m)
if max_f <= paths[m].fitness and paths[m].flag == 0:
max_f = paths[m].fitness
max_path = copy.deepcopy(paths[m])
max_flag = paths[m].flag
print('\033[94m Current maximum fitness:\033[0m\033[92m ' +
str(max_f) +
'\033[0m\033[94m, Generation:\033[0m\033[92m ' +
str(i) + '\033[0m\033[94m, Flag:\033[0m\033[92m ' +
str(max_flag) +
'\033[0m\033[94m, Solution:\033[0m\033[92m ' +
str(m) + ' \033[0m')
for j in range(len(paths[m].points)):
print(paths[m].points[j])
print("the solution", m, len(paths[m].points), max_flag)
break
#alg.get_fitness(paths[0], occ_grid, pri_grid, starting_point, end_point, privacy_sum, obstacle_num)
"""
if max_f <= paths[m].fitness and paths[m].flag == 0:
max_f = paths[m].fitness
max_path = copy.deepcopy(paths[m])
max_flag = paths[m].flag
print('\033[94m Current maximum fitness:\033[0m\033[92m ' + str(
max_f) + '\033[0m\033[94m, Generation:\033[0m\033[92m ' + str(
i) + '\033[0m\033[94m, Flag:\033[0m\033[92m ' + str(max_flag) + ' \033[0m')
for j in range(len(paths[m].points)):
print(paths[m].points[j])
print("the solution", m, len(paths[m].points), max_flag)
"""
#if max_f == 1 and max_flag == 0:
# break
#for p in range(len(paths)):
# paths[p].fitness = alg.get_fitness(paths[p], occ_grid, pri_grid,
# starting_point, end_point, privacy_sum, obstacle_num)
# 选择
selected_path_list = alg.select(paths, self.selection_size,
occ_grid, pri_grid, starting_point,
end_point, privacy_sum,
obstacle_num)
# 形成couple, 交叉， 变异
new_path_list = []
for j in range(self.selection_size):
for k in range(j + 1, self.selection_size):
new_path1 = alg.cross_over(selected_path_list[j].points,
selected_path_list[k].points,
objectives, Kca)
new_path2 = alg.cross_over(selected_path_list[k].points,
selected_path_list[j].points,
objectives, Kca)
new_path1_mutated = alg.mutate(new_path1, objectives, Kca,
grid_x, grid_y, grid_z)
new_path1_mutated.fitness = alg.get_fitness(
new_path1_mutated, occ_grid, pri_grid, starting_point,
end_point, privacy_sum, obstacle_num)
new_path_list.append(new_path1_mutated)
new_path2_mutated = alg.mutate(new_path2, objectives, Kca,
grid_x, grid_y, grid_z)
new_path2_mutated.fitness = alg.get_fitness(
new_path2_mutated, occ_grid, pri_grid, starting_point,
end_point, privacy_sum, obstacle_num)
new_path_list.append(new_path2_mutated)
# 重插入
paths = alg.reinsert(paths, new_path_list, population)
if len(max_path.points) < T_budget:
return max_f, max_path, max_flag
else:
print("No Solution!")
return max_f, max_path, max_flag
for p in range(len(paths)):
paths[p].fitness = alg.get_fitness(paths[p], occ_grid, pri_grid,
starting_point, end_point,
privacy_sum, obstacle_num)
max_p = max(paths, key=lambda x: x.fitness)
max_f = -5
count = 0
fitnv = []
print(len(paths))
for i in range(max_generation):
print(i)
quick_sort(paths)
for m in range(len(paths)):
# print (m)
if max_f < paths[m].fitness:
max_f = paths[m].fitness
max_path = copy.deepcopy(paths[m])
max_flag = paths[m].flag
print('\033[94m Current maximum fitness:\033[0m\033[92m ' +
str(max_f) +
'\033[0m\033[94m, Generation:\033[0m\033[92m ' + str(i) +
' \033[0m')
for j in range(len(paths[m].points)):
print(paths[m].points[j])
print("the solution", m, len(paths[m].points))
break
'''
def motionplan(self, occ_grid_known, pri_grid_known, privacy_sum_known,
obstacle_num, current_p, next_p, T_plan, Kca, grid_x,
grid_y, grid_z):
objectives = [next_p]
Kca = Kca
T_budget = T_plan
occ_grid = occ_grid_known
pri_grid = pri_grid_known
privacy_sum = privacy_sum_known
obstacle_num = obstacle_num
starting_point = current_p
end_point = next_p
alg = gA.GeneticAlgorithm(self.population)
print(
'\033[94m Generating random initial solutions for motion planning... \033[0m'
)
paths = alg.init_population(starting_point, objectives, Kca, grid_x,
grid_y, grid_z)
for i in range(len(paths)):
paths[i] = alg.smooth(paths[i])
for p in range(len(paths)):
paths[p].fitness = alg.get_fitness(paths[p], occ_grid, pri_grid,
starting_point, end_point,
privacy_sum, obstacle_num)
max_p = max(paths, key=lambda x: x.fitness)
max_f = -5
max_path = copy.deepcopy(paths[0])
delete_idx = []
max_flag = 1
for i in range(self.generation):
print(i)
quick_sort(paths)
for m in range(len(paths)):
# print (m)
if max_f <= paths[m].fitness and paths[m].flag == 0:
max_f = paths[m].fitness
max_path = copy.deepcopy(paths[m])
max_flag = paths[m].flag
print('\033[94m Current maximum fitness:\033[0m\033[92m ' +
str(max_f) +
'\033[0m\033[94m, Generation:\033[0m\033[92m ' +
str(i) + '\033[0m\033[94m, Flag:\033[0m\033[92m ' +
str(max_flag) +
'\033[0m\033[94m, Solution:\033[0m\033[92m ' +
str(m) + ' \033[0m')
for j in range(len(paths[m].points)):
print(paths[m].points[j])
print("the solution", m, len(paths[m].points), max_flag)
break
# 选择
selected_path_list = alg.select(paths, self.selection_size,
occ_grid, pri_grid, starting_point,
end_point, privacy_sum,
obstacle_num)
# 形成couple, 交叉， 变异
new_path_list = []
for j in range(self.selection_size):
for k in range(j + 1, self.selection_size):
new_path1 = alg.cross_over(selected_path_list[j].points,
selected_path_list[k].points,
objectives, Kca)
new_path2 = alg.cross_over(selected_path_list[k].points,
selected_path_list[j].points,
objectives, Kca)
new_path1_mutated = alg.mutate(new_path1, objectives, Kca,
grid_x, grid_y, grid_z)
new_path1_mutated.fitness = alg.get_fitness(
new_path1_mutated, occ_grid, pri_grid, starting_point,
end_point, privacy_sum, obstacle_num)
new_path_list.append(new_path1_mutated)
new_path2_mutated = alg.mutate(new_path2, objectives, Kca,
grid_x, grid_y, grid_z)
new_path2_mutated.fitness = alg.get_fitness(
new_path2_mutated, occ_grid, pri_grid, starting_point,
end_point, privacy_sum, obstacle_num)
new_path_list.append(new_path2_mutated)
# 重插入
paths = alg.reinsert(paths, new_path_list, population)
if len(max_path.points) < T_budget:
return max_f, max_path, max_flag
else:
print("No Solution!")
return max_f, max_path, max_flag