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)