def searchNear(self, minF, offsetX, offsetY, offsetZ, cam):
"""
搜索节点周围的点
:param minF:F值最小的节点
:param offsetX:坐标偏移量
:param offsetY:
:return:
"""
# 越界检测
if (minF.point.x + offsetX < 0
or minF.point.x + offsetX > self.grid[0] - 1
or minF.point.y + offsetY < 0
or minF.point.y + offsetY > self.grid[1] - 1
or minF.point.z + offsetZ < 0
or minF.point.z + offsetZ > self.grid[2] - 1):
return
# 如果是障碍，就忽略
# if self.map3d[minF.point.x + offsetX][minF.point.y + offsetY][minF.point.z + offsetZ] != self.passTag:
# if self.map3d[minF.point.x + offsetX][minF.point.y + offsetY][minF.point.z + offsetZ] in self.passTag:
# print("$$$$$$$$$", currentPoint, minF, minF.step)
if self.map3d[minF.point.x +
offsetX][minF.point.y +
offsetY][minF.point.z +
offsetZ] in self.passTag:
return
# 如果在关闭表中，就忽略
currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY,
minF.point.z + offsetZ, cam)
# print("##########", currentPoint, minF, minF.step)
# if self.pointInCloseList(currentPoint):
# return
if self.point_in_close_list(currentPoint, minF.step + 1):
return
"""new setting for time limit"""
# print()
if minF.step + 1 > self.Tbudget:
# print("time limit", minF, minF.step, currentPoint )
return
# 设置单位花费
# step = 1/ self.Toptimal ## 0625
# step = 1
#
# if self.sumpri == 0:
# privacy_threat = 0
# else:
# # privacy_threat = (self.prigrid[minF.point.x + offsetX][minF.point.y + offsetY][minF.point.z + offsetZ] * math.exp(-(cam))) / self.sumpri ## 0625
# privacy_threat = self.prigrid[minF.point.x + offsetX][minF.point.y + offsetY][
# minF.point.z + offsetZ] * math.exp(-(cam))
## 0628 h_i*exp((-1/2)*ws*dis^2)
privacy_threat = caculate_privacy_surround(self.grid, currentPoint,
self.map3d, self.pri_radius)
# privacy_threat = 0
# grid_x = self.grid[0]
# grid_y = self.grid[1]
# grid_z = self.grid[2]
# r = max(self.pri_radius)
# current_x = minF.point.x + offsetX
# current_y = minF.point.y + offsetY
# current_z = minF.point.z + offsetZ
#
# min_x = max(current_x - r, 0)
# min_x = math.floor(min_x)
# max_x = min(current_x + r, grid_x - 1)
# max_x = math.ceil(max_x)
# min_y = max(current_y - r, 0)
# min_y = math.floor(min_y)
# max_y = min(current_x + r, grid_y - 1)
# max_y = math.ceil(max_y)
# min_z = max(current_z - r, 0)
# min_z = math.floor(min_z)
# max_z = min(current_z + r, grid_z - 1)
# max_z = math.ceil(max_z)
# for m in range(min_x, max_x + 1):
# for n in range(min_y, max_y + 1):
# for l in range(min_z, max_z + 1):
# if self.map3d[m][n][l] == 2 or self.map3d[m][n][l] == 3 or self.map3d[m][n][l] == 4:
# dis = np.sqrt(np.power((current_x - m), 2) + np.power((current_y - n), 2) + np.power((current_z - l), 2))
# h = 0
# if dis <= self.pri_radius[int(self.map3d[m][n][l]) - 2]:
# if self.map3d[m][n][l] == 2:
# h = 1
# elif self.map3d[m][n][l] == 3:
# h = 10
# elif self.map3d[m][n][l] == 4:
# h = 100
# privacy_threat += h * math.exp((-1 / 2) * np.power(dis, 2) * cam)
## 加入时间的约束惩罚
time_punishment = 1
if self.Toptimal < 0:
self.Toptimal = 0
if minF.step + 1 > self.Toptimal:
time_punishment = math.exp((minF.step + 1 - self.Toptimal) /
(self.Tbudget - self.Toptimal))
# type1
# print ("delta_g :", time_punishment * privacy_threat)
# delta_g = math.exp(time_punishment * privacy_threat)
delta_g = time_punishment * privacy_threat ## 0703
# type2
# delta_g = privacy_threat
# delta_g = step + cam_off + privacy_threat
# 如果不在openList中，就把它加入openlist
# currentNode = self.pointInOpenList(currentPoint)
# 用一个列表来收集相同的点
same_point_list = self.the_same_points_in_open_list(currentPoint)
if not same_point_list:
# print("currentPoint:", currentPoint, currentNode)
currentNode = AStar.Node(currentPoint,
self.endPoint,
self.ideallength,
g=minF.g + delta_g)
currentNode.father = minF
currentNode.cam = minF.cam + cam
currentNode.step = minF.step + 1
# self.openList.append(currentNode)
heappush(self.openList, currentNode)
#print("MinF$$$$$: ", minF.step, minF.point, currentNode.step, currentNode.point)
return
# # 如果在openList中，判断minF到当前点的G是否更小
# if minF.g + delta_g < currentNode.g: # 如果更小，就重新计算g值，并且改变father
# currentNode.g = minF.g + delta_g
# currentNode.father = minF
# currentNode.step = minF.step + 1
# currentNode.cam = minF.cam
# # print("MinF#####: ", currentNode.father.step, currentNode.father.point, currentNode.step, currentNode.point)
# 检查这些相同的点的step值，如果有相同的，就更新相同的
# 如果没有相同的，就看看当前的step值和最小的step值
# 如果最小的step值小，说明当前的step没用
# 如果当前的step小，说明当前的step值有用，添加到openlist中
smallest_step_num = self.Tbudget
same_step_in_list = False
same_node = None
for node in same_point_list:
if minF.step + 1 == node.step:
same_step_in_list = True
same_node = node
break
if smallest_step_num > node.step:
smallest_step_num = node.step
if same_step_in_list:
if minF.g + delta_g < same_node.g: # 如果更小，就重新计算g值，并且改变father
same_node.g = minF.g + delta_g
same_node.father = minF
same_node.step = minF.step + 1
same_node.cam = minF.cam
else:
if minF.step + 1 < smallest_step_num:
currentNode = AStar.Node(currentPoint,
self.endPoint,
self.ideallength,
g=minF.g + delta_g)
currentNode.father = minF
currentNode.cam = minF.cam + cam
currentNode.step = minF.step + 1
# self.openList.append(currentNode)
heappush(self.openList, currentNode)
def Astar_Path_Planning_online(config, iteration, log, num):
grid_x = config.grid_x
grid_y = config.grid_y
grid_z = config.grid_z
grid = config.grid
safety_threshold = config.safety_threshold
privacy_threshold = config.privacy_threshold
# privacy_radius = 1 ##
privacy_radius = config.privacy_radius
# drone parameter
starting_point = config.starting_point
end_point = config.end_point
T_budget = config.T_budget
T_optimal = config.T_optimal
viewradius = config.viewradius
Kca = config.Kca
threat_list = []
replantime = 0
preference = config.preference
# occ_grid_name = "occ_grid" + str(iteration) + ".npy"
# num is the number of the grid
occ_grid_name = os.getcwd() + "/data/" + "occ_grid-" + str(num) + ".npy"
occ_grid = np.load(file=occ_grid_name)
# occ_grid = np.load(file="occ_grid-1.npy")
pri_grid, privacy_sum = privacy_init(grid_x, grid_y, grid_z, occ_grid,
privacy_radius)
# occ_grid_known_name = "occ_grid_known" + str(iteration) + ".npy"
occ_grid_known_name = os.getcwd(
) + "/data/" + "occ_grid_known_initial" + str(iteration) + ".npy"
occ_grid_known = np.load(file=occ_grid_known_name)
# occ_grid_known = np.load(file="occ_grid_known.npy")
pri_grid_known, privacy_sum_known = privacy_init(grid_x, grid_y, grid_z,
occ_grid_known,
privacy_radius)
a = 0
for i in range(grid_x):
for j in range(grid_y):
for k in range(grid_z):
if occ_grid_known[i][j][k] != occ_grid[i][j][k]:
a += 1
# print(restricted_area_num, a, (grid_x * grid_y * grid_z * privacy_threshold))
exp_rate = 1 - a / (grid_x * grid_y * grid_z * privacy_threshold)
# print("exp_rate ",exp_rate , a)
# print("The occ_grid is: ")
# for m in range(grid_x):
# print("The value of x: ", m)
# print(occ_grid[m])
starttime = time.time()
# aStar = AStar(occ_grid, pri_grid_known, grid, privacy_sum_known, starting_point, end_point, [1], T_budget, threat_list, 0)
# 开始寻路
# trajectory_ref = aStar.start()
reference_path_name = os.getcwd() + "/data/" + "reference_path" + str(
iteration) + ".npy"
trajectory_ref_temp = np.load(file=reference_path_name)
# trajectory_ref_temp = np.load(file="reference_path.npy")
# trajectory_ref_temp = np.load(file="plan_path_Hybrid.npy")
trajectory_ref = []
for i in range(len(trajectory_ref_temp)):
point = Point(int(trajectory_ref_temp[i][0]),
int(trajectory_ref_temp[i][1]),
int(trajectory_ref_temp[i][2]),
int(trajectory_ref_temp[i][3]))
# if (pri_grid_known[point.x][point.y][point.z] > 0):
# print(point)
trajectory_ref.append(point)
endtime = time.time()
dtime = endtime - starttime
# print("程序运行时间：%.8s s" % dtime)
path_grid = copy.deepcopy(occ_grid)
sum = 0
if trajectory_ref == None:
print("No solution!")
exit(0)
else:
for point in trajectory_ref:
# for ii in range(len(trajectory_ref)):
# print(point)
path_grid[point.x][point.y][point.z] = 9
sum += pri_grid_known[point.x][point.y][point.z]
# print(point, pri_grid_known[point.x][point.y][point.z])
# print("----------------------\n", len(trajectory_ref))
# 再次显示地图
# print(path_grid, sum)
# trajectory_ref = [starting_point] + trajectory_ref
trajectory_plan = copy.deepcopy(trajectory_ref)
# sensor_initial = np.zeros(len(trajectory_plan))
# sensor_plan = copy.deepcopy(sensor_initial)
time_step = 0
# print("---------------------------------")
# print("The length of original plan is: ", len(trajectory_plan))
# for m in range(len(trajectory_plan)):
# print("The No.", m, " step: ", trajectory_plan[m])
# print()
idx = 0
num_of_no_solution = 0
while not (idx >= len(trajectory_plan)):
current_p = trajectory_plan[idx]
current_ca = trajectory_plan[idx].ca
if current_p.x == end_point.x and current_p.y == end_point.y and current_p.z == end_point.z:
break
next_p = trajectory_plan[idx + 1]
next_idx = idx + 1
# print("The UAV would move a step: ")
# print("The current point: ", current_p)
# print("The next point: ", next_p)
# print("The index of next point: ", next_idx, "\n")
if current_ca == 2: ## 1 = camera is open with high resolution, 2 = camera is off with lower resolution
time_step += 1
current_p = next_p
idx += 1
# print("The UAV has finished this step.\n")
continue
# take picture
# update occ_grid, pri_grid
flag, occ_grid_known, pri_grid_known, privacy_sum_known, threat_list = hasprivacythreat2(
current_p, occ_grid_known, occ_grid, pri_grid_known,
privacy_sum_known, config)
if flag:
## 0702
point = trajectory_plan[next_idx]
if (caculate_privacy_surround(grid, point, occ_grid_known,
privacy_radius)) == 0:
pass
else:
for j in range(idx + 1, len(trajectory_plan)):
point = trajectory_plan[j]
if (caculate_privacy_surround(grid, point, occ_grid_known,
privacy_radius)) > 0:
# print("have privacy risk!!")
next_p = trajectory_plan[j]
next_idx = j
# print(next_p)
# if (pri_grid_known[trajectory_plan[j].x][trajectory_plan[j].y][trajectory_plan[j].z] * math.exp(
# -(trajectory_plan[j].ca) )) > 0:
# next_p = trajectory_plan[j]
# next_idx = j
else:
break
if next_idx == len(trajectory_plan) - 1:
next_p = trajectory_plan[next_idx]
else:
next_idx += 1
next_p = trajectory_plan[next_idx]
"""删除冗余路径"""
if current_p.x == next_p.x and current_p.y == next_p.y and current_p.z == next_p.z: # 0623
# if current_p == next_p:
# print("delete redundant route", current_p, next_p)
first_part = trajectory_plan[:idx]
if next_idx == len(trajectory_plan) - 1:
next_part = []
else:
next_part = trajectory_plan[next_idx + 1:]
trajectory_plan = first_part + next_part
break
# print("The length of trajectory_plan: ", len(trajectory_plan))
T_plan = T_budget - (len(trajectory_plan) - 1) + (next_idx -
idx)
T_plan_optimal = T_optimal - (len(trajectory_plan) -
1) + (next_idx - idx)
# print("The time limit: ", T_plan, "\n")
# if T_plan < (abs(trajectory_plan.points[next_idx].x - trajectory_plan.points[idx].x) + abs(trajectory_plan.points[next_idx].y - trajectory_plan.points[idx].y) + \
# abs(trajectory_plan.points[next_idx].z - trajectory_plan.points[idx].z)):
# print("no solution!")
# print(T_plan, current_p, next_p)
distance = abs(trajectory_plan[next_idx].x -
trajectory_plan[idx].x) + abs(
trajectory_plan[next_idx].y -
trajectory_plan[idx].y) + abs(
trajectory_plan[next_idx].z -
trajectory_plan[idx].z)
## have enough time for planning
if T_plan >= distance:
# 开始寻路
start1 = time.time()
## 0628
print("producing local planning")
replantime += 1
aStar = AStar(occ_grid, pri_grid_known, grid, privacy_sum,
current_p, next_p, [1, 2, 3, 4], T_plan,
threat_list, 0, T_plan_optimal, preference,
privacy_radius)
trajectory_optimal_pp = aStar.start()
temp_sum = 0
PR_temp_sum_unknown = 0
PR_temp_sum_known = 0
length_PP = 0
no_solution_flag = 0
if trajectory_optimal_pp != None:
length_PP = len(trajectory_optimal_pp)
for jj in range(len(trajectory_optimal_pp)):
point = trajectory_optimal_pp[jj]
# print(point)
# temp_sum += pri_grid_known[point.x][point.y][point.z]
PR_temp_sum_unknown += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_temp_sum_known += caculate_privacy_surround(
grid, point, occ_grid_known, privacy_radius)
if PR_temp_sum_known > 0:
no_solution_flag = 1
elif len(trajectory_optimal_pp) > T_plan_optimal:
no_solution_flag = 2
else:
length_PP = 0
no_solution_flag = 3
## 如果找不到没有侵犯隐私的可行解，或者规划出的可行解超出了时间的约束，说明只进行路径规划不可行
if no_solution_flag > 0:
num_of_no_solution += 1
# print(occ_grid_known)
print(
"Online_Hybrid_Planning: No solution for local planning: from [%d, %d, %d] to [%d, %d, %d]. No soultion flag is %d, PR for PP is %f. length of PP is %d, T plan optimal is %d"
% (current_p.x, current_p.y, current_p.z, next_p.x,
next_p.y, next_p.z, no_solution_flag,
PR_temp_sum_known, length_PP, T_plan_optimal))
log.info(
"Online_Hybrid_Planning: No solution for local planning: from [%d, %d, %d] to [%d, %d, %d]. No soultion flag is %d, PR for PP is %f. length of PP is %d, T plan optimal is %d"
% (current_p.x, current_p.y, current_p.z, next_p.x,
next_p.y, next_p.z, no_solution_flag,
PR_temp_sum_known, length_PP, T_plan_optimal))
# aStar = AStar(occ_grid, pri_grid_known, grid, privacy_sum, current_p, next_p, [1, 2, 3, 4],
# T_plan, threat_list, 1, T_plan_optimal, preference, privacy_radius)
# trajectory_optimal = aStar.start()
trajectory_optimal = copy.deepcopy(
trajectory_optimal_pp)
else:
trajectory_optimal = copy.deepcopy(
trajectory_optimal_pp)
now_trajectory = []
# for m in range(idx + 1, next_idx + 1):
# print("original， The No.", m, " step: ", trajectory_plan[m])
first_part = trajectory_plan[0:idx + 1]
if next_idx == len(trajectory_plan) - 1:
following_part = []
else:
following_part = trajectory_plan[next_idx + 1:]
# following_part = trajectory_plan[next_idx + 1:]
if trajectory_optimal == None:
trajectory_optimal = []
now_trajectory = first_part + trajectory_optimal + following_part
# replan_flag = 0
# for m in range(len(trajectory_optimal)):
# print("plan， The No.", m, " step: ", trajectory_optimal[m])
# if (len(trajectory_optimal) != (next_idx - idx)):
# replan_flag = 1
# break
# if (trajectory_plan[m] != trajectory_optimal[m - idx - 1]):
# replan_flag = 1
#
# if replan_flag:
# replantime += 1 ## 排除重复规划的相同路径 0620
trajectory_plan = copy.deepcopy(now_trajectory)
# for ll in range(len(trajectory_plan)):
# sum += pri_grid_known[trajectory_plan[ll].x][trajectory_plan[ll].y][trajectory_plan[ll].z]
# cam_off += trajectory_plan[ll].ca
# print("now", trajectory_plan[ll])
# print("The length of now_trajectory_plan: ", len(trajectory_plan), sum, cam_off)
time_step += 1
idx = idx + 1
# print("The UAV has finished this step.\n")
#if idx == 4:
# print("the debug begin!")
# print("next point", idx,time_step, len(trajectory_plan))
# print(occ_grid)
path_grid2 = copy.deepcopy(occ_grid)
## log info
# sum of privacy risk with pri_grid
PR_sum_unknown_plan = 0
# sum of privacy risk with pri_grid_knwon
PR_sum_known_plan = 0
# times of camera reconfigured
num_ca_plan = 0
# times of with intrusion of restricted area with pri_grid
num_intruder_notknown_plan = 0
# times of with intrusion of restricted area with pri_grid_knwon
num_intruder_known_plan = 0
# num_should_avoid_intruder_plan = 0
# num_cannot_avoid_intruder_plan = 0
for point in trajectory_plan:
if point.ca == 1:
path_grid2[point.x][point.y][point.z] = 7
else:
path_grid2[point.x][point.y][point.z] = 10
num_ca_plan += 1
# sum_unknown_plan += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca))
# sum_known_plan += pri_grid_known[point.x][point.y][point.z] * math.exp(-(point.ca))
PR_sum_unknown_plan += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_sum_known_plan += caculate_privacy_surround(grid, point,
occ_grid_known,
privacy_radius)
if pri_grid[point.x][point.y][point.z] > 0 and occ_grid[point.x][
point.y][point.z] == 0:
num_intruder_notknown_plan += 1
if pri_grid_known[point.x][point.y][point.z] > 0 and occ_grid_known[
point.x][point.y][point.z] == 0:
num_intruder_known_plan += 1
# if occ_grid[point.x][point.y][point.z] == 2 or occ_grid[point.x][point.y][point.z] == 3 or occ_grid[point.x][point.y][point.z] == 4 :
# num_should_avoid_intruder_plan += 1
#
# if occ_grid_known[point.x][point.y][point.z] == 2 or occ_grid_known[point.x][point.y][point.z] == 3 or occ_grid_known[point.x][point.y][point.z] == 4:
# num_cannot_avoid_intruder_plan += 1
# print(point, pri_grid_known[point.x][point.y][point.z])
print("\033[94mFitness for replanned path:\033[0m \n ",
len(trajectory_plan) - 1, PR_sum_unknown_plan, PR_sum_known_plan,
num_ca_plan, num_intruder_notknown_plan, num_intruder_known_plan)
log.info("Online_Path_Planning: Length of replanned trajectory: %d" %
(len(trajectory_plan) - 1))
log.info(
"Online_Path_Planning: Sum of privacy threat of replanned trajectory(occ_grid): %f"
% PR_sum_unknown_plan)
log.info(
"Online_Path_Planning: Sum of privacy threat of replanned trajectory(occ_grid_known): %f"
% PR_sum_known_plan)
log.info(
"Online_Path_Planning: Times of turning off camera of replanned trajectory: %d"
% num_ca_plan)
# log.info("Online_Path_Planning: Times of intrusion of replanned trajectory: %d" % num_intruder_plan)
log.info(
"Online_Path_Planning: Times of intrusion of replanned trajectory(occ_grid): %d"
% num_intruder_notknown_plan)
log.info(
"Online_Path_Planning: Times of intrusion of replanned trajectory(occ_grid_known): %d"
% num_intruder_known_plan)
# log.info(
# "Online_Path_Planning: Times of intrusion of replanned trajectory(should avoid): %d" % num_should_avoid_intruder_plan)
# log.info(
# "Online_Path_Planning: Times of intrusion of replanned trajectory(cannot avoid): %d" % num_cannot_avoid_intruder_plan)
# 再次显示地图
PR_sum_unknown_ref = 0
PR_sum_known_ref = 0
num_ca_ref = 0
num_intruder_notknown_ref = 0
num_intruder_known_ref = 0
# num_should_avoid_intruder_ref = 0
# num_cannot_avoid_intruder_ref = 0
for point in trajectory_ref:
# sum_unknown_ref += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca) )
# sum_known_ref += pri_grid_known[point.x][point.y][point.z] * math.exp(-(point.ca))
PR_sum_unknown_ref += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_sum_known_ref += caculate_privacy_surround(grid, point,
occ_grid_known,
privacy_radius)
if point.ca == 2:
num_ca_ref += 1
# if pri_grid[point.x][point.y][point.z] != pri_grid_known[point.x][point.y][point.z]:
# print("$$$$$$$$$")
# print(point, pri_grid_known[point.x][point.y][point.z])
if pri_grid[point.x][point.y][point.z] > 0 and occ_grid[point.x][
point.y][point.z] == 0:
num_intruder_notknown_ref += 1
if pri_grid_known[point.x][point.y][point.z] > 0 and occ_grid_known[
point.x][point.y][point.z] == 0:
num_intruder_known_ref += 1
# if occ_grid[point.x][point.y][point.z] == 2 or occ_grid[point.x][point.y][point.z] == 3 or occ_grid[point.x][point.y][point.z] == 4 :
# num_should_avoid_intruder_ref += 1
#
# if occ_grid_known[point.x][point.y][point.z] == 2 or occ_grid_known[point.x][point.y][point.z] == 3 or occ_grid_known[point.x][point.y][point.z] == 4:
# num_cannot_avoid_intruder_ref += 1
# print("\033[94m Fitness for reference path:\033[0m \n", len(trajectory_ref) - 1, sum_ref, num_ca_ref,
# num_intruder_ref)
print("\033[94mFitness for preplanned path:\033[0m \n ",
len(trajectory_plan) - 1, PR_sum_unknown_ref, PR_sum_known_ref,
num_ca_ref, num_intruder_notknown_ref, num_intruder_known_ref)
log.info(
"Online_Path_Planning: Sum of privacy threat of preplanned trajectory(occ_grid): %f"
% PR_sum_unknown_ref)
log.info(
"Online_Path_Planning: Sum of privacy threat of preplanned trajectory(occ_grid_known): %f"
% PR_sum_known_ref)
log.info(
"Online_Path_Planning: Times of turning off camera of preplanned trajectory: %d"
% num_ca_ref)
# log.info("Online_Path_Planning: Times of intrusion of preplanned trajectory: %d" % num_intruder_ref)
log.info(
"Online_Path_Planning: Times of intrusion of preplanned trajectory(occ_grid): %d"
% num_intruder_notknown_ref)
log.info(
"Online_Path_Planning: Times of intrusion of preplanned trajectory(occ_grid_known): %d"
% num_intruder_known_ref)
# log.info(
# "Online_Path_Planning: Times of intrusion of preplanned trajectory(should avoid): %d" % num_should_avoid_intruder_ref)
# log.info(
# "Online_Path_Planning: Times of intrusion of preplanned trajectory(cannot avoid): %d" % num_cannot_avoid_intruder_ref)
#print(path_grid2, sum)
# print("---------------------------------")
# print("The last plan is finished!")
# print("The length of last plan is: ", len(trajectory_plan))
# for m in range(len(trajectory_plan)):
# print("The No.", m, " step: ", trajectory_plan[m])
end = time.time()
dtime = end - starttime
# print("程序运行时间：%.8s s" % dtime)
print("\033[94m Replan times: \033[0m", replantime)
log.info("Online_Path_Planning: Replanning times: %d" % replantime)
print("\033[94m No solution times: \033[0m", num_of_no_solution)
log.info("Online_Path_Planning: No solution times: %d" %
num_of_no_solution)
#grid_visualization(occ_grid, starting_point, end_point, trajectory_plan, trajectory_ref)
occ_grid_known_name = os.getcwd() + "/data/" + "occ_grid_known" + str(
iteration) + ".npy"
np.save(file=occ_grid_known_name, arr=occ_grid_known)
# np.save(file="occ_grid_known.npy", arr=occ_grid_known)
# b = np.load(file="occ_grid_known.npy")
# for m in range(grid_x):
# print("The value of x: ", m)
# print(b[m])
plan_path = np.zeros((len(trajectory_plan), 4))
for i in range(len(trajectory_plan)):
plan_path[i] = [
trajectory_plan[i].x, trajectory_plan[i].y, trajectory_plan[i].z,
trajectory_plan[i].ca
]
plan_path_PP_name = os.getcwd() + "/data/" + "plan_path_PP" + str(
iteration) + ".npy"
np.save(file=plan_path_PP_name, arr=plan_path)
# np.save(file="plan_path_PP.npy", arr=plan_path)
# c = np.load(file="plan_path_pp.npy")
# print(c, len(c))
exploration_rate = 0
for i in range(grid_x):
for j in range(grid_y):
for k in range(grid_z):
if occ_grid_known[i][j][k] != occ_grid[i][j][k]:
exploration_rate += 1
exploration_rate = 1 - exploration_rate / (grid_x * grid_y * grid_z *
privacy_threshold)
print("\033[94m exploration rate: \033[0m", exploration_rate)
log.info("Online_Path_Planning: Exploration rate: %f" % exploration_rate)
return
def PathInitial(config, reinitial_flag, iteration, log, num):
grid_x = config.grid_x
grid_y = config.grid_y
grid_z = config.grid_z
grid = config.grid
safety_threshold = config.safety_threshold
privacy_threshold = config.privacy_threshold
# privacy_radius = 1 ##
privacy_radius = config.privacy_radius
exploration_rate = config.exploration_rate
# drone parameter
starting_point = config.starting_point
end_point = config.end_point
T_budget = config.T_budget
T_optimal = config.T_optimal
viewradius = config.viewradius
Kca = config.Kca
threat_list = []
reinitial_flag = reinitial_flag
preference = config.preference
# 全局信息，用作baseline
# occ_grid_name = "occ_grid" + str(iteration) + ".npy"
occ_grid_name = os.getcwd() + "/data/" + "occ_grid-" + str(num) + ".npy"
occ_grid = np.load(file=occ_grid_name)
# occ_grid = np.load(file="occ_grid-1.npy")
pri_grid, privacy_sum = privacy_init(grid_x, grid_y, grid_z, occ_grid,
privacy_radius)
if reinitial_flag != 0:
# occ_grid_known, pri_grid_known, privacy_sum_known = initialmapwithknowngrid_ratio(grid_x, grid_y, grid_z,
# privacy_threshold, privacy_radius,
# occ_grid, exploration_rate)
# 最初始探索的地图
occ_grid_known_name = os.getcwd(
) + "/data/" + "occ_grid_known_initial" + str(iteration) + ".npy"
occ_grid_known = np.load(file=occ_grid_known_name)
pri_grid_known, privacy_sum_known = privacy_init(
grid_x, grid_y, grid_z, occ_grid_known, privacy_radius)
else:
# 本局地图信息，更新后的
occ_grid_known_name = os.getcwd() + "/data/" + "occ_grid_known" + str(
iteration) + ".npy"
occ_grid_known = np.load(file=occ_grid_known_name)
# occ_grid_known = np.load(file="occ_grid_known.npy")
pri_grid_known, privacy_sum_known = privacy_init(
grid_x, grid_y, grid_z, occ_grid_known, privacy_radius)
trajectory_ref = []
no_solution_flag = 0
if reinitial_flag == 1:
starttime = time.time()
# aStar1 = AStar(occ_grid_known, pri_grid_known, grid, privacy_sum_known, starting_point, end_point, [1], T_budget,
# threat_list, T_optimal)
aStar1 = AStar(occ_grid_known, pri_grid_known, grid, privacy_sum,
starting_point, end_point, [1, 2, 3, 4], T_budget,
threat_list, T_optimal, preference, privacy_radius)
#aStar1 = AStar(occ_grid_known, pri_grid_known, grid, privacy_sum, starting_point, end_point, [1], T_optimal,
# threat_list, T_optimal, preference)
trajectory_ref = aStar1.start()
endtime = time.time()
dtime = endtime - starttime
# print("程序运行时间：%.8s s" % dtime)
if trajectory_ref == None:
return no_solution_flag
else:
trajectory_ref = [starting_point] + trajectory_ref
# no_solution_flag = 1
refpath = np.zeros((len(trajectory_ref), 4))
for i in range(len(trajectory_ref)):
refpath[i] = [
trajectory_ref[i].x, trajectory_ref[i].y, trajectory_ref[i].z,
trajectory_ref[i].ca
]
reference_path_name = os.getcwd() + "/data/" + "reference_path" + str(
iteration) + ".npy"
np.save(file=reference_path_name, arr=refpath)
# b = np.load(file="reference_path.npy")
# print(b, len(b))
elif reinitial_flag == 2:
reference_path_name = os.getcwd() + "/data/" + "reference_path" + str(
iteration) + ".npy"
trajectory_ref_temp = np.load(file=reference_path_name)
for i in range(len(trajectory_ref_temp)):
point = Point(int(trajectory_ref_temp[i][0]),
int(trajectory_ref_temp[i][1]),
int(trajectory_ref_temp[i][2]),
int(trajectory_ref_temp[i][3]))
trajectory_ref.append(point)
planpath = np.zeros((len(trajectory_ref), 4))
# sum of privacy risk with occ_grid for reference path
PR_sum_unknown_ref = 0
# sum of privacy risk with occ_grid_known for reference path
PR_sum_known_ref = 0
# print("The occ_grid is: ")
# for m in range(grid_x):
# print("The value of x: ", m)
# print(occ_grid[m])
# print("The occ_grid_known is: ")
# for m in range(grid_x):
# print("The value of x: ", m)
# print(occ_grid_known[m])
num_ca = 0
num_intruder = 0
for point in trajectory_ref:
# sum_ref += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca))
# if pri_grid[point.x][point.y][point.z] > 0:
# print(point, pri_grid_known[point.x][point.y][point.z])
# print(point, caculate_privacy_surround(grid, point, occ_grid, privacy_radius), caculate_privacy_surround(grid, point, occ_grid_known, privacy_radius))
# print(caculate_privacy_surround(grid, point, occ_grid, privacy_radius))
# print(caculate_privacy_surround(grid, point, occ_grid_known, privacy_radius))
PR_sum_unknown_ref += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_sum_known_ref += caculate_privacy_surround(grid, point,
occ_grid_known,
privacy_radius)
print("\033[94m Fitness for reference path:\033[0m \n",
len(trajectory_ref) - 1, PR_sum_unknown_ref, PR_sum_known_ref)
# print(privacy_sum)
log.info("Initial_planning: Length of reference trajectory: %d" %
(len(trajectory_ref) - 1))
log.info(
"Initial_planning: Sum of privacy threat of reference trajectory(occ_grid): %f"
% PR_sum_unknown_ref)
log.info(
"Initial_planning: Sum of privacy threat of reference trajectory(occ_grid_known): %f"
% PR_sum_known_ref)
no_solution_flag = 0 ## 0628
if reinitial_flag == 1:
aStar2 = AStar(occ_grid, pri_grid, grid, privacy_sum, starting_point,
end_point, [1, 2, 3, 4], T_budget, threat_list,
T_optimal, preference, privacy_radius)
trajectory_plan = aStar2.start()
if trajectory_plan == None: ## 0628
return no_solution_flag
else:
trajectory_plan = [starting_point] + trajectory_plan
no_solution_flag = 1
# trajectory_plan = [starting_point] + trajectory_plan
planpath = np.zeros((len(trajectory_plan), 4))
for i in range(len(trajectory_plan)):
planpath[i] = [
trajectory_plan[i].x, trajectory_plan[i].y,
trajectory_plan[i].z, trajectory_plan[i].ca
]
plan_path_name = os.getcwd() + "/data/" + "plan_path" + str(
iteration) + ".npy"
np.save(file=plan_path_name, arr=planpath)
# c = np.load(file="plan_path.npy")
# print(c, len(c))
occ_grid_known = copy.deepcopy(occ_grid)
# sum of privacy risk with occ_grid for best path
PR_sum_unknown_plan = 0
# sum of privacy risk with occ_grid_known for best path
PR_sum_known_plan = 0
num_intruder_plan = 0
for point in trajectory_plan:
# sum_plan += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca))
# if pri_grid[point.x][point.y][point.z] > 0:
# print(point, caculate_privacy_surround(grid, point, occ_grid, privacy_radius), caculate_privacy_surround(grid, point, occ_grid_known, privacy_radius))
# print(point, pri_grid_known[point.x][point.y][point.z])
PR_sum_unknown_plan += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_sum_known_plan += caculate_privacy_surround(
grid, point, occ_grid_known, privacy_radius)
print("\033[94m Fitness for replanned path:\033[0m \n",
len(trajectory_plan) - 1, PR_sum_unknown_plan, PR_sum_known_plan)
# print(privacy_sum)
log.info("Initial_planning: Length of best trajectory: %d" %
(len(trajectory_plan) - 1))
log.info(
"Initial_planning: Sum of privacy threat of best trajectory(occ_grid): %f"
% PR_sum_unknown_plan)
log.info(
"Initial_planning: Sum of privacy threat of best trajectory(occ_grid_known): %f"
% PR_sum_known_plan)
# if reinitial_flag:
# occ_grid_known_name = "occ_grid_known" + str(iteration) + ".npy"
# np.save(file=occ_grid_known_name, arr=occ_grid_known)
# np.save(file="occ_grid_known.npy", arr=occ_grid_known)
# c = np.load(file="occ_grid_known.npy")
# for m in range(grid_x):
# print("The value of x: ", m)
# print(c[m])
return no_solution_flag
def PathInitial(config, reinitial_flag, iteration, log, num):
grid_x = config.grid_x
grid_y = config.grid_y
grid_z = config.grid_z
grid = config.grid
safety_threshold = config.safety_threshold
privacy_threshold = config.privacy_threshold
# privacy_radius = 1 ##
privacy_radius = config.privacy_radius
# drone parameter
starting_point = config.starting_point
end_point = config.end_point
T_budget = config.T_budget
T_optimal = config.T_optimal
reinitial_flag = reinitial_flag
# 全局信息，用作baseline
# occ_grid_name = "occ_grid" + str(iteration) + ".npy"
occ_grid_name = os.getcwd() + "/data_raw/" + "occ_grid_" + str(
num) + ".npy"
occ_grid = np.load(file=occ_grid_name)
# occ_grid = np.load(file="occ_grid-1.npy")
pri_grid, privacy_sum = privacy_init(grid_x, grid_y, grid_z, occ_grid,
privacy_radius)
refpath = []
reference_path = []
objective_list = []
no_solution_flag = 1
if reinitial_flag == 1:
reference_path.append([0, 0, 0, 1])
reference_path.append([1, 0, 0, 1])
reference_path.append([2, 0, 0, 1])
# objective_list.append([1, 0, 0])
# objective_list.append([1, 0, 3])
# objective_list.append([1, 4, 3])
# objective_list.append([1, 4, 6])
# objective_list.append([1, 9, 6])
# objective_list.append([1, 9, 9])
objective_list.append([3, 0, 0])
objective_list.append([3, 0, 3])
objective_list.append([3, 4, 3])
objective_list.append([3, 4, 7])
objective_list.append([3, 9, 7])
objective_list.append([3, 9, 9])
# objective_list.append([3, 18, 0])
# y_list = [0, 12, 18, 31, 37, 43, 49]
# x_list = [0, 16, 17, 24, 25, 33, 34 , 49]
#
# while objective_list[-1] != [1, 49, 49]:
# index = random.randint(0,1)
# if index == 0:
# now_x = objective_list[-1][1]
# if now_x == x_list[-1]:
# continue
# now_y = objective_list[-1][2]
# index_x = x_list.index(now_x)
# now_index_x = random.randint(index_x + 1, len(x_list)-1)
# objective_list.append([1, x_list[now_index_x], now_y])
# elif index == 1:
# now_x = objective_list[-1][1]
# now_y = objective_list[-1][2]
# if now_y == y_list[-1]:
# continue
# index_y = y_list.index(now_y)
# now_index_y = random.randint(index_y + 1, len(y_list) - 1)
# if y_list[now_index_y] == 0:
# continue
# objective_list.append([1, now_x, y_list[now_index_y]])
print(objective_list)
# objective_list = [[1, 0, 0],[1, 0, 12],[1, 25, 12],[1, 49, 12], [1, 49, 49]]
for i in range(1, len(objective_list)):
delta_x = objective_list[i][1] - objective_list[i - 1][1]
delta_y = objective_list[i][2] - objective_list[i - 1][2]
if delta_x > 0:
for j in range(abs(delta_x)):
reference_path.append([
3, objective_list[i - 1][1] + j,
objective_list[i - 1][2], 1
])
elif delta_x < 0:
for j in range(abs(delta_x)):
reference_path.append([
3, objective_list[i - 1][1] - j,
objective_list[i - 1][2], 1
])
elif delta_y > 0:
for j in range(abs(delta_y)):
reference_path.append([
3, objective_list[i - 1][1],
objective_list[i - 1][2] + j, 1
])
elif delta_y < 0:
for j in range(abs(delta_y)):
reference_path.append([
3, objective_list[i - 1][1],
objective_list[i - 1][2] - j, 1
])
# for i in range(38):
# reference_path.append([1, 0, i, 1])
# for i in range (1, 35):
# reference_path.append([1,i,37,1])
# for i in range (38, 50):
# reference_path.append([1,34,i,1])
# for i in range (25, 34):
# reference_path.append([1,i,37,1])
# for i in range (38,50):
# reference_path.append([1,33,i,1])
# for i in range (35,50):
# reference_path.append([1,i,49,1])
# reference_path.append([3, 9, 9, 1])
reference_path.append([3, 9, 9, 1])
reference_path.append([2, 9, 9, 1])
reference_path.append([1, 9, 9, 1])
reference_path.append([0, 9, 9, 1])
# reference_path.append([3, 18, 0, 1])
# reference_path.append([2, 18, 0, 1])
# reference_path.append([1, 18, 0, 1])
# reference_path.append([0, 18, 0, 1])
# reference_path.append([2, 14, 14, 1])
# reference_path.append([1, 14, 14, 1])
# reference_path.append([0, 14, 14, 1])
print(reference_path)
for i in range(len(reference_path)):
point = Point(int(reference_path[i][0]), int(reference_path[i][1]),
int(reference_path[i][2]), int(reference_path[i][3]))
refpath.append(point)
# print(refpath)
reference_path_name = os.getcwd(
) + "/data_raw/" + "reference_path" + str(iteration) + ".npy"
np.save(file=reference_path_name, arr=reference_path)
# b = np.load(file="reference_path.npy")
# print(b, len(b))
elif reinitial_flag == 2:
reference_path_name = os.getcwd(
) + "/data_raw/" + "reference_path" + str(iteration) + ".npy"
trajectory_ref_temp = np.load(file=reference_path_name)
for i in range(len(trajectory_ref_temp)):
point = Point(int(trajectory_ref_temp[i][0]),
int(trajectory_ref_temp[i][1]),
int(trajectory_ref_temp[i][2]),
int(trajectory_ref_temp[i][3]))
if occ_grid[point.x][point.y][point.z] != 0:
print("wrong")
# refpath.append(point)
# sum of privacy risk with occ_grid for reference path
PR_sum_unknown_ref = 0
# sum of privacy risk with occ_grid_known for reference path
PR_sum_known_ref = 0
num_ca = 0
num_intruder = 0
for point in refpath:
# sum_ref += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca))
# if pri_grid[point.x][point.y][point.z] > 0:
# print(point, pri_grid_known[point.x][point.y][point.z])
# print(point, caculate_privacy_surround(grid, point, occ_grid, privacy_radius), caculate_privacy_surround(grid, point, occ_grid_known, privacy_radius))
# print(caculate_privacy_surround(grid, point, occ_grid, privacy_radius))
# print(caculate_privacy_surround(grid, point, occ_grid_known, privacy_radius))
PR_sum_unknown_ref += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_sum_known_ref += caculate_privacy_surround(grid, point,
occ_grid_known,
privacy_radius)
print("\033[94m Fitness for reference path:\033[0m \n",
len(refpath) - 1, PR_sum_unknown_ref, PR_sum_known_ref)
# print(privacy_sum)
log.info("Initial_planning: Length of reference trajectory: %d" %
(len(refpath) - 1))
log.info(
"Initial_planning: Sum of privacy threat of reference trajectory(occ_grid): %f"
% PR_sum_unknown_ref)
log.info(
"Initial_planning: Sum of privacy threat of reference trajectory(occ_grid_known): %f"
% PR_sum_known_ref)
privacy_radius = [1, 1.5, 4]
grid = [5, 15, 15]
# 再次显示地图
PR_sum_unknown_ref = 0
PR_sum_known_ref = 0
num_ca_ref = 0
num_intruder_notknown_ref = 0
num_intruder_known_ref = 0
# num_should_avoid_intruder_ref = 0
# num_cannot_avoid_intruder_ref = 0
for i in range(len(trajectory_ref)):
point = Point(int(trajectory_ref[i][0]), int(trajectory_ref[i][1]),
int(trajectory_ref[i][2]), int(trajectory_ref[i][3]))
# sum_unknown_ref += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca) )
# sum_known_ref += pri_grid_known[point.x][point.y][point.z] * math.exp(-(point.ca))
PR_sum_unknown_ref += caculate_privacy_surround(grid, point, occ_grid,
privacy_radius)
PR_sum_known_ref += caculate_privacy_surround(grid, point, occ_grid_known,
privacy_radius)
if point.ca == 2:
num_ca_ref += 1
# if pri_grid[point.x][point.y][point.z] != pri_grid_known[point.x][point.y][point.z]:
# print("$$$$$$$$$")
# print(point, pri_grid_known[point.x][point.y][point.z])
# if pri_grid[point.x][point.y][point.z] > 0 and occ_grid[point.x][point.y][point.z] == 0:
# num_intruder_notknown_ref += 1
#
# if pri_grid_known[point.x][point.y][point.z] > 0 and occ_grid_known[point.x][point.y][point.z] == 0:
# num_intruder_known_ref += 1
# if occ_grid[point.x][point.y][point.z] == 2 or occ_grid[point.x][point.y][point.z] == 3 or occ_grid[point.x][point.y][point.z] == 4 :
def Astar_Sensor_Config_online(config, iteration, log, num):
grid_x = config.grid_x
grid_y = config.grid_y
grid_z = config.grid_z
grid = config.grid
safety_threshold = config.safety_threshold
privacy_threshold = config.privacy_threshold
# privacy_radius = 1 ##
privacy_radius = config.privacy_radius
# drone parameter
starting_point = config.starting_point
end_point = config.end_point
T_budget = config.T_budget
T_optimal = config.T_optimal
viewradius = config.viewradius
Kca = config.Kca
threat_list = []
#occ_grid, obstacle_num, occ_grid_known, pri_grid_known, privacy_sum_known = initialmap(grid_x, grid_y, grid_z,
# starting_point, end_point,
# safety_threshold,
# privacy_threshold,
# privacy_radius)
# occ_grid_name = "occ_grid" + str(iteration) + ".npy"
occ_grid_name = os.getcwd() + "/data/" + "occ_grid-" + str(num) + ".npy"
occ_grid = np.load(file=occ_grid_name)
# occ_grid = np.load(file="occ_grid-1.npy")
pri_grid, privacy_sum = privacy_init(grid_x, grid_y, grid_z, occ_grid,
privacy_radius)
# occ_grid_known_name = "occ_grid_known" + str(iteration) + ".npy"
occ_grid_known_name = os.getcwd(
) + "/data/" + "occ_grid_known_initial" + str(iteration) + ".npy"
occ_grid_known = np.load(file=occ_grid_known_name)
# occ_grid_known = np.load(file="occ_grid_known.npy")
pri_grid_known, privacy_sum_known = privacy_init(grid_x, grid_y, grid_z,
occ_grid_known,
privacy_radius)
#occ_grid_known, pri_grid_known, privacy_sum_known = initialmapwithknowngrid(grid_x, grid_y, grid_z,
# privacy_threshold, privacy_radius,
# occ_grid)
#pri_grid, privacy_sum = privacy_init(grid_x, grid_y, grid_z, occ_grid, privacy_radius)
# print("The occ_grid is: ")
# for m in range(grid_x):
# print("The value of x: ", m)
# print(occ_grid[m])
starttime = time.time()
#aStar = AStar(occ_grid, pri_grid_known, grid, privacy_sum_known, starting_point, end_point, 1 , T_budget)
# 开始寻路
#trajectory_ref = aStar.start()
#trajectory_ref_temp = np.load(file="plan_path.npy")
# 导入初规划路径路径结果
reference_path_name = os.getcwd() + "/data/" + "reference_path" + str(
iteration) + ".npy"
trajectory_ref_temp = np.load(file=reference_path_name)
# trajectory_ref_temp = np.load(file="reference_path.npy")
trajectory_ref = []
for i in range(len(trajectory_ref_temp)):
point = Point(int(trajectory_ref_temp[i][0]),
int(trajectory_ref_temp[i][1]),
int(trajectory_ref_temp[i][2]),
int(trajectory_ref_temp[i][3]))
trajectory_ref.append(point)
endtime = time.time()
dtime = endtime - starttime
# print("程序运行时间：%.8s s" % dtime)
path_grid = copy.deepcopy(occ_grid)
# print(len(pathList))
sum = 0
if trajectory_ref == None:
print("No solution!")
exit(0)
else:
for point in trajectory_ref:
#print(point)
path_grid[point.x][point.y][point.z] = 9
sum += pri_grid_known[point.x][point.y][point.z]
# print(point, pri_grid_known[point.x][point.y][point.z])
# print("----------------------", len(trajectory_ref))
# 再次显示地图
# print(path_grid, sum)
#trajectory_ref = [starting_point] + trajectory_ref
trajectory_plan = copy.deepcopy(trajectory_ref)
# sensor_initial = np.zeros(len(trajectory_plan))
# sensor_plan = copy.deepcopy(sensor_initial)
time_step = 0
# print("---------------------------------")
# print("The length of original plan is: ", len(trajectory_plan))
# for m in range(len(trajectory_plan)):
# print("The No.", m, " step: ", trajectory_plan[m])
# print()
idx = 0
current_f = sum + len(trajectory_plan)
replantime = 0
while not (idx >= len(trajectory_plan)):
current_p = trajectory_plan[idx]
current_ca = trajectory_plan[idx].ca
#print("currentnow:", current_p, idx)
if current_p.x == end_point.x and current_p.y == end_point.y and current_p.z == end_point.z:
# print("current:", current_p, idx)
break
next_p = trajectory_plan[idx + 1]
next_idx = idx + 1
# print (current_p,next_p,next_idx)
# print("The UAV would move a step: ")
# print("The current point: ", current_p)
# print("The next point: ", next_p)
# print("The index of next point: ", next_idx, "\n")
if current_ca == 2: ## 1 = camera is open with high resolution, 2 = camera is off with lower resolution 0630
time_step += 1
current_p = next_p
idx += 1
# print("The UAV has finished this step.\n")
continue
# take picture
# update occ_grid, pri_grid
flag, occ_grid_known, pri_grid_known, privacy_sum_known, threat_list = hasprivacythreat2(
current_p, occ_grid_known, occ_grid, pri_grid_known,
privacy_sum_known, config)
# print("The length of privacy_list: ", len(threat_list))
#for m in range(len(threat_list)):
# print(threat_list[m])
if flag:
# localization
# p_threat, h_impact = privacy_modeling()
# update occ_grid, pri_grid
replan_flag = 0
for j in range(idx + 1, len(trajectory_plan)):
if pri_grid_known[trajectory_plan[j].x][trajectory_plan[j].y][
trajectory_plan[j].z] > 0:
if trajectory_plan[j].ca != 2:
trajectory_plan[j].ca = 2
replan_flag = 1
# print("change sensor configuration for next point")
if replan_flag:
replantime += 1
# print("replan_time:", replantime)
# cam_off = 0
# for ll in range(len(trajectory_plan)):
# sum += pri_grid_known[trajectory_plan[ll].x][trajectory_plan[ll].y][trajectory_plan[ll].z]
# cam_off += trajectory_plan[ll].ca
# print("now", trajectory_plan[ll])
# print("The length of now_trajectory_plan: ", len(trajectory_plan), sum, cam_off)
# current_f = sum + len(trajectory_plan) + cam_off
# print("fitness", current_f)
time_step += 1
idx = idx + 1
# print("The UAV has finished this step.\n")
# print(occ_grid)
path_grid2 = copy.deepcopy(occ_grid)
## log info
# sum of privacy risk with pri_grid
PR_sum_unknown_plan = 0
# sum of privacy risk with pri_grid_knwon
PR_sum_known_plan = 0
# times of camera reconfigured
num_ca_plan = 0
# times of with intrusion of restricted area with pri_grid
num_intruder_notknown_plan = 0
# times of with intrusion of restricted area with pri_grid_knwon
num_intruder_known_plan = 0
# num_should_avoid_intruder_plan = 0
# num_cannot_avoid_intruder_plan = 0
for point in trajectory_plan:
if point.ca == 1:
path_grid2[point.x][point.y][point.z] = 7
else:
path_grid2[point.x][point.y][point.z] = 10
num_ca_plan += 1
# sum_unknown_plan += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca))
# sum_known_plan += pri_grid_known[point.x][point.y][point.z] * math.exp(-(point.ca))
PR_sum_unknown_plan += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_sum_known_plan += caculate_privacy_surround(grid, point,
occ_grid_known,
privacy_radius)
if pri_grid[point.x][point.y][point.z] > 0 and occ_grid[point.x][
point.y][point.z] == 0:
num_intruder_notknown_plan += 1
if pri_grid_known[point.x][point.y][point.z] > 0 and occ_grid_known[
point.x][point.y][point.z] == 0:
num_intruder_known_plan += 1
# if occ_grid[point.x][point.y][point.z] == 2 or occ_grid[point.x][point.y][point.z] == 3 or occ_grid[point.x][point.y][point.z] == 4 :
# num_should_avoid_intruder_plan += 1
#
# if occ_grid_known[point.x][point.y][point.z] == 2 or occ_grid_known[point.x][point.y][point.z] == 3 or occ_grid_known[point.x][point.y][point.z] == 4:
# num_cannot_avoid_intruder_plan += 1
# print(point, pri_grid_known[point.x][point.y][point.z])
print("\033[94mFitness for replanned path:\033[0m \n ",
len(trajectory_plan) - 1, PR_sum_unknown_plan, PR_sum_known_plan,
num_ca_plan, num_intruder_notknown_plan, num_intruder_known_plan)
log.info("Online_Sensor_Config: Length of replanned trajectory: %d" %
(len(trajectory_plan) - 1))
log.info(
"Online_Sensor_Config: Sum of privacy threat of replanned trajectory(occ_grid): %f"
% PR_sum_unknown_plan)
log.info(
"Online_Sensor_Config: Sum of privacy threat of replanned trajectory(occ_grid_known): %f"
% PR_sum_known_plan)
log.info(
"Online_Sensor_Config: Times of turning off camera of replanned trajectory: %d"
% num_ca_plan)
# log.info("Online_Sensor_Config: Times of intrusion of replanned trajectory: %d" % num_intruder_plan)
log.info(
"Online_Sensor_Config: Times of intrusion of replanned trajectory(occ_grid): %d"
% num_intruder_notknown_plan)
log.info(
"Online_Sensor_Config: Times of intrusion of replanned trajectory(occ_grid_known): %d"
% num_intruder_known_plan)
# log.info(
# "Online_Sensor_Config: Times of intrusion of replanned trajectory(should avoid): %d" % num_should_avoid_intruder_plan)
# log.info(
# "Online_Sensor_Config: Times of intrusion of replanned trajectory(cannot avoid): %d" % num_cannot_avoid_intruder_plan)
# 再次显示地图
PR_sum_unknown_ref = 0
PR_sum_known_ref = 0
num_ca_ref = 0
num_intruder_notknown_ref = 0
num_intruder_known_ref = 0
# num_should_avoid_intruder_ref = 0
# num_cannot_avoid_intruder_ref = 0
for point in trajectory_ref:
# sum_unknown_ref += pri_grid[point.x][point.y][point.z] * math.exp(-(point.ca))
# sum_known_ref += pri_grid_known[point.x][point.y][point.z] * math.exp(-(point.ca))
PR_sum_unknown_ref += caculate_privacy_surround(
grid, point, occ_grid, privacy_radius)
PR_sum_known_ref += caculate_privacy_surround(grid, point,
occ_grid_known,
privacy_radius)
if point.ca == 2:
num_ca_ref += 1
# print(point, pri_grid_known[point.x][point.y][point.z])
if pri_grid[point.x][point.y][point.z] > 0 and occ_grid[point.x][
point.y][point.z] == 0:
num_intruder_notknown_ref += 1
if pri_grid_known[point.x][point.y][point.z] > 0 and occ_grid_known[
point.x][point.y][point.z] == 0:
num_intruder_known_ref += 1
# if occ_grid[point.x][point.y][point.z] == 2 or occ_grid[point.x][point.y][point.z] == 3 or occ_grid[point.x][point.y][point.z] == 4 :
# num_should_avoid_intruder_ref += 1
#
# if occ_grid_known[point.x][point.y][point.z] == 2 or occ_grid_known[point.x][point.y][point.z] == 3 or occ_grid_known[point.x][point.y][point.z] == 4:
# num_cannot_avoid_intruder_ref += 1
# print("\033[94m Fitness for reference path:\033[0m \n", len(trajectory_ref) - 1, sum_ref, num_ca_ref,
# num_intruder_ref)
print("\033[94mFitness for preplanned path:\033[0m \n ",
len(trajectory_plan) - 1, PR_sum_unknown_ref, PR_sum_known_ref,
num_ca_ref, num_intruder_notknown_ref, num_intruder_known_ref)
log.info("Online_Sensor_Config: Length of preplanned trajectory: %d" %
(len(trajectory_ref) - 1))
log.info(
"Online_Sensor_Config: Sum of privacy threat of replanned trajectory(occ_grid): %f"
% PR_sum_unknown_ref)
log.info(
"Online_Sensor_Config: Sum of privacy threat of replanned trajectory(occ_grid_known): %f"
% PR_sum_known_ref)
log.info(
"Online_Sensor_Config: Times of turning off camera of preplanned trajectory: %d"
% num_ca_ref)
# log.info("Online_Sensor_Config: Times of intrusion of preplanned trajectory: %d" % num_intruder_ref)
log.info(
"Online_Sensor_Config: Times of intrusion of preplanned trajectory(occ_grid): %d"
% num_intruder_notknown_ref)
log.info(
"Online_Sensor_Config: Times of intrusion of preplanned trajectory(occ_grid_known): %d"
% num_intruder_known_ref)
# log.info(
# "Online_Sensor_Config: Times of intrusion of preplanned trajectory(should avoid): %d" % num_should_avoid_intruder_ref)
# log.info(
# "Online_Sensor_Config: Times of intrusion of preplanned trajectory(cannot avoid): %d" % num_cannot_avoid_intruder_ref)
#print(path_grid2, sum)
# print("---------------------------------")
# print("The last plan is finished!")
# print("The length of last plan is: ", len(trajectory_plan))
# for m in range(len(trajectory_plan)):
# print("The No.", m, " step: ", trajectory_plan[m])
end = time.time()
dtime = end - starttime
# print("程序运行时间：%.8s s" % dtime)
print("\033[94m Replan times: \033[0m", replantime)
log.info("Online_Sensor_Config: Replanning times: %d" % replantime)
#grid_visualization(occ_grid, starting_point, end_point, trajectory_plan, trajectory_ref)
occ_grid_known_name = os.getcwd() + "/data/" + "occ_grid_known" + str(
iteration) + ".npy"
np.save(file=occ_grid_known_name, arr=occ_grid_known)
# np.save(file="occ_grid_known.npy", arr=occ_grid_known)
# b = np.load(file="occ_grid_known.npy")
# for m in range(grid_x):
# print("The value of x: ", m)
# print(b[m])
plan_path = np.zeros((len(trajectory_plan), 4))
for i in range(len(trajectory_plan)):
plan_path[i] = [
trajectory_plan[i].x, trajectory_plan[i].y, trajectory_plan[i].z,
trajectory_plan[i].ca
]
plan_path_SC_name = os.getcwd() + "/data/" + "plan_path_SC" + str(
iteration) + ".npy"
np.save(file=plan_path_SC_name, arr=plan_path)
# np.save(file="plan_path_SC.npy", arr=plan_path)
# c = np.load(file="plan_path_SC.npy")
# print(c, len(c))
exploration_rate = 0
for i in range(grid_x):
for j in range(grid_y):
for k in range(grid_z):
if occ_grid_known[i][j][k] != occ_grid[i][j][k]:
exploration_rate += 1
exploration_rate = 1 - exploration_rate / (grid_x * grid_y * grid_z *
privacy_threshold)
print("\033[94m exploration rate: \033[0m", exploration_rate)
log.info("Online_Sensor_Config: Exploration rate: %f" % exploration_rate)
return
def searchNear(self, minF, offsetX, offsetY, offsetZ, cam):
"""
搜索节点周围的点
:param minF:F值最小的节点
:param offsetX:坐标偏移量
:param offsetY:
:return:
"""
# 越界检测
if (minF.point.x + offsetX < 0
or minF.point.x + offsetX > self.grid[0] - 1
or minF.point.y + offsetY < 0
or minF.point.y + offsetY > self.grid[1] - 1
or minF.point.z + offsetZ < 0
or minF.point.z + offsetZ > self.grid[2] - 1):
return
# 如果是障碍，就忽略
# if self.map3d[minF.point.x + offsetX][minF.point.y + offsetY][minF.point.z + offsetZ] != self.passTag:
# if self.map3d[minF.point.x + offsetX][minF.point.y + offsetY][minF.point.z + offsetZ] in self.passTag:
if self.map3d[minF.point.x +
offsetX][minF.point.y +
offsetY][minF.point.z +
offsetZ] in self.passTag:
return
# 如果在关闭表中，就忽略
currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY,
minF.point.z + offsetZ, cam)
if self.pointInCloseList(currentPoint):
return
"""new setting for time limit"""
# 设置单位花费
# step = 1/self.Toptimal
step = 1
# privacy_threat = (self.prigrid[minF.point.x + offsetX][minF.point.y + offsetY][minF.point.z + offsetZ] * math.exp(-(cam)) )/self.sumpri
privacy_threat = caculate_privacy_surround(self.grid, currentPoint,
self.map3d, self.pri_radius)
time_punishment = 1
if minF.step + 1 > self.Toptimal:
time_punishment = math.exp((minF.step + 1 - self.Toptimal) /
(self.Tbudget - self.Toptimal))
# delta_g = self.preference * time_punishment * step + privacy_threat
# type1
delta_g = time_punishment * privacy_threat
# type2
#delta_g = privacy_threat
# 如果不在openList中，就把它加入openlist
# currentNode = self.pointInOpenList(currentPoint)
# 用一个列表来收集相同的点
"""
same_point_list = self.the_same_points_in_open_list(currentPoint)
if not same_point_list:
# print("currentPoint:", currentPoint, currentNode)
currentNode = AStar.Node(currentPoint, self.endPoint, self.ideallength, g=minF.g + delta_g)
currentNode.father = minF
currentNode.cam = minF.cam + cam
currentNode.step = minF.step + 1
self.openList.append(currentNode)
#print("MinF$$$$$: ", minF.step, minF.point, currentNode.step, currentNode.point)
return
"""
currentNode = self.pointInOpenList(currentPoint)
if not currentNode:
currentNode = AStar.Node(currentPoint,
self.endPoint,
self.ideallength,
g=minF.g + delta_g)
currentNode.father = minF
#self.openList.append(currentNode)
heappush(self.openList, currentNode)
return
# 如果在openList中，判断minF到当前点的G是否更小
if minF.g + delta_g < currentNode.g: # 如果更小，就重新计算g值，并且改变father
currentNode.g = minF.g + delta_g
currentNode.father = minF
"""
def searchNear(self, minF, offsetX, offsetY, offsetZ, cam):
"""
搜索节点周围的点
:param minF:F值最小的节点
:param offsetX:坐标偏移量
:param offsetY:
:return:
"""
# 越界检测
if (minF.point.x + offsetX < 0
or minF.point.x + offsetX > self.grid[0] - 1
or minF.point.y + offsetY < 0
or minF.point.y + offsetY > self.grid[1] - 1
or minF.point.z + offsetZ < 0
or minF.point.z + offsetZ > self.grid[2] - 1):
return
# 如果是障碍，就忽略
if self.map3d[minF.point.x +
offsetX][minF.point.y +
offsetY][minF.point.z +
offsetZ] in self.passTag:
return
# 如果在关闭表中，就忽略
currentPoint = Point(minF.point.x + offsetX, minF.point.y + offsetY,
minF.point.z + offsetZ, cam)
if self.point_in_close_list(currentPoint, minF.step + 1):
return
"""new setting for time limit"""
# print()
if minF.step + 1 > self.Tbudget:
return
# 设置单位花费
privacy_threat = caculate_privacy_surround(self.grid, currentPoint,
self.map3d, self.pri_radius)
# if currentPoint.x == 2 and currentPoint.y == 3 and currentPoint.z == 2:
# print("grid:", self.map3d)
# print("privacy risk:", privacy_threat)
## 加入时间的约束惩罚
# time_punishment = 1
# # if self.Toptimal < 0:
# # self.Toptimal = 0
# if minF.step + 1 > self.Toptimal:
# alpha = 1
# time_punishment = alpha * math.exp((minF.step + 1 - self.Toptimal) / (self.Tbudget - self.Toptimal))
# if privacy_threat == 0:
# delta_g = time_punishment
# else:
# delta_g = time_punishment * privacy_threat
# else:
# delta_g = time_punishment * privacy_threat
if minF.step + 1 <= self.Toptimal:
delta_g = privacy_threat
else:
delta_g = privacy_threat + (minF.step + 1 - self.Toptimal) / (
self.Tbudget - self.Toptimal)
# delta_g = privacy_threat + (minF.step + 1 - self.Toptimal) / (self.Tbudget - self.Toptimal)
# delta_g = privacy_threat + (minF.step + 1 - self.Toptimal)
# 用一个列表来收集相同的点
same_point_list = self.the_same_points_in_open_list(currentPoint)
if not same_point_list:
currentNode = AStar.Node(currentPoint,
self.endPoint,
self.ideallength,
g=minF.g + delta_g)
currentNode.father = minF
currentNode.cam = minF.cam + cam
currentNode.step = minF.step + 1
# self.openList.append(currentNode)
heappush(self.openList, currentNode)
return
smallest_step_num = self.Tbudget
same_step_in_list = False
same_node = None
for node in same_point_list:
if minF.step + 1 == node.step:
same_step_in_list = True
same_node = node
break
if smallest_step_num > node.step:
smallest_step_num = node.step
if same_step_in_list:
if minF.g + delta_g < same_node.g: # 如果更小，就重新计算g值，并且改变father
same_node.g = minF.g + delta_g
same_node.father = minF
same_node.step = minF.step + 1
same_node.cam = minF.cam
else:
if minF.step + 1 < smallest_step_num:
currentNode = AStar.Node(currentPoint,
self.endPoint,
self.ideallength,
g=minF.g + delta_g)
currentNode.father = minF
currentNode.cam = minF.cam + cam
currentNode.step = minF.step + 1
# self.openList.append(currentNode)
heappush(self.openList, currentNode)
