def check_user_input(active, automate, lr, epsilon, agent, network_path, client, old_posit, initZ, phase, fig_z, fig_nav, env_folder):
for event in pygame.event.get():
if event.type == pygame.QUIT:
active = False
pygame.quit()
# Training keys control
if event.type == pygame.KEYDOWN and phase =='train':
if event.key == pygame.K_l:
# Load the parameters - epsilon
cfg = read_cfg(config_filename='configs/config.cfg', verbose=False)
lr = cfg.lr
print('Updated Parameters')
print('Learning Rate: ', cfg.lr)
if event.key == pygame.K_RETURN:
# take_action(-1)
automate = False
print('Saving Model')
# agent.save_network(iter, save_path, ' ')
agent.save_network(network_path)
# agent.save_data(iter, data_tuple, tuple_path)
print('Model Saved: ', network_path)
if event.key == pygame.K_BACKSPACE:
automate = automate ^ True
if event.key == pygame.K_r:
# reconnect
client = []
client = airsim.MultirotorClient()
client.confirmConnection()
# posit1_old = client.simGetVehiclePose()
client.simSetVehiclePose(old_posit,
ignore_collison=True)
agent.client = client
if event.key == pygame.K_m:
agent.get_state()
print('got_state')
# automate = automate ^ True
# Set the routine for manual control if not automate
if not automate:
# print('manual')
# action=[-1]
if event.key == pygame.K_UP:
action = 0
elif event.key == pygame.K_RIGHT:
action = 1
elif event.key == pygame.K_LEFT:
action = 2
elif event.key == pygame.K_d:
action = 3
elif event.key == pygame.K_a:
action = 4
elif event.key == pygame.K_DOWN:
action = -2
elif event.key == pygame.K_y:
pos = client.getPosition()
client.moveToPosition(pos.x_val, pos.y_val, 3 * initZ, 1)
time.sleep(0.5)
elif event.key == pygame.K_h:
client.reset()
# agent.take_action(action)
elif event.type == pygame.KEYDOWN and phase == 'infer':
if event.key == pygame.K_s:
# Save the figures
file_path = env_folder + 'results/'
fig_z.savefig(file_path+'altitude_variation.png', dpi=1000)
fig_nav.savefig(file_path+'navigation.png', dpi=1000)
print('Figures saved')
if event.key == pygame.K_BACKSPACE:
client.moveByVelocityAsync(vx=0, vy=0, vz=0, duration=0.1)
automate = automate ^ True
return active, automate, lr, client
import setup_path
import airsim
import argparse
import time
# Parse configuration files
parser = argparse.ArgumentParser(description='hello_drone')
parser.add_argument('--ip', type=str, default='localhost') # ip config
# parser.add_argument('--pretrain_num_epochs', type=int, default=15) # how many epoch to pretrain
args = parser.parse_args()
ipaddr = args.ip
client = airsim.MultirotorClient(ip=ipaddr)
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
landed = client.getMultirotorState().landed_state
print(landed)
if landed == airsim.LandedState.Landed:
print("taking off...")
client.takeoffAsync().join()
else:
print("already flying...")
client.takeoffAsync().join()
client.hoverAsync().join()
client.moveToPositionAsync(0, 0, -10, 5).join()
wayPointsSize = options.waypoints
OFFSETS = {
"Drone1": [0, 0, 0],
"Drone2": [0, -5, 0],
"Drone3": [5, 0, 0],
"Drone4": [5, 5, 0],
"Drone5": [10, 5, 0],
"Drone6": [5, 10, 0]
}
dronesID = list(OFFSETS.keys())
ip_id = f"127.0.0.{options.ip}"
client = airsim.MultirotorClient(ip=ip_id)
client.confirmConnection()
if GLOBAL_HAWK_ACTIVE:
setGlobalHawk(client)
controllers = []
for drone in dronesID:
controllers.append(
controller(client,
drone,
OFFSETS[drone],
ip=options.ip,
wayPointsSize=wayPointsSize,
estimatorWindow=options.estimatorWindow))
def __init__(self,
radius=2,
altitude=10,
speed=2,
iterations=1,
center=[1, 0],
snapshots=None):
self.radius = radius
self.altitude = altitude
self.speed = speed
self.iterations = iterations
self.snapshots = snapshots
self.snapshot_delta = None
self.next_snapshot = None
self.z = None
self.snapshot_index = 0
self.takeoff = False # whether we did a take off
if self.snapshots is not None and self.snapshots > 0:
self.snapshot_delta = 360 / self.snapshots
if self.iterations <= 0:
self.iterations = 1
if len(center) != 2:
raise Exception(
"Expecting '[x,y]' for the center direction vector")
# center is just a direction vector, so normalize it to compute the actual cx,cy locations.
cx = float(center[0])
cy = float(center[1])
length = math.sqrt((cx * cx) + (cy * cy))
cx /= length
cy /= length
cx *= self.radius
cy *= self.radius
self.client = airsim.MultirotorClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.home = self.client.getMultirotorState(
).kinematics_estimated.position
# check that our home position is stable
start = time.time()
count = 0
while count < 100:
pos = self.client.getMultirotorState(
).kinematics_estimated.position
if abs(pos.z_val - self.home.z_val) > 1:
count = 0
self.home = pos
if time.time() - start > 10:
print(
"Drone position is drifting, we are waiting for it to settle down..."
)
start = time
else:
count += 1
self.center = pos
self.center.x_val += cx
self.center.y_val += cy
"""
test python environment
"""
import airsim
# connect to the AirSim simulator
client = airsim.MultirotorClient() #与sirsim建立联系
client.confirmConnection() #确认联系建立成功
# get control
client.enableApiControl(
True) #获得API控制权，确保不会被遥控器抢占。使用 isApiControlEnabled 可以检查 API 是否具有控制权。
# unlock
client.armDisarm(True) #与现实结合，考虑到解锁上锁的操作
# Async methods returns Future. Call join() to wait for task to complete.
client.takeoffAsync().join() #起飞，。join等待完成
client.landAsync().join() #降落
# lock
client.armDisarm(False)
# release control
client.enableApiControl(False) #释放API控制权
def start_game_in_editor(self):
if not (settings.ip == '127.0.0.1'):
print("can not start the game in a remote machine")
exit(0)
if (os.name == "nt"):
unreal_pids_before_launch = utils.find_process_id_by_name(
"UE4Editor.exe")
subprocess.Popen(self.cmd, shell=True)
time.sleep(2)
unreal_pids_after_launch = utils.find_process_id_by_name(
"UE4Editor.exe")
else:
unreal_pids_before_launch = utils.find_process_id_by_name(
"UE4Editor")
subprocess.Popen(self.cmd, shell=True)
time.sleep(2)
unreal_pids_after_launch = utils.find_process_id_by_name(
"UE4Editor")
diff_proc = [
] # a list containing the difference between the previous UE4 processes
# and the one that is about to be launched
# wait till there is a UE4Editor process
while not (len(diff_proc) == 1):
time.sleep(3)
diff_proc = (utils.list_diff(unreal_pids_after_launch,
unreal_pids_before_launch))
settings.game_proc_pid = diff_proc[0]
#time.sleep(30)
client = airsim.MultirotorClient(settings.ip)
connection_established = False
connection_ctr = 0 # counting the number of time tried to connect
# wait till connected to the multi rotor
time.sleep(1)
while not (connection_established):
try:
#os.system(self.press_play_file)
time.sleep(2)
connection_established = client.confirmConnection()
except Exception as e:
if (connection_ctr >= settings.connection_count_threshold
and msgs.restart_game_count >=
settings.restart_game_from_scratch_count_threshold):
print(
"couldn't connect to the UE4Editor multirotor after multiple tries"
)
print("memory utilization:" +
str(psutil.virtual_memory()[2]) + "%")
exit(0)
if (connection_ctr == settings.connection_count_threshold):
self.restart_game()
print("connection not established yet")
time.sleep(5)
connection_ctr += 1
client = airsim.MultirotorClient(settings.ip)
pass
"""
def iniciar_drone(self, n, L1, L2, GPS):
airsim.YawMode.is_rate = False
self.client = airsim.MultirotorClient()
self.client.confirmConnection()
self.setInfo(n=n, L1=L1, L2=L2, GPS=GPS)
print(self.nombre, " Conectado")
import airsim
import rospy
import time
from math import pi, sqrt, cos, sin
import io
import numpy as np
from PIL import Image
from airsim_ros_pkgs.msg import GimbalAngleEulerCmd
try:
drone = airsim.MultirotorClient()
drone.confirmConnection()
except Exception as err:
print("Please start airsim first")
exit()
buck = "DeerBothBP2_19"
drone.simSetObjectPose(
buck, airsim.Pose(airsim.Vector3r(10, 0, 0),
airsim.Quaternionr(0, 0, 0, 1)))
def real_time_classification(cov_mei,
cov_mhi,
mean_mei_hu,
mean_mhi_hu,
is_frames=True):
# parameters
sim_speed = 1 # you can use this to slow down the simulation if your machine can't keep up
rgb_images = deque()
seg_images = deque()
scores_cache = deque()
slide_step = 1
average_size = 3
train_labels = list(
['walking', 'jogging', 'waving', 'pointing', 'hovering'])
scores_sum = np.zeros(len(train_labels))
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
# set object ID
found = client.simSetSegmentationObjectID("[\w]*", 2, True)
print("Set background object id: %r" % found)
found = client.simSetSegmentationObjectID("BP_Personnel_2", 0, True)
print("Set target 1 object id: %r" % found)
target_colors = []
palette = cv2.imread("seg_palette.png")
target_colors.append(palette[0, 0, :])
target_colors.append(palette[0, 1, :])
# real-time classification with sliding window method. results is the average of last 3 windows.
if is_frames:
t_max = 50.0 / 30 # simulation length
dt = 1.0 / 30 # time between frames
tol = 35
window_size = 30
else:
t_max = 100.0 / 300 # simulation length
dt = 1.0 / 30 # time between frames
sensor_size = [360, 480] # resize
dvs = DVS(sensor_size=sensor_size, thresh=0.25) # load dvs class
window_size = 50
client.simContinueForTime(dt / sim_speed)
client.simPause(True)
for i in range(int(t_max / dt)):
print("collecting frame {} / {}".format(i, int(t_max / dt)))
# get raw, depth, and segmentation images
responses = client.simGetImages(
[airsim.ImageRequest("0", airsim.ImageType.Scene, False, False)])
response = responses[0]
img1d = np.frombuffer(response.image_data_uint8,
dtype=np.uint8) # get numpy array
img = img1d.reshape(response.height, response.width, 3)
img = cv2.resize(img, (480, 360), interpolation=cv2.INTER_AREA)
rgb_images.append(cv2.cvtColor(img, cv2.COLOR_BGR2GRAY))
client.simContinueForTime(dt / sim_speed)
if len(rgb_images) == window_size:
if is_frames:
mei, mhi = calmeimhi(np.asarray(rgb_images).astype(np.float64),
0,
window_size - 1,
slide_step,
tol,
is_frames=True)
else:
all_events_arr, time_event, tf = dvs.get_event_sequence(
np.asarray(rgb_images).astype(np.float64), dt)
mei, mhi = calmeimhi(time_event,
None,
None,
None,
None,
is_frames=False)
mei_hu = hu_moments(mei)
mhi_hu = hu_moments(mhi)
label_record = []
mahad_record = []
for f in range(mean_mei_hu.shape[1]):
mahad_mei = np.sqrt(
np.dot(
np.matmul(
(mei_hu -
np.expand_dims(mean_mei_hu[:, f], axis=1)).T,
inv(cov_mei)),
(mei_hu - np.expand_dims(mean_mei_hu[:, f], axis=1))))
# if mahad_mei[0] < 28:
mahad_mhi = np.sqrt(
np.dot(
np.matmul(
(mhi_hu -
np.expand_dims(mean_mhi_hu[:, f], axis=1)).T,
inv(cov_mhi)),
(mhi_hu - np.expand_dims(mean_mhi_hu[:, f], axis=1))))
# if mahad_mhi[0] < 28:
label_record.append(train_labels[f])
mahad_record.append(mahad_mei + mahad_mhi)
scores_cache.append(mahad_record)
scores_sum += np.asarray(mahad_record)[:, 0, 0]
if len(scores_cache) == average_size:
classified_labels = label_record[int(np.argmin(scores_sum))]
print('classified labels: {labels}'.format(
labels=classified_labels))
scores_sum -= np.asarray(scores_cache.popleft())[:, 0, 0]
rgb_images.popleft()
client.simPause(False)
client.armDisarm(False)
client.reset()
client.enableApiControl(False)
def start_controller(self):
drone_name = self.drone_name
is_mininet_enabled = self.is_mininet_enabled
cur_yaw = self.cur_yaw
scale = self.scale
speed = self.speed
# create airsim client
client = airsim.MultirotorClient()
# create helpers
screenshot_helper = screenshotHelper(drone_name, client)
mininet_helper = mininetHelper()
# connect to the AirSim simulator
client.confirmConnection()
client.enableApiControl(True, drone_name)
client.armDisarm(True, drone_name)
multirotor_state = client.getMultirotorState(vehicle_name=drone_name)
log.info('multirotor_state={}'.format(multirotor_state))
landed = multirotor_state.landed_state
if landed == airsim.LandedState.Landed:
log.info("taking off...")
client.takeoffAsync(vehicle_name=drone_name).join()
else:
log.info("already flying...")
client.hoverAsync(vehicle_name=drone_name).join()
multirotor_state = client.getMultirotorState(vehicle_name=drone_name)
home_gps_location = multirotor_state.gps_location
pos = GpsUtils.geodeticToNedFast(home_gps_location, home_gps_location)
log.info('gps={}, pos={}'.format(home_gps_location, pos))
kinematics_state = client.simGetGroundTruthKinematics(
vehicle_name=drone_name)
log.debug('kinematics_state={}'.format(kinematics_state))
origin_pos = kinematics_state.position
display_info = (
'Press AWSD,R(up)F(down)QE(turn left/right) key to move the drone.\n'
'Press P to take images.\n'
'Press B key to reset to original state.\n'
'Press O key to release API control.\n'
'drone will be moved {}m with speed {}m/s.\n').format(scale, speed)
while True:
pressed_key = airsim.wait_key(display_info).decode('utf-8').upper()
log.info("pressed_key={}".format(pressed_key))
if pressed_key == 'P':
image_file_list = screenshot_helper.do_screenshot(is_display=True)
if is_mininet_enabled:
mininet_helper.send_image(image_file_list[0])
elif pressed_key == 'O':
client.enableApiControl(False, drone_name)
break
elif pressed_key == 'B':
client.reset()
else:
move_xyz_yaw = self._DIRECTION_DICT.get(pressed_key, None)
if move_xyz_yaw is None:
log.error('invalid key')
continue
dx, dy, dz, cur_yaw = self.compute_abs_direction(
cur_yaw, scale, move_xyz_yaw)
# move the drong
cur_state = client.getMultirotorState(vehicle_name=drone_name)
cur_pos = cur_state.kinematics_estimated.position
next_pos = airsim.Vector3r(
cur_pos.x_val + dx, cur_pos.y_val + dy, cur_pos.z_val + dz)
log.info("try to move: {} -> {}".format(cur_pos, next_pos))
rc = client.moveToPositionAsync(
next_pos.x_val, next_pos.y_val, next_pos.z_val, speed,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=cur_yaw),
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
vehicle_name=drone_name)
log.info("rc: {}".format(rc))
if is_mininet_enabled:
mininet_helper.move_drone(cur_pos, next_pos)
# is collision?
collision_info = client.simGetCollisionInfo(vehicle_name=drone_name)
if collision_info.has_collided:
log.error('collided! collision_info={}'.format(collision_info))
client.enableApiControl(False, drone_name)
break
import setup_path
import airsim
client = airsim.MultirotorClient(ip="asus1")
#client = airsim.MultirotorClient(ip="192.168.86.61")
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
landed = client.getMultirotorState().landed_state
if landed == airsim.LandedState.Landed:
print("already landed...")
client.landAsync().join()
else:
print("landing...")
client.landAsync().join()
client.armDisarm(False)
client.enableApiControl(False)
def __init__(self):
# Connect to the Airsim environment
self.cl = airsim.MultirotorClient()
self.cl.confirmConnection()
self.duration = 0.3
# from controller.Controller import *
class CNNController(Controller):
def take_action(self, action):
velocity = 1
if (action == 1):
angle = -2 * pi / 20
elif (action == 0):
angle = 0
elif (action == 2):
angle = 2 * pi / 20
self.moveByVolocity(velocity, angle)
return velocity, angle
def control(self, predict):
self.take_action(np.argmax(predict))
cnn_model = CNNModel('controller/cnn/models/CNNModel.json',
'controller/cnn/models/CNNWeight.hdf5')
if __name__ == "__main__":
cnn_controller = CNNController(airsim.MultirotorClient())
cnn_controller.take_off()
while True:
try:
img = cnn_controller.getRGBImg()
predict = cnn_model.predict(img)
cnn_controller.control(predict)
except KeyboardInterrupt:
break
import setup_path
import airsim
import pprint
def print_state():
print("===============================================================")
state = client.getMultirotorState()
print("state: %s" % pprint.pformat(state))
return state
client = airsim.MultirotorClient(ip="msi")
client.confirmConnection()
state = print_state()
client.enableApiControl(True)
if state.ready:
print("drone is ready!")
else:
print("drone is not ready!")
if state.ready_message:
print(state.ready_message)
import setup_path
import airsim
import time
# connect to the AirSim simulator
client = airsim.MultirotorClient(ip='msi')
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
print("fly")
client.moveToPositionAsync(0, 0, -5, 5).join()
print("reset")
client.reset()
client.enableApiControl(True)
client.armDisarm(True)
time.sleep(5)
print("done")
print("fly")
client.moveToPositionAsync(0, 0, -10, 5).join()
def __init__(self):
# connect to the AirSim simulator
self.client = airsim.MultirotorClient()
self.client.confirmConnection()
self.action_size = 3
self.level = 0
from threading import Thread
# Open the calibration parameter file created from step 3
calibration = open('.\src\calibration\calibration.pckl', 'rb')
# Grab the data and store it in variables
cameraMatrix, distCoeffs = pickle.load(calibration)
# Close the file
calibration.close()
# Default aruco dictionary
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
# Create the airsim client
flyClient = airsim.MultirotorClient()
flyClient.confirmConnection()
#flyClient.enableApiControl(True)
#flyClient.armDisarm(True)
# Takeoff thread
def takeoff():
t = Thread(target=flyClient.takeoffAsync())
t.start()
# Fly thread
def fly(direction):
"""
+x = forward
-x = backward
+y = right
def __init__(self, ip="0.0.0.0", portBase=8400):
# We Spawn 'n' threads corresponding to number of channels
self.channels = 9
self.ports = list()
self.ip = ip
self.threads = list()
self.portBase = portBase
self.socks = dict() #list()
self.connections = dict() #list()
self.channelTable = {
0: {
"name": "throttle",
"func": self.setThrottle
},
1: {
"name": "pitch",
"func": self.setPitch
},
2: {
"name": "roll",
"func": self.setRoll
},
3: {
"name": "yaw",
"func": self.setYaw
},
4: {
"name": "aux1",
"func": self.setAux1
},
5: {
"name": "aux2",
"func": self.setAux2
},
}
# self.channelDescriptors
print("Spawning Listener Threads to listen to the Base Station...")
for i in range(0, self.channels):
self.ports.append(portBase + i)
self.threads.append(
Thread(target=self.ChannelControllers, args=(i, )))
self.MainThread = Thread(target=self.ChannelSyncToSim)
print("Starting Listener Threads, Base PORT is " + str(portBase))
# We Now need to setup the AirSim Simulator before we can start the threads!
############################ AirSim MultiCopter Initialization ############################
client = airsim.MultirotorClient()
self.client = client
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
# Some Parameter Definitions, tweak them for realistic behavior
self.timeSlice = 0.0005
self.rmin = -1
self.rmax = 1
self.pmin = -1
self.pmax = 1
self.ymin = -6
self.ymax = 6
self.tmin = 0
self.tmax = 6
self.t = self.r = self.y = self.p = self.a1 = self.a2 = 0
def __init__(self):
self.client = airsim.MultirotorClient(ip="192.168.0.7", port=41451)
self.client.confirmConnection()
self.client.enableApiControl(True)
self.client.armDisarm(True)
import airsim
import numpy as np
import time
import math
# 参数初始化
# 初始化载入的无人机，数量用len(Drone)表示 注意不要写成 All_Drones = ['"Drone1"', '"Drone2"'...] 形式，虽然print是"Drone1"，但是实际是'"Drone1"'
All_Drones = [
"Drone1", "Drone2", "Drone3", "Drone4", "Drone5", "Drone6", "Drone7",
"Drone8"
] # 无人机在此添加
NumOfDrones = len(All_Drones) # 得到无人机的数量
NumOfState = 3 # 后修改state的个数直接在此修改，现在只用了X Y Z三个state，后面可以用一个矩阵保存所有state的信息，该矩阵的列数就是状态数
T_control = 0.001 # 初始化无人机api接口的执行周期
# 实例化各个函数类对象
Drone_takeoff = [
airsim.MultirotorClient().takeoffAsync() for i in range(NumOfDrones)
] # 起飞实例化多个对象
Drone_moveToZ = [
airsim.MultirotorClient().moveToZAsync(0, 0) for j in range(NumOfDrones)
] # 上升到指定高度实例化多个对象
Drone_move = [
airsim.MultirotorClient().moveByVelocityAsync(0, 0, 0, 0)
for k in range(NumOfDrones)
] # 移动实例化多个对象
# 后面其实可以用nx3的矩阵保存数据
Z = np.array([[-5.0], [-4.5], [-4.0], [-6.0], [-5.0], [-4.5], [-4.0],
[-6.0]]) # 代码优化后可以设置为初始飞行指定高度
deviation_X = np.array([[0.0], [-3.0], [-9.0], [-15.0], [-21.0], [-18.0],
[-12.0], [-6.0]]) # 机体坐标系和全局坐标系之间的初始差距,
deviation = np.array([[0.0, 0.0, 0.0], [-3.0, 0.0, 0.0], [-9.0, 0.0, 0.0],
[-15.0, 0.0, 0.0], [-21.0, 0.0, 0.0], [-18.0, 0.0, 0.0],
def check_user_input(active, automate, lr, epsilon, agent, network_path,
client, old_posit, initZ):
for event in pygame.event.get():
if event.type == pygame.QUIT:
active = False
pygame.quit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_l:
# Load the parameters - epsilon
cfg = read_cfg(config_filename='configs/config.cfg',
verbose=False)
lr = cfg.lr
print('Updated Parameters')
print('Learning Rate: ', cfg.lr)
if event.key == pygame.K_RETURN:
# take_action(-1)
automate = False
print('Saving Model')
# agent.save_network(iter, save_path, ' ')
agent.save_network(network_path)
# agent.save_data(iter, data_tuple, tuple_path)
print('Model Saved: ', network_path)
if event.key == pygame.K_BACKSPACE:
automate = automate ^ True
if event.key == pygame.K_r:
# reconnect
client = []
client = airsim.MultirotorClient()
client.confirmConnection()
# posit1_old = client.simGetVehiclePose()
client.simSetVehiclePose(old_posit, ignore_collison=True)
agent.client = client
if event.key == pygame.K_m:
agent.get_state()
print('got_state')
# automate = automate ^ True
# Set the routine for manual control if not automate
if not automate:
# print('manual')
# action=[-1]
if event.key == pygame.K_UP:
action = 0
elif event.key == pygame.K_RIGHT:
action = 1
elif event.key == pygame.K_LEFT:
action = 2
elif event.key == pygame.K_d:
action = 3
elif event.key == pygame.K_a:
action = 4
elif event.key == pygame.K_DOWN:
action = -2
elif event.key == pygame.K_y:
pos = client.getPosition()
client.moveToPosition(pos.x_val, pos.y_val, 3 * initZ, 1)
time.sleep(0.5)
elif event.key == pygame.K_h:
client.reset()
# agent.take_action(action)
return active, automate, lr, client
def disconnect_drone():
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(False)
def __init__(self):
# Connect to the AirSim simulator
print("Connecting...")
self.client = airsim.MultirotorClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.client.armDisarm(True)
# Number of people meshes in the scene
self.numPeople = 9
self.initialPose = []
# Info about the initial pose of all target models
for i in range(self.numPeople):
pose = self._safe_simGetObjectPose("TargetActor" + str(i))
self.initialPose.append(pose)
# Currently active target person mesh
self.activeMesh = -1
# The position of the face of current target model
self.targetPos = None
# The target position in which the camera should be placed for a frontal shot
self.frontalPos = None
# Orientation of front_center camera
self.qCam = None
# Speed
self.duration = 0.3
# Maximum possible speed
self.maxSpeed = 0.5
# Maximum possible camera angle change
self.maxAngle = 0.1
# Time
self.episode_duration = 20
self.start_time = None
# Times the target is reached
self.targetReached = 0
# Action space
self.action_space = spaces.Box(low=np.array([
-self.maxSpeed, -self.maxSpeed, -self.maxSpeed, -self.maxAngle,
-self.maxAngle
]),
high=np.array([
+self.maxSpeed, +self.maxSpeed,
+self.maxSpeed, +self.maxAngle,
+self.maxAngle
]),
dtype=np.float64)
# Observation space
responses = self.client.simGetImages([
airsim.ImageRequest("front_center", airsim.ImageType.Scene, False,
False)
])
self.observation_space = spaces.Box(low=0,
high=255,
shape=(responses[0].height,
responses[0].width, 3),
dtype=np.uint8)
self.seed()
def yahSearch(goal):
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
client.takeoffAsync().join()
goalX, goalY = goal
isGoal = False
pq = PriorityQueue()
visited = []
currentNode = Node((0, 0))
isGoal = False
pq.put((1, (currentNode)))
while not isGoal:
z = pq.get()
while not pq.empty():
pq.get()
h = z[0]
x, y = z[1].getCargo()
print(x, y)
currentNode = z[1]
print(currentNode)
client.moveToPositionAsync(x,
y,
-1,
5,
drivetrain=DrivetrainType.ForwardOnly,
yaw_mode=YawMode(False, 0))
visited.append((x, y))
# GOAL TEST
if (x, y) == (goalX, goalY):
break
northSafe, eastSafe, southSafe, westSafe, noneSafe = checkAdj(
x, y, visited, client)
if northSafe:
newNode1 = Node((x + 10, y), currentNode)
dist = calcHeuristic(x + 10, y, goalX, goalY)
pq.put((dist, (newNode1)))
if eastSafe:
newNode2 = Node((x, y + 10), currentNode)
dist = calcHeuristic(x, y + 10, goalX, goalY)
pq.put((dist, (newNode2)))
if southSafe:
newNode3 = Node((x - 10, y), currentNode)
dist = calcHeuristic(x - 10, y, goalX, goalY)
pq.put((dist, (newNode3)))
if westSafe:
newNode4 = Node((x, y - 10), currentNode)
dist = calcHeuristic(x, y - 10, goalX, goalY)
pq.put((dist, (newNode4)))
if noneSafe:
currentNode = currentNode.parent
x, y = currentNode.getCargo()
h = calcHeuristic(x, y, goalX, goalY)
pq.put((h, (currentNode)))
returnPath(currentNode)
def airsim_initialization():
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
client.takeoffAsync().join()
def yahSearch(goal,droneNetwork):
moveMag = 10
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
client.takeoffAsync().join()
goalX,goalY = goal
isGoal = False
pq = PriorityQueue()
visited = []
currentNode = Node((0,0))
isGoal = False
pq.put((1,(currentNode)))
hpriq = PriorityQueue()
inInitial = True
while not isGoal:
time.sleep(3)
z = pq.get()
h = z[0]
x,y = z[1].getCargo()
curX, curY = (x,y)
currentNode = z[1]
time.sleep(3)
visited.append((x,y))
# GOAL TEST
if (x,y) == (goalX,goalY):
print("GOAL REACHED!")
break
if (x+moveMag,y) not in visited:
upH = (calcHeuristic(x+moveMag,y,goalX,goalY),(x+moveMag,y),0)
hpriq.put(upH)
if (x,y-moveMag) not in visited:
leftH = (calcHeuristic(x,y-moveMag,goalX,goalY),(x,y-moveMag),-1.5)
hpriq.put(leftH)
if inInitial:
downH = (calcHeuristic(x-moveMag,y,goalX,goalY),(x-moveMag,y),3)
hpriq.put(downH)
inInitial = False
if (x,y+moveMag) not in visited:
rightH = (calcHeuristic(x,y+moveMag,goalX,goalY),(x,y+moveMag),1.5)
hpriq.put(rightH)
safeMove = False
while not hpriq.empty():
movement = hpriq.get()
x, y = movement[1]
h = movement[0]
if checkSafe(client,movement[2],droneNetwork):
time.sleep(5)
safeMove = True
newNode = Node((x,y),currentNode)
pq.put((h,(newNode)))
while not hpriq.empty():
hpriq.get()
client.moveToPositionAsync(x,y,-1,3,drivetrain=DrivetrainType.ForwardOnly,yaw_mode=YawMode(False,0))
time.sleep(7)
client.hoverAsync()
break
if safeMove == False:
currentNode = currentNode.parent
x,y = currentNode.getCargo()
h = calcHeuristic(x,y,goalX,goalY)
pq.put((h,(currentNode)))
printPathAndGraph(currentNode)
"""
飞正方形（速度控制）
"""
import airsim
import time
#本程序存在问题，先后进行，并不是同时操作的。
client = airsim.MultirotorClient() # connect to the AirSim simulator
client.confirmConnection()
client.enableApiControl(True, "Drone1")
client.enableApiControl(True, "Drone2")
client.armDisarm(True, "Drone1")
client.armDisarm(True, "Drone2")
uav1 = client.takeoffAsync(vehicle_name="Drone1") # 第一阶段：起飞
uav2 = client.takeoffAsync(vehicle_name="Drone2") # 第一阶段：起飞
uav1.join()
uav2.join()
uav1 = client.moveToZAsync(
-5,
1,
vehicle_name="Drone1",
) # 第二阶段：上升到5米高度
uav2 = client.moveToZAsync(
-5,
1,
vehicle_name="Drone2",
) # 第二阶段：上升到5米高度
uav1.join()
uav2.join()
# 飞正方形 速度为1m/s
uav1 = client.moveByVelocityZAsync(1, 0, -5, 8,
import airsim
client = airsim.MultirotorClient("10.211.55.4")
client.confirmConnection()
from dronekit import connect, VehicleMode
vehicle = connect("127.0.0.1:14551", wait_ready=True)
import time, datetime, math
import numpy as np
from pymavlink import mavutil
from pymavlink import mavextra
from pymavlink.rotmat import Vector3
# from MAVProxy.modules.lib import LowPassFilter2p
# vel_filter = LowPassFilter2p.LowPassFilter2p(200.0, 30.0)
def get_gps_time(tnow):
leapseconds = 18
SEC_PER_WEEK = 7 * 86400
epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - leapseconds
epoch_seconds = int(tnow - epoch)
week = int(epoch_seconds) // SEC_PER_WEEK
t_ms = int(tnow * 1000) % 1000
week_ms = (epoch_seconds % SEC_PER_WEEK) * 1000 + ((t_ms//200) * 200)
return week, week_ms
last_msg_time = time.time()
last_gps_pos = None
now = time.time()
import airsim
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
client.takeoffAsync().join()
client.moveToPositionAsync(-10, 10, -10, 5).join()
# take images
responses = client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.DepthVis),
airsim.ImageRequest("1", airsim.ImageType.DepthPlanner, True)
])
print('Retrieved images: %d', len(responses))
# do something with the images
for response in responses:
if response.pixels_as_float:
print("Type %d, size %d" %
(response.image_type, len(response.image_data_float)))
airsim.write_pfm(os.path.normpath('/temp/py1.pfm'),
airsim.getPfmArray(response))
else:
print("Type %d, size %d" %
(response.image_type, len(response.image_data_uint8)))
airsim.write_file(os.path.normpath('/temp/py1.png'),
response.image_data_uint8)
def __init__(self, logger_name="plane_log.txt", if_network=0):
self.if_debug = 0
self.if_log = 1
self.if_save_log = 1
self.if_rnn_network = 0
self.logger = plane_logger(logger_name)
self.client = airsim.MultirotorClient()
self.current_pos = np.zeros([3, 1])
self.current_spd = np.zeros([3, 1])
self.current_acc = np.zeros([3, 1])
self.current_rot = np.eye(3, 3)
self.target_pos = np.zeros([3, 1])
self.target_spd = np.zeros([3, 1])
self.target_acc = np.zeros([3, 1])
self.target_rot = np.eye(3, 3)
print(self.target_rot)
self.target_force = np.zeros([3, 1])
self.target_force_max = 5
self.target_roll_vec_preference = np.matrix([1, 0, 0]).T
self.target_roll_vec = np.matrix([1, 0, 0]).T
self.target_pitch_vec = np.matrix([0, 1, 0]).T
self.target_yaw_vec = np.matrix([0, 0, 1]).T
self.pid_ctrl = Pid_controller()
self.set_hover_pid_parameter()
self.transform_R_world_to_ned = np.matrix([[1, 0, 0], [0, -1, 0],
[0, 0, -1]]).T
self.control_amp = 1
# self.Kp = self.control_amp * 20 * np.eye(3, 3)
# self.Kv = self.control_amp * 20 * np.eye(3, 3)
self.Kp = 2.0 * np.array([[1, 0, 0], [0, 1, 0], [0, 0, 3]])
self.Kv = self.control_amp * 10.0 * np.eye(3, 3)
self.Ka = 0.95 * np.eye(3, 3)
self.target_force_max = 2000
self.gravity = np.array([[0], [0], [-9.8]])
self.quad_painter = []
self.t_start = cv2.getTickCount()
self.if_network = if_network
if (self.if_network):
self.if_save_log = 0
if (self.if_rnn_network):
print("Load rnn net")
self.control_network = Rnn_net()
self.hidden_state = None
self.control_network = torch.load(
"../deep_drone/demo_network_final_rnn.pkl").cpu()
else:
# self.control_network = torch.load("../deep_drone/demo_network_final.pkl").cpu()
self.control_network = torch.load(
"../deep_drone/demo_network_50000.pkl").cpu()
self.plan_network = torch.load(
"../deep_drone/rapid_traj_network_7362000.pkl").cpu()
print(self.control_network)
self.nn_input_vec = np.zeros([1, 9])
self.nn_output_vec = np.zeros([1, 4])
self.torch_input_vec = torch.from_numpy(self.nn_input_vec).float()
self.torch_output_vec = torch.from_numpy(
self.nn_output_vec).float()
print(self.control_network)
print("Init finish")
