def UAV_plot(self): T = self.xBest[0:N * ns] n = self.xBest[N * ns:2 * N * ns] phi = self.xBest[2 * N * ns:3 * N * ns] deltat = self.xBest[-1] T = np.reshape(T, (N, ns)) n = np.reshape(n, (N, ns)) phi = np.reshape(phi, (N, ns)) # 绘制迭代次数-适应度值 plt.figure() plt.title('适应度进化函数') plt.xlabel('迭代次数') plt.ylabel('适应度值') plt.plot(self.trace, color='g', linewidth=2) # 绘制最终的无人机位置的二维图 plt.figure() plt.title('二维图') My_UAV = UAV.UAV(N=5, ns=5, Dsafe=5, Dcomm=45, sigma=1.8e7) My_UAV.init_state() My_UAV.UAV_state(T, n, phi, deltat) J = My_UAV.UAV_fitness(self.xBest[-1]) x = J[1] y = J[2] plt.annotate('中心无人机', xy=(x[2, -1], y[2, -1])) plt.plot(x[:, -1], y[:, -1], '-o', markersize=7) plt.show()
def func(x): My_UAV = UAV.UAV(N=5, ns=5, Dsafe=5, Dcomm=45, sigma=1.8e7) My_UAV.init_state() My_UAV.UAV_state(x[0:N * ns], x[N * ns:2 * N * ns], x[2 * N * ns:3 * N * ns], x[-1]) J = My_UAV.UAV_fitness(x[-1]) return -J[0]
def __init__(self, width, height, rCount, sCount): self.width = width # width of board in meters self.height = height # width of board in meters self.rC = [] # roomba list self.srC = [] # spike roomba list self.boardTime = TimeMultiplier(1) pixelspermeter = 30 # number of pixels per meter theta = 2 * pi / rCount boardCenter = (width / 2) * pixelspermeter + 25 for x in range(rCount): # initialize roombas pos = Vector(boardCenter + 30 * sin(theta * x), boardCenter + 30 * cos(theta * x), 0) # put roombas in diagonal vel = Vector(sin(theta * x) / 3, cos(theta * x) / 3, 0) # random velocities (magnitude 1 m/s) rCircle = Circle(Point(pos.x, pos.y), 9.0 / 2) # creating circle object for roomba rLine = Line(Point(pos.x, pos.y), Point(pos.x + vel.x * 50, pos.y + vel.y * 50)) # velcoity vector line self.rC.append( Roomba(pos, vel, rCircle, rLine, self.boardTime)) # add new Roomba object to list for i in range( sCount ): # initialize spike roombas at theta = 0, pi/2, pi, and 3pi/2 pos = Vector(30 * width / 2 + 25 + cos(i * pi / 2) * 5 * 30, 30 * height / 2 + 25 + sin(i * pi / 2) * 5 * 30, 0) vel = Vector(-sin(i * pi / 2) / 3, cos(i * pi / 2) / 3, 0) rCircle = Circle(Point(pos.x, pos.y), 9.0 / 2) # creating spike roomba circle object self.srC.append( Spike(pos, vel, rCircle, i, self.boardTime)) # add new Spike object to list self.rCsrC = self.rC + self.srC #UAV initialization pos = Vector(boardCenter, boardCenter, 0) vel = Vector(0, 0, 0) rCircle = Circle(Point(pos.x, pos.y), 6.0) maxSpeed = 2 # in m/s self.uav = UAV(pos, vel, rCircle, maxSpeed, self.boardTime, self.rC, self.srC)
def UAV_plot(self): # 绘制迭代次数-适应度值 plt.figure() plt.title('适应度进化函数') plt.xlabel('迭代次数') plt.ylabel('适应度值') plt.plot(self.gb, color='g', linewidth=2) # 绘制最终的无人机位置的二维图 plt.figure() plt.title('二维图') My_UAV = UAV.UAV(N=5, ns=5, Dsafe=5, Dcomm=45, sigma=1.8e7) My_UAV.init_state() My_UAV.UAV_state(self.g[0:N * ns], self.g[N * ns:2 * N * ns], self.g[2 * N * ns:3 * N * ns], self.g[-1]) J = My_UAV.UAV_fitness(self.g[-1]) x = J[1] y = J[2] plt.annotate('中心无人机', xy=(x[2, -1], y[2, -1])) plt.plot(x[:, -1], y[:, -1], '-o', markersize=7) plt.show()
def plan(inital_pos, input_map, target): # inital_pos与target都是大小为2的列表 uav = UAV.UAV(inital_pos, input_map, 1, 2) path_record = [] path_record.append(inital_pos) enemyUK = enemy_record.get_enemy_pos() new_enemy_list = [] replanning = 0 while get_dis.eucliDist_m(uav.now_pos, target) > 0.001: if replanning == 1: update_map(uav, new_enemy_list) replanning = 0 dis_list = uav.dijskra_next_pos() next_pos = dis_list[target[0] * 20 + target[1]][0] move_length = get_dis.eucliDist_m(next_pos, uav.now_pos) if move_length > uav.step_length: mid = uav.step_length / move_length next_pos = [ uav.now_pos[0] + abs(next_pos[0] - uav.now_pos[0]) * mid, uav.now_pos[1] + abs(next_pos[1] - uav.now_pos[1]) * mid ] print('get new pos:') print(next_pos) uav.now_pos = next_pos path_record.append(next_pos) for enemy in enemyUK: if get_dis.eucliDist_m([enemy[0], enemy[1]], uav.now_pos) < uav.visual_field: replanning = 1 new_enemy_list.append(enemy) uav.enemy_find_list.append(enemy) enemyUK.remove(enemy) print('get new enemy') print(new_enemy_list) print(uav.map) return path_record
def run(): global turns_of_change global rate_of_change global destination time_swap = 1 MAP_RECORD = [] UAV_SET = [] MOVE_RECORD.clear() global_map = ( np.multiply(np.random.randn(CONFIG['width'], CONFIG['height']), 255.0 / (2.58 * 2)) + 127.5).astype(np.uint8) observer_uav = UAV(-1, CONFIG_PATH, logger) observer_uav.position[0] = 0 observer_uav.position[1] = 0 observer_uav.reset_destination(destination) observer_uav.openPrint = False observer_uav.weight_regression = False MAP_RECORD.append(copy.deepcopy(global_map)) #initialization for i in range(UAV_COUNTS): current_uav = UAV(i + 1, CONFIG_PATH, logger) current_uav.position[0] = 0 current_uav.position[1] = 0 current_uav.initialize_global_information(global_map) current_uav.reset_destination(destination) current_uav.openPrint = False UAV_SET.append(current_uav) while (len(UAV_SET) > 0 or observer_uav.working is True): #print("**time_swap: %d**"%time_swap) """ change_flag = (random.random() <= turns_of_change) if(change_flag is True): #print("[Notice]global map info is changed") element_count = CONFIG['width']*CONFIG['height'] change_count = int(element_count*rate_of_change) for i in range(change_count): number = random.randint(1,element_count) x = (number - 1)//int(CONFIG['height']) y = (number - 1)%int(CONFIG['height']) global_map[x][y] = random.randint(0,255) """ MAP_RECORD.append(copy.deepcopy(global_map)) #print("[Notice]update neighborhood information") for uav in UAV_SET: uav.update_one_skip_neighbor(UAV_SET) for uav in UAV_SET: uav.update_two_skip_neighbor() #print("[Notice]update environment information") for uav in UAV_SET: uav.update_local_environment_information(global_map, time_swap) for uav in UAV_SET: uav.update_environment_information() #print("[Notice]into stradegy period") for uav in UAV_SET: if ((uav.uid - 1) // UAV_BATCH < time_swap): uav.path_planning() #print("[Notice]into working period") for uav in UAV_SET: if ((uav.uid - 1) // UAV_BATCH < time_swap): uav.move() for uav in UAV_SET: if (uav.working is False): MOVE_RECORD[uav.uid] = uav.move_record UAV_SET.remove(uav) if (observer_uav.working is True): observer_uav.initialize_global_information(global_map) observer_uav.path_planning() observer_uav.move() time_swap += 1 avg_cost = 0 for uid, record in MOVE_RECORD.items(): path_cost = 0 path_record = [] position_record = [] time_step = 0 for element in record: time_step += 1 x = element[0] y = element[1] position_record.append([x, y]) if (len(path_record) > 0 and element[2] == path_record[-1][0]): continue path_record.append([element[2], time_step]) for element in path_record: grid_x = (element[0] - 1) // int(CONFIG['height']) grid_y = (element[0] - 1) % int(CONFIG['height']) path_cost += MAP_RECORD[element[1] - 1][grid_x][grid_y] avg_cost += path_cost if (observer_uav.openPrint is True): path = [element[0] for element in path_record] print("uav_{}'s move record is {}".format(uid, position_record)) print("uav_{}'s move grid's record is {}".format(uid, path)) print("uav_%d's path cost is %d" % (uid, path_cost)) avg_cost //= len(MOVE_RECORD) observer_path_cost = 0 observer_path_record = [] observer_position_record = [] time_step = 0 for element in observer_uav.move_record: time_step += 1 x = element[0] y = element[1] observer_position_record.append([x, y]) if (len(observer_path_record) > 0 and element[2] == observer_path_record[-1][0]): continue observer_path_record.append([element[2], time_step]) for element in observer_path_record: grid_x = (element[0] - 1) // int(CONFIG['height']) grid_y = (element[0] - 1) % int(CONFIG['height']) observer_path_cost += MAP_RECORD[element[1] - 1][grid_x][grid_y] if (observer_uav.openPrint is True): observer_path = [element[0] for element in observer_path_record] print("uav_observer's move record is {}".format( observer_position_record)) print("uav_observer's move grid's record is {}".format(observer_path)) print("uav_observer's path cost is %d" % (observer_path_cost)) print("average uav's path cost is %d" % (avg_cost)) extra_cost = float(avg_cost - observer_path_cost) / float(observer_path_cost) return extra_cost, observer_position_record
print("uav_3's path sum: %d "%sum3) print([node[2] for node in path3]) print("uav_4's path sum: %d "%sum4) print([node[2] for node in path4]) print("uav_5(shortest)'s path sum: %d "%sum5) print([node[2] for node in path5]) """ i = 0 all_extra_cost = 0.0 all_saved_time = 0.0 while(i < 100): print("process {}%".format(i+1)) uav_6 = UAV.UAV(6,'./config.json',logger) wide = uav_6.config['height']*uav_6.config['grid_size'] global_map = np.random.randn(uav_6.config['width'],uav_6.config['height']) global_map = (np.multiply(global_map,255.0/(2.58*2)) + 127.5).astype(np.uint8) uav_6.initialize_global_information(global_map) uav_6.update_all_level_maps() uav_6.position[0] = 0 uav_6.position[1] = 0 uav_7 = UAV.UAV(7,'./config.json',logger) uav_7.initialize_global_information(global_map) uav_7.update_all_level_maps(maxpool=True) uav_7.position[0] = uav_6.position[0] uav_7.position[1] = uav_6.position[1] uav_6.multi_levels = False uav_6.addWeight = False
def main(): ############################################################ ######################## DEFINITONS ####################### ############################################################ def upload_telemetry(client, last_telem, out): telemetry = interop.Telemetry(latitude=38.145215, longitude=-76.427942, altitude_msl=50, uas_heading=90) #send that info to the interop server delta = datetime.datetime.now() - last_telem out.set(1 / (delta.total_seconds())) client.post_telemetry(telemetry) def upload_all_targets(client, target_json, sys_db, out): cur = db.cursor() #allows execution of all SQL queries cur.execute("SELECT * FROM targets") #Fetches every row of the table; #Columns are as follows: #1st - target_id, 2 - type, 3 - latitude, 4 - longitude, 5 - orientation, 6 - shape, 7 - background-color, 8 - alphanumeric, 9 - alphanumeric_color # 10 - image path #note: target_id is a long/int, and latitude and longitude are floats/doubles for row in cur.fetchall(): target = interop.Target( type=row[ 1], #indexing starts from 0, data doesn't include target_id latitude=row[2], longitude=row[3], orientation=row[4], shape=row[5], background_color=row[6], alphanumeric=row[7], alphanumeric_color=row[8]) target = client.post_target(target) #send target values to server #open corresponding image file. Assumes the naming convention goes "1_lettercolor_letter_shapecolor_shape.png". Ex. "2_white_B_green_square.png" with open(imagedir + "/" + row[10] + '.png', 'rb') as f: #the 'rb' option reads the file in binary, as opposed to as a text file image_data = f.read() client.put_target_image(target.id, image_data) def view_current_targets(sys_db, out): cur = db.cursor() #allows execution of all SQL queries cur.execute("SELECT * FROM targets") for row in cur.fetchall(): target = interop.Target( type=row[ 1], #indexing starts from 0, data doesn't include target_id latitude=row[2], longitude=row[3], orientation=row[4], shape=row[5], background_color=row[6], alphanumeric=row[7], alphanumeric_color=row[8]) out.insert(END, target) out.insert(END, "\n") out.see(END) def download_mission(client, sys_db, out): try: missions = client.get_missions() out.insert( END, "Mission info downloaded from interoperability server\n") for wp in missions[0].mission_waypoints: insert_stmt = ( "INSERT INTO waypoints (wp_order, latitude, longitude, altitude, type) " "VALUES (%s, %s, %s, %s, %s)") data = (wp.order, wp.latitude, wp.longitude, wp.altitude_msl, "waypoint") cur = db.cursor() print(insert_stmt, data) cur.execute(insert_stmt, data) db.commit() out.insert(END, "Mission info uploaded to database.\n") out.see(END) except: out.insert(END, 'Something went wrong when downloading mission\n') out.see(END) def download_obstacles(client, sys_db, out): try: missions = client.get_missions() out.insert(END, "Obstacle info downloaded from interop\n") for wp in missions[0].mission_waypoints: insert_stmt = ( "INSERT INTO obstacles (wp_order, latitude, longitude, altitude, type) " "VALUES (%s, %s, %s, %s, %s)") data = (wp.order, wp.latitude, wp.longitude, wp.altitude_msl, "waypoint") cur = db.cursor() print(insert_stmt, data) cur.execute(insert_stmt, data) db.commit() out.insert(END, "Obstacle info uploaded to database.\n") out.insert(END, "\n") out.see(END) except: out.insert(END, 'Something went wrong when downloading obstacles\n') out.see(END) def bottle_drop(drone, out): try: drone.bottle_drop() out.insert(END, "Bottle drop signal sent\n") out.see(END) except: out.insert(END, "Error sending bottle drop signal\n") out.see(END) def ping_drone(drone, out): try: info = drone.get_info() out.insert(END, info["message"]) out.insert(END, "\n") out.see(END) except: out.insert(END, "Error pinging drone.\n") out.see(END) def drone_start_video(drone, out): stream = drone.take_picture() return 0 def drone_take_picture(drone, sys_db, out, pic_out): try: picture = drone.take_picture() out.insert(END, "Picture signal sent\n") insert_stmt = ( "INSERT INTO target_images (id, image) VALUES (%s, %s)") data = (picture["id"], picture["image"]) cur = db.cursor() cur.execute(insert_stmt, data) db.commit() im = PIL.Image.open(BytesIO(base64.b64decode(picture["image"]))) im2 = PIL.ImageTk.PhotoImage(im.resize((180, 160)).rotate(180)) pic_out.configure(image=im2) pic_out.image = im2 out.insert( END, "Picture: " + picture["filename"] + " uploaded to database\n") out.see(END) except: out.insert(END, "Error sending take picture signal\n") out.see(END) def interop_connect(url, username, password, out): try: #set up the connection to the interop server at the specified #url with the specified username/password client = interop.Client(url=url, username=username, password=password) out.insert( END, "Connected to " + url + " with username '" + username + "' and password '" + password + "'.\n") out.see(END) except: out.insert(END, "Something when wrong when trying to connect\n") out.see(END) def on_closing(): window.destroy() ############################################################ ###################### INITIALIZATION ###################### ############################################################ telemetry_open = False client = interop.Client(url='http://127.0.0.1:8000', username='******', password='******') datarate = 0 # to store avg datarate last_telem = datetime.datetime.now() imagedir = 'target_images' drone = UAV.UAV('http://192.168.1.31:5000', 'MSUUS', 'Unmanned2017') #Connect to the mySQL database db = MySQLdb.connect(host="localhost", user="******", passwd="password", db="MSUUS") #Use own credentials for actual database ############################################################ ###################### WINDOW SETUP ######################## ############################################################ window = tkinter.Tk() window.protocol("WM_DELETE_WINDOW", on_closing) window.title("MSUUS System Software v0.6") window.geometry("590x560") url = StringVar(window) url.set('http://127.0.0.1:8000') username = StringVar(window) username.set('testuser') password = StringVar(window) password.set('testpass') url_label = Label(window, text="Interop URL") url_label.place(x=10, y=10) url_textbox = Entry(window, textvariable=url) url_textbox.place(x=100, y=10) username_label = Label(window, text="Username:"******"Password:"******"Output") output_label.place(x=10, y=190) output_textbox = Text(window, width=79, wrap=WORD) output_textbox.place(x=10, y=210) output_scrollbar = Scrollbar(window, command=output_textbox.yview) output_textbox['yscrollcommand'] = output_scrollbar.set output_scrollbar.place(x=570, y=210, height=340) data_rate_str = StringVar(window) data_rate_str.set('0') data_rate_label = Label(window, text="Telemetry Data Rate:") data_rate_label.place(x=290, y=10) data_rate_field = Entry(window, textvariable=data_rate_str) data_rate_field.place(x=430, y=10, width=60) last_pic = PIL.ImageTk.PhotoImage(PIL.Image.open('blank.png')) picture_box_label = Label(window, text="Last Image:") picture_box_label.place(x=290, y=40) picture_box_field = Label(window, image=last_pic) picture_box_field.place(x=370, y=40, width=180, height=160) connect_button = Button( window, text="Interop Connect", command=lambda: interop_connect(url.get(), username.get(), password.get(), output_textbox)) connect_button.place(x=10, y=90) ping_button = Button(window, text="Ping Drone", command=lambda: ping_drone(drone, output_textbox)) ping_button.place(x=136, y=90) take_picture_button = Button( window, text="Take Picture", command=lambda: drone_take_picture(drone, db, output_textbox, picture_box_field)) take_picture_button.place(x=10, y=120) auto_picture_button = Button( window, text="Auto Picture", command=lambda: drone_take_picture(drone, db, output_textbox, picture_box_field)) auto_picture_button.place(x=112, y=120) auto_picture_button.configure(state='disable') stream_button = Button( window, text="Start Stream", command=lambda: drone_start_video(drone, db, output_textbox)) stream_button.place(x=214, y=120) stream_button.configure(state='disable') target_upload_button = Button( window, text="Download Mission", command=lambda: download_mission(client, db, output_textbox)) target_upload_button.place(x=10, y=150) ############################################################ ######################## MAIN LOOP ######################### ############################################################ while True: upload_telemetry(client, last_telem, data_rate_str) last_telem = datetime.datetime.now() window.update_idletasks() window.update()