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()
