def __init__(self,
drone_name="drone_1",
viz_traj=True,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0],
viz_image_cv2=True):
self.drone_name = drone_name
self.gate_poses_ground_truth = None
self.viz_image_cv2 = viz_image_cv2
self.viz_traj = viz_traj
self.viz_traj_color_rgba = viz_traj_color_rgba
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
# we need two airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe
# so we poll images in a thread using one airsim MultirotorClient object
# and use another airsim MultirotorClient for querying state commands
self.airsim_client_images = airsim.MultirotorClient()
self.airsim_client_images.confirmConnection()
self.airsim_client_odom = airsim.MultirotorClient()
self.airsim_client_odom.confirmConnection()
self.level_name = None
self.image_callback_thread = threading.Thread(
target=self.repeat_timer_image_callback,
args=(self.image_callback, 0.03))
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, 0.02))
self.is_image_thread_active = False
self.is_odometry_thread_active = False
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10 # see https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/38
def __init__(self,
drone_name="drone_1",
plot_transform=True,
viz_traj=True,
viz_image_cv2=True):
self.drone_name = drone_name
self.gate_poses_ground_truth = None
self.plot_transform = plot_transform
self.viz_image_cv2 = viz_image_cv2
self.viz_traj = viz_traj
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
# we need two airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe
# so we poll images in a thread using one airsim MultirotorClient object
# and use another airsim MultirotorClient for querying state commands
self.airsim_client_images = airsim.MultirotorClient()
self.airsim_client_images.confirmConnection()
self.airsim_client_odom = airsim.MultirotorClient()
self.airsim_client_odom.confirmConnection()
self.level_name = None
self.image_callback_thread = threading.Thread(
target=self.repeat_timer_image_callback,
args=(self.image_callback, 0.03))
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, 0.02))
self.is_image_thread_active = False
self.is_odometry_thread_active = False
def __init__(self,
drone_name="drone_1",
viz_traj=True,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0]):
# Settings.
self.drone_name = drone_name
self.gate_poses = False
self.gates_passed = 0
# Conenction.
self.client = asim.MultirotorClient()
self.client.confirmConnection()
self.odom = asim.MultirotorClient()
self.odom.confirmConnection()
self.ctl_period = 0.05
# Planner related.
self.g = 9.81
self.fmin = 0.2 * self.g
self.fmax = 2.0 * self.g # w.r.t to weight
self.wmax = 6.0
# 50 Hz control, 20 ms callback.
self.next_input = None
self.horizon = 20 # 0.8 second look ahead.
self.mpc_control = None
# Threads.
# Odometry thread calls back current pose and finds optimal contro input.
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, ))
self.is_odometry_thread_active = False
self.control_thread = threading.Thread(target=self.apply_control,
args=(self.ctl_period, ))
self.is_control_thread_active = False
self.is_race_finished = False
# Bug fix.
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10
# Init planner and controller.
self.rapid_planner = planning.RapidPlanner(self.fmin, self.fmax,
self.wmax, self.ctl_period)
self.mpc_control = control.MpcControl(self.horizon)
# Check beyond goal:
# up - - [stack1 2 3]- - tilt down - cube1 2 3 4
# Gate parameters at 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
self.special_checks = []
self.velocity_constraints = []
self.default_look_ahead_s = 0.405
self.default_distance_thresh = 8.0
self.default_velocity = None
self.fake_gate = []
self.passed = -1
# load goals;
self.goal_z_shift = 0.5
def __init__(self, drone_name = "drone_1"):
self.airsim_client = airsimneurips.MultirotorClient()
self.airsim_client_2 = airsimneurips.MultirotorClient()
self.airsim_client_3 = airsimneurips.MultirotorClient()
self.drone_name = drone_name
self.is_thread_active = False
self.thread_reset = threading.Thread(target=self.repeat_timer, args=(self.reset, 0.05))
self.thread_reset_race = threading.Thread(target=self.repeat_timer, args=(self.reset_race, 0.03))
self.thread_reset_and_reset_race = threading.Thread(target=self.repeat_timer, args=(self.reset_and_reset_race, 0.09))
self.is_thread_active = False
def __init__(self,
drone_name="drone_1",
viz_traj=True,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0],
viz_image_cv2=True):
self.drone_name = drone_name
self.gate_poses_ground_truth = None
self.viz_image_cv2 = viz_image_cv2
self.viz_traj = viz_traj
self.viz_traj_color_rgba = viz_traj_color_rgba
self.net = cv2.dnn.readNetFromTensorflow(
"C:\\Users\\User\\PycharmProjects\\AirSim_3.7\\baselines\\net_archs\\ssd_mobilenet_v1\\frozen_inference_graph.pb",
"C:\\Users\\User\\PycharmProjects\\AirSim_3.7\\baselines\\net_archs\\ssd_mobilenet_v1\\graph.pbtxt"
)
self.min_conf = 0.2
self.in_curse = False
self.prev_goal = None
self.time_to_goal = 0
self.yaw_angle = -1.57
self.count_im = 0
self.sum_conf = 0
self.start_time = 0
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
# we need two airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe
# so we poll images in a thread using one airsim MultirotorClient object
# and use another airsim MultirotorClient for querying state commands
self.airsim_client_images = airsim.MultirotorClient()
self.airsim_client_images.confirmConnection()
self.airsim_client_odom = airsim.MultirotorClient()
self.airsim_client_odom.confirmConnection()
self.level_name = None
self.stack = queue.LifoQueue()
self.image_callback_thread = threading.Thread(
target=self.repeat_timer_image_callback,
args=(self.image_callback, 0.05, self.stack))
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, 0.02))
self.is_image_thread_active = False
self.is_odometry_thread_active = False
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10 # see https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/38
def __init__(self, drone_name = "drone_1", plot_transform=True, viz_traj=False):
self.drone_name = drone_name
self.gate_poses_ground_truth = None
self.plot_transform = plot_transform
self.viz_traj = viz_traj
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
self.level_name = None
print(self.viz_traj)
def __init__(self,
traj_params,
drone_names,
drone_i,
drone_params,
use_vel_constraints=False,
viz_traj=True,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0],
plot_gtp=False):
super().__init__(drone_name=drone_names[drone_i],
viz_traj=viz_traj,
viz_traj_color_rgba=viz_traj_color_rgba,
viz_image_cv2=False)
self.drone_names = drone_names
self.drone_i = drone_i
self.drone_params = drone_params
self.traj_params = traj_params
self.use_vel_constraints = use_vel_constraints
self.plot_gtp = plot_gtp
self.controller = None
self.replan_every_sec = 1.0
self.check_log_file_every_sec = 0.1
self.replan_callback_thread = threading.Thread(
target=self.repeat_timer_replan,
args=(self.replan_callback, self.replan_every_sec))
self.log_monitor_thread = threading.Thread(
target=self.repeat_timer_log_monitor,
args=(self.log_monitor_callback, self.check_log_file_every_sec))
self.timer_callback_thread = threading.Thread(
target=self.repeat_timer, args=(self.timer_callback, 0.1))
self.timer_callback_thread.daemon = True
self.is_replan_thread_active = False
self.is_log_monitor_thread_active = False
self.airsim_client_replan_thread = airsim.MultirotorClient()
self.airsim_client_replan_thread.confirmConnection()
# For plotting: Just some fig, ax and line objects to keep track of
if self.plot_gtp:
self.fig, self.ax = plt.subplots()
self.line_state = None
self.lines = [None] * 2
self.racing_log_path = None
self.should_reset_race = False
self.ignore_log_monitor_callback = False
self.ignore_replan_callback = False
self.raceEnded = False
#self.ignore_threads = False
self.ignore_main_thread = False
def main(args):
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl()
client.arm()
traj_tracker_gains = airsim.TrajectoryTrackerGains(
kp_cross_track = 5.0, kd_cross_track = 0.0,
kp_vel_cross_track = 3.0, kd_vel_cross_track = 0.0,
kp_along_track = 0.4, kd_along_track = 0.0,
kp_vel_along_track = 0.04, kd_vel_along_track = 0.0,
kp_z_track = 2.0, kd_z_track = 0.0,
kp_vel_z = 0.4, kd_vel_z = 0.0,
kp_yaw = 3.0, kd_yaw = 0.1
)
client.setTrajectoryTrackerGains(traj_tracker_gains)
client.takeoffAsync().join()
def __init__(self,
drone_name="drone_1",
viz_traj=True,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0],
viz_image_cv2=True):
## gate idx trackers
self.last_gate_passed_idx = -1
self.last_gate_idx_moveOnSpline_was_called_on = -1
self.next_gate_idx = 0
self.next_next_gate_idx = 1
self.drone_name = drone_name
self.gate_poses_ground_truth = None
self.gate_object_names_sorted = None
self.viz_image_cv2 = viz_image_cv2
self.viz_traj = viz_traj
self.viz_traj_color_rgba = viz_traj_color_rgba
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
# we need two airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe
# so we poll images in a thread using one airsim MultirotorClient object
# and use another airsim MultirotorClient for querying state commands
self.airsim_client_images = airsim.MultirotorClient()
self.airsim_client_images.confirmConnection()
self.airsim_client_odom = airsim.MultirotorClient()
self.airsim_client_odom.confirmConnection()
self.level_name = None
self.image_callback_thread = threading.Thread(
target=self.repeat_timer_image_callback,
args=(self.image_callback, 0.03))
self.is_image_thread_active = False
self.got_odom = False
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, 0.5))
self.is_odometry_thread_active = False
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10 # see https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/383
self.finished_race = False
self.terminated_program = False
################### gate detection result variables #################
self.img_mutex = threading.Lock()
self.W = 0
self.H = 0
self.Mx = 0
self.My = 0
self.detect_flag = False
self.previous_detect_flag = False
################# Hyper-parameter Optimization#####################
self.hyper_opt = hyOpt.hyOpt(FINISH_GATE_IDX + 1)
self.hyper_opt.best_hyper.set_v_range((V_MIN, V_MAX))
self.hyper_opt.best_hyper.set_a_range((A_MIN, A_MAX))
self.hyper_opt.best_hyper.set_d_range((D_MIN, D_MAX))
self.hyper_opt.best_hyper.init_hypers(12, 50, 3.5)
self.hyper_opt.best_hyper.init_time()
self.use_new_hyper_for_next_race(self.hyper_opt.best_hyper)
## if the simulation crashes, continue from last iteration by putting best hyperparameters here
self.save_to_file_name = "data_logging_random_search10.txt"
last_iter = 0
path = '/home/usrg/god_ws/hyperopt_ea_game_of_drones/baselines/'
last_iter, best_time, best_v, best_a, best_d = retrieve_best(
path + self.save_to_file_name)
self.hyper_opt.best_hyper.v = np.array(best_v)
self.hyper_opt.best_hyper.a = np.array(best_a)
self.hyper_opt.best_hyper.d = np.array(best_d)
self.hyper_opt.best_hyper.time = np.array(best_time)
self.hyper_opt.curr_hyper.random_init_hypers()
self.iteration = last_iter + 1
# For airsim.exe training binary
import airsimneurips as airsim
import numpy as np
import json
client = airsim.MultirotorClient()
client.confirmConnection()
id2rgb = {}
rgb2id = {}
for id in range(256):
client.simSetSegmentationObjectID("Mesh_soccerField_3", id)
responses = client.simGetImages([
# floating point uncompressed image
airsim.ImageRequest("fpv_cam",
airsim.ImageType.Segmentation,
pixels_as_float=False,
compress=False),
])
seg_response = responses[0]
seg_1d = np.frombuffer(seg_response.image_data_uint8, dtype=np.uint8)
img_seg = seg_1d.reshape(seg_response.height, seg_response.width, 3)
midpt = (int(seg_response.height / 2), int(seg_response.height / 2))
id_rgb = img_seg[int(seg_response.height / 2),
int(seg_response.height / 2), :].astype(int).tolist()
id2rgb[id] = id_rgb
rgb2id[(id_rgb[0], id_rgb[1], id_rgb[2])] = id
def __init__(self,
drone_name="drone_1",
viz_traj=True,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0],
viz_image_cv2=True):
self.drone_name = drone_name
self.gate_poses_ground_truth = None
self.viz_image_cv2 = viz_image_cv2
self.viz_traj = viz_traj
self.viz_traj_color_rgba = viz_traj_color_rgba
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
# we need two airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe
# so we poll images in a thread using one airsim MultirotorClient object
# and use another airsim MultirotorClient for querying state commands
self.airsim_client_images = airsim.MultirotorClient()
self.airsim_client_images.confirmConnection()
self.airsim_client_odom = airsim.MultirotorClient()
self.airsim_client_odom.confirmConnection()
self.level_name = None
self.image_callback_thread = threading.Thread(
target=self.repeat_timer_image_callback,
args=(self.image_callback, 0.02))
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, 1))
self.control_thread = threading.Thread(
target=self.repeat_timer_control_callback,
args=(self.control_callback, 0.05))
self.is_image_thread_active = False
self.is_odometry_thread_active = False
self.is_control_thread_active = False
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10 # see https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/38
self.img_mutex = threading.Lock()
###################gate detection result variables#################
self.W = 0
self.H = 0
self.Mx = 0
self.My = 0
self.Area = 0
self.detect_flag = False
################### isPassing function variable ####################
self.next_gate_idx = 0
self.gate_passed_thresh = 1
self.pass_cnt = 0
self.check_pass = 1
################### using moveToPosition function variable ###########
self.chk_first_flag = True
self.cannot_detect_count = 0
self.using_moveToPosition_threshold = 10
self.gate_back_dis = 5
self.lam = 0.5 # 0 ~ 1
self.lam_z = 0.99
self.vision_lam = 0.2 # low value is big ocsilation
self.pass_position_vec = None
self.estimate_depth = 8
self.position_control_on = False
self.prev_time = 0
self.curr_time = 0
self.distance_y_prev = 0
self.distance_y_curr = 0
self.distance_z_prev = 0
self.distance_z_curr = 0
self.desired_yaw_prev = 0
self.desired_yaw_curr = 0
self.prev_vel = 0
self.prev_vel_mag = 99
############################## TaeYeon method ###############################
self.duration_move_ahead = 0.5
############################ Save img ########################
self.count = 0
self.day = datetime.datetime.today().day
self.minute = datetime.datetime.today().minute
self.hour = datetime.datetime.today().hour
#######################
self.go_ahead_flag = False
def __init__(self,
drone_name="drone_1",
viz_traj=False,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0],
viz_image_cv2=False):
# Information of drone
self.oldtime = time.time()
self.old_control_time = time.time()
self.drone_name = drone_name
self.drone_position = None
self.drone_orientation = None
self.drone_linear_velocity = None
self.drone_angular_velocity = None
self.drone_linear_acceleration = None
self.drone_angular_acceleration = None
self.drone_roll = None
self.drone_pitch = None
self.drone_yaw = None
self.real_drone_roll = None
self.real_drone_pitch = None
self.real_drone_yaw = None
self.x_pos = 0
self.y_pos = 0
self.z_pos = 0
self.old_x_pos = 0
self.old_y_pos = 0
self.old_z_pos = 0
self.x_vel = 0
self.y_vel = 0
self.z_vel = 0
self.old_x_vel = 0
self.old_y_vel = 0
self.old_z_vel = 0
self.x_acc = 0
self.y_acc = 0
self.z_acc = 0
self.stop_motion_c = 0
self.design_vel = 4
# Information of gate
self.gate_poses_ground_truth = None
self.gate_num = None
self.gate_counter = 0
self.old_gate_counter = 0
# Information of img_g
self.Img_g = None
self.Img_rgb = None
# Information of error
self.x_error = 0
self.y_error = 0
self.z_error = 0
self.error = 0
self.yaw_error = 0
self.old_error = 0
self.old_yaw_error = 0
# Setting of airsim View
self.viz_traj = viz_traj
self.viz_traj_color_rgba = viz_traj_color_rgba
self.viz_image_cv2 = viz_image_cv2
# Setting of Gym Like Parameter
self.observation_space = gym.spaces.Box(-30, 30, (8, ))
self.action_space = gym.spaces.Box(-1, 1, (4, ))
self._max_episode_steps = 99999
self.ep_time_step = 0
self.done = False
self.MAX_ep_step = 300
self.max_acc = 150
self.control_mode = "vel_rpyt"
self.reward_parameter = {
"pass_Gate": 150,
"error_punish": -2.0,
"yaw_error_punish": -0.5,
"vel_punish": -0.01,
"action_punish": -0.0,
"step_punish": -100,
"no_motion_punish": -100
}
# we need three airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe
# Drone in a thread using one airsim MultirotorClient object
# Images in a thread using one airsim MultirotorClient object
# Odometry in a thread using one airsim MultirotorClient object (querying state commands)
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
self.airsim_client_images = airsim.MultirotorClient()
self.airsim_client_images.confirmConnection()
self.airsim_client_odom = airsim.MultirotorClient()
self.airsim_client_odom.confirmConnection()
self.airsim_client_pass = airsim.MultirotorClient()
self.airsim_client_pass.confirmConnection()
self.level_name = None
# Set the "image, odometry, pass" thread
self.image_callback_thread = threading.Thread(
target=self.repeat_timer_image_callback,
args=(self.image_callback, 0.03))
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, 0.02))
self.pass_callback_thread = threading.Thread(
target=self.repeat_timer_pass_callback,
args=(self.pass_callback, 0.03))
self.is_image_thread_active = False
self.is_odometry_thread_active = False
self.is_pass_thread_active = False
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10 # see https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/38
def setupASClient():
client = airsim.MultirotorClient()
client.confirmConnection()
return client
def __init__(self,
drone_name="drone_1",
viz_traj=True,
viz_traj_color_rgba=[1.0, 0.0, 0.0, 1.0],
viz_image_cv2=True):
## gate idx trackers
self.last_gate_passed_idx = -1
self.last_gate_idx_moveOnSpline_was_called_on = -1
self.next_gate_idx = 0
self.next_next_gate_idx = 1
self.drone_name = drone_name
self.gate_poses_ground_truth = None
self.gate_object_names_sorted = None
self.viz_image_cv2 = viz_image_cv2
self.viz_traj = viz_traj
self.viz_traj_color_rgba = viz_traj_color_rgba
self.airsim_client = airsim.MultirotorClient()
self.airsim_client.confirmConnection()
# we need two airsim MultirotorClient objects because the comm lib we use (rpclib) is not thread safe
# so we poll images in a thread using one airsim MultirotorClient object
# and use another airsim MultirotorClient for querying state commands
self.airsim_client_images = airsim.MultirotorClient()
self.airsim_client_images.confirmConnection()
self.airsim_client_odom = airsim.MultirotorClient()
self.airsim_client_odom.confirmConnection()
self.level_name = None
self.image_callback_thread = threading.Thread(
target=self.repeat_timer_image_callback,
args=(self.image_callback, 0.03))
self.is_image_thread_active = False
self.got_odom = False
self.odometry_callback_thread = threading.Thread(
target=self.repeat_timer_odometry_callback,
args=(self.odometry_callback, 0.5))
self.is_odometry_thread_active = False
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = 10 # see https://github.com/microsoft/AirSim-NeurIPS2019-Drone-Racing/issues/383
self.finished_race = False
self.terminated_program = False
################### gate detection result variables #################
self.img_mutex = threading.Lock()
self.W = 0
self.H = 0
self.Mx = 0
self.My = 0
self.detect_flag = False
self.previous_detect_flag = False
################# Hyper-parameter Optimization#####################
self.hyper_opt = hyOpt.hyOpt(FINISH_GATE_IDX + 1)
self.hyper_opt.best_hyper.set_v_range((V_MIN, V_MAX))
self.hyper_opt.best_hyper.set_a_range((A_MIN, A_MAX))
self.hyper_opt.best_hyper.set_d_range((D_MIN, D_MAX))
self.hyper_opt.best_hyper.init_hypers(12, 50, 3.5)
self.hyper_opt.best_hyper.init_time()
self.use_new_hyper_for_next_race(self.hyper_opt.best_hyper)
# if the simulation crashes, continue from last iteration by putting best hyperparameters here
self.hyper_opt.best_hyper.v = np.array([
23.36, 11.3, 32.19, 22.46, 13.37, 25.65, 12.0, 27.25, 25.33, 12.0,
12.0, 12.0, 21.09, 30.56
])
self.hyper_opt.best_hyper.a = np.array([
70.71, 117.34, 146.34, 91.64, 43.16, 99.92, 50.0, 50.0, 50.0, 50.0,
50.0, 50.0, 50.0, 50.0
])
self.hyper_opt.best_hyper.d = np.array([
3.5, 3.5, 6.14, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5,
2.0
])
self.hyper_opt.best_hyper.time = np.array([
6.08, 8.01, 10.5, 12.76, 15.22, 18.07, 33.86, 37.66, 39.83, 42.57,
44.57, 47.46, 49.97, 52.58
])
self.logging_file_name = "po5_2.txt"
self.iteration = 1
