[formatting] format with python black
This commit is contained in:
madratman
Родитель
56630e756e
Коммит
ecb0993c7d
|
|
@ -0,0 +1,4 @@
|
|||
{
|
||||
"editor.formatOnSave": true,
|
||||
"python.formatting.provider": "black"
|
||||
}
|
||||
|
|
@ -9,7 +9,13 @@ import math
|
|||
|
||||
# drone_name should match the name in ~/Document/AirSim/settings.json
|
||||
class BaselineRacer(object):
|
||||
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):
|
||||
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
|
||||
|
|
@ -20,26 +26,33 @@ class BaselineRacer(object):
|
|||
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
|
||||
# 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.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-Drone-Racing-Lab/issues/38
|
||||
self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS = (
|
||||
10 # see https://github.com/microsoft/AirSim-Drone-Racing-Lab/issues/38
|
||||
)
|
||||
|
||||
# loads desired level
|
||||
def load_level(self, level_name, sleep_sec = 2.0):
|
||||
def load_level(self, level_name, sleep_sec=2.0):
|
||||
self.level_name = level_name
|
||||
self.airsim_client.simLoadLevel(self.level_name)
|
||||
self.airsim_client.confirmConnection() # failsafe
|
||||
time.sleep(sleep_sec) # let the environment load completely
|
||||
self.airsim_client.confirmConnection() # failsafe
|
||||
time.sleep(sleep_sec) # let the environment load completely
|
||||
|
||||
# Starts an instance of a race in your given level, if valid
|
||||
def start_race(self, tier=3):
|
||||
|
|
@ -54,28 +67,54 @@ class BaselineRacer(object):
|
|||
self.airsim_client.enableApiControl(vehicle_name=self.drone_name)
|
||||
self.airsim_client.arm(vehicle_name=self.drone_name)
|
||||
|
||||
# set default values for trajectory tracker gains
|
||||
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)
|
||||
# set default values for trajectory tracker gains
|
||||
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,
|
||||
)
|
||||
|
||||
self.airsim_client.setTrajectoryTrackerGains(traj_tracker_gains, vehicle_name=self.drone_name)
|
||||
self.airsim_client.setTrajectoryTrackerGains(
|
||||
traj_tracker_gains, vehicle_name=self.drone_name
|
||||
)
|
||||
time.sleep(0.2)
|
||||
|
||||
def takeoffAsync(self):
|
||||
self.airsim_client.takeoffAsync().join()
|
||||
|
||||
# like takeoffAsync(), but with moveOnSpline()
|
||||
def takeoff_with_moveOnSpline(self, takeoff_height = 1.0):
|
||||
start_position = self.airsim_client.simGetVehiclePose(vehicle_name=self.drone_name).position
|
||||
takeoff_waypoint = airsim.Vector3r(start_position.x_val, start_position.y_val, start_position.z_val-takeoff_height)
|
||||
def takeoff_with_moveOnSpline(self, takeoff_height=1.0):
|
||||
start_position = self.airsim_client.simGetVehiclePose(
|
||||
vehicle_name=self.drone_name
|
||||
).position
|
||||
takeoff_waypoint = airsim.Vector3r(
|
||||
start_position.x_val,
|
||||
start_position.y_val,
|
||||
start_position.z_val - takeoff_height,
|
||||
)
|
||||
|
||||
self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_position_constraint=True, add_velocity_constraint=False,
|
||||
add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name).join()
|
||||
self.airsim_client.moveOnSplineAsync(
|
||||
[takeoff_waypoint],
|
||||
vel_max=15.0,
|
||||
acc_max=5.0,
|
||||
add_position_constraint=True,
|
||||
add_velocity_constraint=False,
|
||||
add_acceleration_constraint=False,
|
||||
viz_traj=self.viz_traj,
|
||||
viz_traj_color_rgba=self.viz_traj_color_rgba,
|
||||
vehicle_name=self.drone_name,
|
||||
).join()
|
||||
|
||||
# stores gate ground truth poses as a list of airsim.Pose() objects in self.gate_poses_ground_truth
|
||||
def get_ground_truth_gate_poses(self):
|
||||
|
|
@ -83,54 +122,110 @@ class BaselineRacer(object):
|
|||
# gate_names_sorted_bad is of the form `GateN_GARBAGE`. for example:
|
||||
# ['Gate0', 'Gate10_21', 'Gate11_23', 'Gate1_3', 'Gate2_5', 'Gate3_7', 'Gate4_9', 'Gate5_11', 'Gate6_13', 'Gate7_15', 'Gate8_17', 'Gate9_19']
|
||||
# we sort them by their ibdex of occurence along the race track(N), and ignore the unreal garbage number after the underscore(GARBAGE)
|
||||
gate_indices_bad = [int(gate_name.split('_')[0][4:]) for gate_name in gate_names_sorted_bad]
|
||||
gate_indices_correct = sorted(range(len(gate_indices_bad)), key=lambda k: gate_indices_bad[k])
|
||||
gate_names_sorted = [gate_names_sorted_bad[gate_idx] for gate_idx in gate_indices_correct]
|
||||
gate_indices_bad = [
|
||||
int(gate_name.split("_")[0][4:]) for gate_name in gate_names_sorted_bad
|
||||
]
|
||||
gate_indices_correct = sorted(
|
||||
range(len(gate_indices_bad)), key=lambda k: gate_indices_bad[k]
|
||||
)
|
||||
gate_names_sorted = [
|
||||
gate_names_sorted_bad[gate_idx] for gate_idx in gate_indices_correct
|
||||
]
|
||||
self.gate_poses_ground_truth = []
|
||||
for gate_name in gate_names_sorted:
|
||||
curr_pose = self.airsim_client.simGetObjectPose(gate_name)
|
||||
counter = 0
|
||||
while (math.isnan(curr_pose.position.x_val) or math.isnan(curr_pose.position.y_val) or math.isnan(curr_pose.position.z_val)) and (counter < self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS):
|
||||
while (
|
||||
math.isnan(curr_pose.position.x_val)
|
||||
or math.isnan(curr_pose.position.y_val)
|
||||
or math.isnan(curr_pose.position.z_val)
|
||||
) and (counter < self.MAX_NUMBER_OF_GETOBJECTPOSE_TRIALS):
|
||||
print(f"DEBUG: {gate_name} position is nan, retrying...")
|
||||
counter += 1
|
||||
curr_pose = self.airsim_client.simGetObjectPose(gate_name)
|
||||
assert not math.isnan(curr_pose.position.x_val), f"ERROR: {gate_name} curr_pose.position.x_val is still {curr_pose.position.x_val} after {counter} trials"
|
||||
assert not math.isnan(curr_pose.position.y_val), f"ERROR: {gate_name} curr_pose.position.y_val is still {curr_pose.position.y_val} after {counter} trials"
|
||||
assert not math.isnan(curr_pose.position.z_val), f"ERROR: {gate_name} curr_pose.position.z_val is still {curr_pose.position.z_val} after {counter} trials"
|
||||
assert not math.isnan(
|
||||
curr_pose.position.x_val
|
||||
), f"ERROR: {gate_name} curr_pose.position.x_val is still {curr_pose.position.x_val} after {counter} trials"
|
||||
assert not math.isnan(
|
||||
curr_pose.position.y_val
|
||||
), f"ERROR: {gate_name} curr_pose.position.y_val is still {curr_pose.position.y_val} after {counter} trials"
|
||||
assert not math.isnan(
|
||||
curr_pose.position.z_val
|
||||
), f"ERROR: {gate_name} curr_pose.position.z_val is still {curr_pose.position.z_val} after {counter} trials"
|
||||
self.gate_poses_ground_truth.append(curr_pose)
|
||||
|
||||
# this is utility function to get a velocity constraint which can be passed to moveOnSplineVelConstraints()
|
||||
# this is utility function to get a velocity constraint which can be passed to moveOnSplineVelConstraints()
|
||||
# the "scale" parameter scales the gate facing vector accordingly, thereby dictating the speed of the velocity constraint
|
||||
def get_gate_facing_vector_from_quaternion(self, airsim_quat, scale = 1.0):
|
||||
def get_gate_facing_vector_from_quaternion(self, airsim_quat, scale=1.0):
|
||||
import numpy as np
|
||||
# convert gate quaternion to rotation matrix.
|
||||
|
||||
# convert gate quaternion to rotation matrix.
|
||||
# ref: https://en.wikipedia.org/wiki/Rotation_matrix#Quaternion; https://www.lfd.uci.edu/~gohlke/code/transformations.py.html
|
||||
q = np.array([airsim_quat.w_val, airsim_quat.x_val, airsim_quat.y_val, airsim_quat.z_val], dtype=np.float64)
|
||||
q = np.array(
|
||||
[
|
||||
airsim_quat.w_val,
|
||||
airsim_quat.x_val,
|
||||
airsim_quat.y_val,
|
||||
airsim_quat.z_val,
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
n = np.dot(q, q)
|
||||
if n < np.finfo(float).eps:
|
||||
return airsim.Vector3r(0.0, 1.0, 0.0)
|
||||
q *= np.sqrt(2.0 / n)
|
||||
q = np.outer(q, q)
|
||||
rotation_matrix = np.array([[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0]],
|
||||
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0]],
|
||||
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2]]])
|
||||
gate_facing_vector = rotation_matrix[:,1]
|
||||
return airsim.Vector3r(scale * gate_facing_vector[0], scale * gate_facing_vector[1], scale * gate_facing_vector[2])
|
||||
rotation_matrix = np.array(
|
||||
[
|
||||
[1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0]],
|
||||
[q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0]],
|
||||
[q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2]],
|
||||
]
|
||||
)
|
||||
gate_facing_vector = rotation_matrix[:, 1]
|
||||
return airsim.Vector3r(
|
||||
scale * gate_facing_vector[0],
|
||||
scale * gate_facing_vector[1],
|
||||
scale * gate_facing_vector[2],
|
||||
)
|
||||
|
||||
def fly_through_all_gates_one_by_one_with_moveOnSpline(self):
|
||||
if self.level_name == "Building99_Hard":
|
||||
vel_max = 5.0
|
||||
acc_max = 2.0
|
||||
|
||||
if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy", "ZhangJiaJie_Medium"] :
|
||||
if self.level_name in [
|
||||
"Soccer_Field_Medium",
|
||||
"Soccer_Field_Easy",
|
||||
"ZhangJiaJie_Medium",
|
||||
]:
|
||||
vel_max = 10.0
|
||||
acc_max = 5.0
|
||||
|
||||
return self.airsim_client.moveOnSplineAsync([gate_pose.position], vel_max=vel_max, acc_max=acc_max,
|
||||
add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name)
|
||||
return self.airsim_client.moveOnSplineAsync(
|
||||
[gate_pose.position],
|
||||
vel_max=vel_max,
|
||||
acc_max=acc_max,
|
||||
add_position_constraint=True,
|
||||
add_velocity_constraint=False,
|
||||
add_acceleration_constraint=False,
|
||||
viz_traj=self.viz_traj,
|
||||
viz_traj_color_rgba=self.viz_traj_color_rgba,
|
||||
vehicle_name=self.drone_name,
|
||||
)
|
||||
|
||||
def fly_through_all_gates_at_once_with_moveOnSpline(self):
|
||||
if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy", "ZhangJiaJie_Medium", "Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3", "Final_Tier_1", "Final_Tier_2", "Final_Tier_3"] :
|
||||
if self.level_name in [
|
||||
"Soccer_Field_Medium",
|
||||
"Soccer_Field_Easy",
|
||||
"ZhangJiaJie_Medium",
|
||||
"Qualifier_Tier_1",
|
||||
"Qualifier_Tier_2",
|
||||
"Qualifier_Tier_3",
|
||||
"Final_Tier_1",
|
||||
"Final_Tier_2",
|
||||
"Final_Tier_3",
|
||||
]:
|
||||
vel_max = 30.0
|
||||
acc_max = 15.0
|
||||
|
||||
|
|
@ -138,14 +233,23 @@ class BaselineRacer(object):
|
|||
vel_max = 4.0
|
||||
acc_max = 1.0
|
||||
|
||||
return self.airsim_client.moveOnSplineAsync([gate_pose.position for gate_pose in self.gate_poses_ground_truth], vel_max=vel_max, acc_max=acc_max,
|
||||
add_position_constraint=True, add_velocity_constraint=False, add_acceleration_constraint=False, viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name)
|
||||
return self.airsim_client.moveOnSplineAsync(
|
||||
[gate_pose.position for gate_pose in self.gate_poses_ground_truth],
|
||||
vel_max=vel_max,
|
||||
acc_max=acc_max,
|
||||
add_position_constraint=True,
|
||||
add_velocity_constraint=False,
|
||||
add_acceleration_constraint=False,
|
||||
viz_traj=self.viz_traj,
|
||||
viz_traj_color_rgba=self.viz_traj_color_rgba,
|
||||
vehicle_name=self.drone_name,
|
||||
)
|
||||
|
||||
def fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints(self):
|
||||
add_velocity_constraint = True
|
||||
add_acceleration_constraint = False
|
||||
|
||||
if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy"] :
|
||||
if self.level_name in ["Soccer_Field_Medium", "Soccer_Field_Easy"]:
|
||||
vel_max = 15.0
|
||||
acc_max = 3.0
|
||||
speed_through_gate = 2.5
|
||||
|
|
@ -161,15 +265,30 @@ class BaselineRacer(object):
|
|||
speed_through_gate = 0.5
|
||||
add_velocity_constraint = False
|
||||
|
||||
# scale param scales the gate facing vector by desired speed.
|
||||
return self.airsim_client.moveOnSplineVelConstraintsAsync([gate_pose.position],
|
||||
[self.get_gate_facing_vector_from_quaternion(gate_pose.orientation, scale = speed_through_gate)],
|
||||
vel_max=vel_max, acc_max=acc_max,
|
||||
add_position_constraint=True, add_velocity_constraint=add_velocity_constraint, add_acceleration_constraint=add_acceleration_constraint,
|
||||
viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name)
|
||||
# scale param scales the gate facing vector by desired speed.
|
||||
return self.airsim_client.moveOnSplineVelConstraintsAsync(
|
||||
[gate_pose.position],
|
||||
[
|
||||
self.get_gate_facing_vector_from_quaternion(
|
||||
gate_pose.orientation, scale=speed_through_gate
|
||||
)
|
||||
],
|
||||
vel_max=vel_max,
|
||||
acc_max=acc_max,
|
||||
add_position_constraint=True,
|
||||
add_velocity_constraint=add_velocity_constraint,
|
||||
add_acceleration_constraint=add_acceleration_constraint,
|
||||
viz_traj=self.viz_traj,
|
||||
viz_traj_color_rgba=self.viz_traj_color_rgba,
|
||||
vehicle_name=self.drone_name,
|
||||
)
|
||||
|
||||
def fly_through_all_gates_at_once_with_moveOnSplineVelConstraints(self):
|
||||
if self.level_name in ["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium"]:
|
||||
if self.level_name in [
|
||||
"Soccer_Field_Easy",
|
||||
"Soccer_Field_Medium",
|
||||
"ZhangJiaJie_Medium",
|
||||
]:
|
||||
vel_max = 15.0
|
||||
acc_max = 7.5
|
||||
speed_through_gate = 2.5
|
||||
|
|
@ -179,17 +298,29 @@ class BaselineRacer(object):
|
|||
acc_max = 2.0
|
||||
speed_through_gate = 1.0
|
||||
|
||||
return self.airsim_client.moveOnSplineVelConstraintsAsync([gate_pose.position for gate_pose in self.gate_poses_ground_truth],
|
||||
[self.get_gate_facing_vector_from_quaternion(gate_pose.orientation, scale = speed_through_gate) for gate_pose in self.gate_poses_ground_truth],
|
||||
vel_max=vel_max, acc_max=acc_max,
|
||||
add_position_constraint=True, add_velocity_constraint=True, add_acceleration_constraint=False,
|
||||
viz_traj=self.viz_traj, viz_traj_color_rgba=self.viz_traj_color_rgba, vehicle_name=self.drone_name)
|
||||
return self.airsim_client.moveOnSplineVelConstraintsAsync(
|
||||
[gate_pose.position for gate_pose in self.gate_poses_ground_truth],
|
||||
[
|
||||
self.get_gate_facing_vector_from_quaternion(
|
||||
gate_pose.orientation, scale=speed_through_gate
|
||||
)
|
||||
for gate_pose in self.gate_poses_ground_truth
|
||||
],
|
||||
vel_max=vel_max,
|
||||
acc_max=acc_max,
|
||||
add_position_constraint=True,
|
||||
add_velocity_constraint=True,
|
||||
add_acceleration_constraint=False,
|
||||
viz_traj=self.viz_traj,
|
||||
viz_traj_color_rgba=self.viz_traj_color_rgba,
|
||||
vehicle_name=self.drone_name,
|
||||
)
|
||||
|
||||
def image_callback(self):
|
||||
# get uncompressed fpv cam image
|
||||
request = [airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False)]
|
||||
response = self.airsim_client_images.simGetImages(request)
|
||||
img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8)
|
||||
img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8)
|
||||
img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3)
|
||||
if self.viz_image_cv2:
|
||||
cv2.imshow("img_rgb", img_rgb)
|
||||
|
|
@ -199,12 +330,12 @@ class BaselineRacer(object):
|
|||
# get uncompressed fpv cam image
|
||||
drone_state = self.airsim_client_odom.getMultirotorState()
|
||||
# in world frame:
|
||||
position = drone_state.kinematics_estimated.position
|
||||
position = drone_state.kinematics_estimated.position
|
||||
orientation = drone_state.kinematics_estimated.orientation
|
||||
linear_velocity = drone_state.kinematics_estimated.linear_velocity
|
||||
angular_velocity = drone_state.kinematics_estimated.angular_velocity
|
||||
|
||||
# call task() method every "period" seconds.
|
||||
# call task() method every "period" seconds.
|
||||
def repeat_timer_image_callback(self, task, period):
|
||||
while self.is_image_thread_active:
|
||||
task()
|
||||
|
|
@ -239,9 +370,15 @@ class BaselineRacer(object):
|
|||
self.odometry_callback_thread.join()
|
||||
print("Stopped odometry callback thread.")
|
||||
|
||||
|
||||
def main(args):
|
||||
# ensure you have generated the neurips planning settings file by running python generate_settings_file.py
|
||||
baseline_racer = BaselineRacer(drone_name="drone_1", viz_traj=args.viz_traj, viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0], viz_image_cv2=args.viz_image_cv2)
|
||||
baseline_racer = BaselineRacer(
|
||||
drone_name="drone_1",
|
||||
viz_traj=args.viz_traj,
|
||||
viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0],
|
||||
viz_image_cv2=args.viz_image_cv2,
|
||||
)
|
||||
baseline_racer.load_level(args.level_name)
|
||||
if args.level_name == "Qualifier_Tier_1":
|
||||
args.race_tier = 1
|
||||
|
|
@ -256,7 +393,7 @@ def main(args):
|
|||
baseline_racer.start_image_callback_thread()
|
||||
baseline_racer.start_odometry_callback_thread()
|
||||
|
||||
if args.planning_baseline_type == "all_gates_at_once" :
|
||||
if args.planning_baseline_type == "all_gates_at_once":
|
||||
if args.planning_and_control_api == "moveOnSpline":
|
||||
baseline_racer.fly_through_all_gates_at_once_with_moveOnSpline().join()
|
||||
if args.planning_and_control_api == "moveOnSplineVelConstraints":
|
||||
|
|
@ -268,19 +405,52 @@ def main(args):
|
|||
if args.planning_and_control_api == "moveOnSplineVelConstraints":
|
||||
baseline_racer.fly_through_all_gates_one_by_one_with_moveOnSplineVelConstraints().join()
|
||||
|
||||
# Comment out the following if you observe the python script exiting prematurely, and resetting the race
|
||||
# Comment out the following if you observe the python script exiting prematurely, and resetting the race
|
||||
baseline_racer.stop_image_callback_thread()
|
||||
baseline_racer.stop_odometry_callback_thread()
|
||||
baseline_racer.reset_race()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = ArgumentParser()
|
||||
parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium", "Building99_Hard",
|
||||
"Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3", "Final_Tier_1", "Final_Tier_2", "Final_Tier_3"], default="ZhangJiaJie_Medium")
|
||||
parser.add_argument('--planning_baseline_type', type=str, choices=["all_gates_at_once","all_gates_one_by_one"], default="all_gates_at_once")
|
||||
parser.add_argument('--planning_and_control_api', type=str, choices=["moveOnSpline", "moveOnSplineVelConstraints"], default="moveOnSpline")
|
||||
parser.add_argument('--enable_viz_traj', dest='viz_traj', action='store_true', default=False)
|
||||
parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False)
|
||||
parser.add_argument('--race_tier', type=int, choices=[1,2,3], default=1)
|
||||
parser.add_argument(
|
||||
"--level_name",
|
||||
type=str,
|
||||
choices=[
|
||||
"Soccer_Field_Easy",
|
||||
"Soccer_Field_Medium",
|
||||
"ZhangJiaJie_Medium",
|
||||
"Building99_Hard",
|
||||
"Qualifier_Tier_1",
|
||||
"Qualifier_Tier_2",
|
||||
"Qualifier_Tier_3",
|
||||
"Final_Tier_1",
|
||||
"Final_Tier_2",
|
||||
"Final_Tier_3",
|
||||
],
|
||||
default="ZhangJiaJie_Medium",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--planning_baseline_type",
|
||||
type=str,
|
||||
choices=["all_gates_at_once", "all_gates_one_by_one"],
|
||||
default="all_gates_at_once",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--planning_and_control_api",
|
||||
type=str,
|
||||
choices=["moveOnSpline", "moveOnSplineVelConstraints"],
|
||||
default="moveOnSpline",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--enable_viz_traj", dest="viz_traj", action="store_true", default=False
|
||||
)
|
||||
parser.add_argument(
|
||||
"--enable_viz_image_cv2",
|
||||
dest="viz_image_cv2",
|
||||
action="store_true",
|
||||
default=False,
|
||||
)
|
||||
parser.add_argument("--race_tier", type=int, choices=[1, 2, 3], default=1)
|
||||
args = parser.parse_args()
|
||||
main(args)
|
||||
|
|
|
|||
|
|
@ -7,22 +7,33 @@ import numpy as np
|
|||
import cv2
|
||||
from baseline_racer import BaselineRacer
|
||||
|
||||
|
||||
class BaselineRacerImageBenchmarker(BaselineRacer):
|
||||
def __init__(self,
|
||||
img_benchmark_type = 'simGetImage',
|
||||
drone_name = "drone_1",
|
||||
viz_traj = False,
|
||||
viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0],
|
||||
viz_image_cv2 = False):
|
||||
super().__init__(drone_name=drone_name, viz_traj=viz_traj, viz_image_cv2=viz_image_cv2)
|
||||
def __init__(
|
||||
self,
|
||||
img_benchmark_type="simGetImage",
|
||||
drone_name="drone_1",
|
||||
viz_traj=False,
|
||||
viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0],
|
||||
viz_image_cv2=False,
|
||||
):
|
||||
super().__init__(
|
||||
drone_name=drone_name, viz_traj=viz_traj, viz_image_cv2=viz_image_cv2
|
||||
)
|
||||
|
||||
self.image_benchmark_num_images = 0
|
||||
self.image_benchmark_total_time = 0.0
|
||||
self.image_callback_thread = None
|
||||
if img_benchmark_type == "simGetImage":
|
||||
self.image_callback_thread = threading.Thread(target=self.repeat_timer_img, args=(self.image_callback_benchmark_simGetImage, 0.05))
|
||||
self.image_callback_thread = threading.Thread(
|
||||
target=self.repeat_timer_img,
|
||||
args=(self.image_callback_benchmark_simGetImage, 0.05),
|
||||
)
|
||||
if img_benchmark_type == "simGetImages":
|
||||
self.image_callback_thread = threading.Thread(target=self.repeat_timer_img, args=(self.image_callback_benchmark_simGetImages, 0.05))
|
||||
self.image_callback_thread = threading.Thread(
|
||||
target=self.repeat_timer_img,
|
||||
args=(self.image_callback_benchmark_simGetImages, 0.05),
|
||||
)
|
||||
self.is_image_thread_active = False
|
||||
|
||||
def start_img_benchmark_thread(self):
|
||||
|
|
@ -43,45 +54,73 @@ class BaselineRacerImageBenchmarker(BaselineRacer):
|
|||
time.sleep(period)
|
||||
|
||||
def print_benchmark_results(self):
|
||||
avg_fps = 1.0 / ((self.image_benchmark_total_time) / float(self.image_benchmark_num_images))
|
||||
print(self.level_name + ": {} avg_fps for {} num of images".format(avg_fps, self.image_benchmark_num_images))
|
||||
avg_fps = 1.0 / (
|
||||
(self.image_benchmark_total_time) / float(self.image_benchmark_num_images)
|
||||
)
|
||||
print(
|
||||
self.level_name
|
||||
+ ": {} avg_fps for {} num of images".format(
|
||||
avg_fps, self.image_benchmark_num_images
|
||||
)
|
||||
)
|
||||
|
||||
def image_callback_benchmark_simGetImage(self):
|
||||
self.image_benchmark_num_images += 1
|
||||
iter_start_time = time.time()
|
||||
response = self.airsim_client_images.simGetImage("fpv_cam", airsim.ImageType.Scene)
|
||||
img_rgb = cv2.imdecode(airsim.string_to_uint8_array(response), cv2.IMREAD_UNCHANGED)
|
||||
response = self.airsim_client_images.simGetImage(
|
||||
"fpv_cam", airsim.ImageType.Scene
|
||||
)
|
||||
img_rgb = cv2.imdecode(
|
||||
airsim.string_to_uint8_array(response), cv2.IMREAD_UNCHANGED
|
||||
)
|
||||
self.image_benchmark_total_time += time.time() - iter_start_time
|
||||
avg_fps = 1.0 / ((self.image_benchmark_total_time) / float(self.image_benchmark_num_images))
|
||||
print(self.level_name + ": {} avg_fps for {} num of images".format(avg_fps, self.image_benchmark_num_images))
|
||||
avg_fps = 1.0 / (
|
||||
(self.image_benchmark_total_time) / float(self.image_benchmark_num_images)
|
||||
)
|
||||
print(
|
||||
self.level_name
|
||||
+ ": {} avg_fps for {} num of images".format(
|
||||
avg_fps, self.image_benchmark_num_images
|
||||
)
|
||||
)
|
||||
# uncomment following lines to viz image
|
||||
# if self.viz_image_cv2:
|
||||
# cv2.imshow("img_rgb", img_rgb_1d_new)
|
||||
# cv2.waitKey(1)
|
||||
# cv2.imshow("img_rgb", img_rgb_1d_new)
|
||||
# cv2.waitKey(1)
|
||||
|
||||
def image_callback_benchmark_simGetImages(self):
|
||||
self.image_benchmark_num_images += 1
|
||||
iter_start_time = time.time()
|
||||
request = [airsim.ImageRequest("fpv_cam", airsim.ImageType.Scene, False, False)]
|
||||
response = self.airsim_client_images.simGetImages(request)
|
||||
img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8)
|
||||
img_rgb_1d = np.fromstring(response[0].image_data_uint8, dtype=np.uint8)
|
||||
img_rgb = img_rgb_1d.reshape(response[0].height, response[0].width, 3)
|
||||
self.image_benchmark_total_time += time.time() - iter_start_time
|
||||
avg_fps = 1.0 / ((self.image_benchmark_total_time) / float(self.image_benchmark_num_images))
|
||||
print(self.level_name + ": {} avg_fps for {} num of images".format(avg_fps, self.image_benchmark_num_images))
|
||||
avg_fps = 1.0 / (
|
||||
(self.image_benchmark_total_time) / float(self.image_benchmark_num_images)
|
||||
)
|
||||
print(
|
||||
self.level_name
|
||||
+ ": {} avg_fps for {} num of images".format(
|
||||
avg_fps, self.image_benchmark_num_images
|
||||
)
|
||||
)
|
||||
# uncomment following lines to viz image
|
||||
# if self.viz_image_cv2:
|
||||
# cv2.imshow("img_rgb", img_rgb_1d_new)
|
||||
# cv2.waitKey(1)
|
||||
# cv2.imshow("img_rgb", img_rgb_1d_new)
|
||||
# cv2.waitKey(1)
|
||||
|
||||
|
||||
def main(args):
|
||||
# ensure you have generated the neurips planning settings file by running python generate_settings_file.py
|
||||
baseline_racer = BaselineRacerImageBenchmarker(img_benchmark_type=args.img_benchmark_type, \
|
||||
drone_name="drone_1", \
|
||||
viz_traj=args.viz_traj, \
|
||||
viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0], \
|
||||
viz_image_cv2=args.viz_image_cv2)
|
||||
|
||||
baseline_racer = BaselineRacerImageBenchmarker(
|
||||
img_benchmark_type=args.img_benchmark_type,
|
||||
drone_name="drone_1",
|
||||
viz_traj=args.viz_traj,
|
||||
viz_traj_color_rgba=[1.0, 1.0, 0.0, 1.0],
|
||||
viz_image_cv2=args.viz_image_cv2,
|
||||
)
|
||||
|
||||
baseline_racer.load_level(args.level_name)
|
||||
if args.level_name == "Qualifier_Tier_1":
|
||||
args.race_tier = 1
|
||||
|
|
@ -99,14 +138,39 @@ def main(args):
|
|||
baseline_racer.stop_img_benchmark_thread()
|
||||
baseline_racer.print_benchmark_results()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = ArgumentParser()
|
||||
parser.add_argument('--level_name', type=str, choices=["Soccer_Field_Easy", "Soccer_Field_Medium", "ZhangJiaJie_Medium", "Building99_Hard",
|
||||
"Qualifier_Tier_1", "Qualifier_Tier_2", "Qualifier_Tier_3"], default="ZhangJiaJie_Medium")
|
||||
parser.add_argument('--enable_viz_traj', dest='viz_traj', action='store_true', default=False)
|
||||
parser.add_argument('--img_benchmark_type', type=str, choices=["simGetImage", "simGetImages"], default="simGetImages")
|
||||
parser.add_argument('--enable_viz_image_cv2', dest='viz_image_cv2', action='store_true', default=False)
|
||||
parser.add_argument('--race_tier', type=int, choices=[1,2,3], default=1)
|
||||
parser.add_argument(
|
||||
"--level_name",
|
||||
type=str,
|
||||
choices=[
|
||||
"Soccer_Field_Easy",
|
||||
"Soccer_Field_Medium",
|
||||
"ZhangJiaJie_Medium",
|
||||
"Building99_Hard",
|
||||
"Qualifier_Tier_1",
|
||||
"Qualifier_Tier_2",
|
||||
"Qualifier_Tier_3",
|
||||
],
|
||||
default="ZhangJiaJie_Medium",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--enable_viz_traj", dest="viz_traj", action="store_true", default=False
|
||||
)
|
||||
parser.add_argument(
|
||||
"--img_benchmark_type",
|
||||
type=str,
|
||||
choices=["simGetImage", "simGetImages"],
|
||||
default="simGetImages",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--enable_viz_image_cv2",
|
||||
dest="viz_image_cv2",
|
||||
action="store_true",
|
||||
default=False,
|
||||
)
|
||||
parser.add_argument("--race_tier", type=int, choices=[1, 2, 3], default=1)
|
||||
|
||||
args = parser.parse_args()
|
||||
main(args)
|
||||
main(args)
|
||||
|
|
|
|||
|
|
@ -1,3 +1,3 @@
|
|||
from utils import AirSimSettingsCreator
|
||||
|
||||
AirSimSettingsCreator().write_airsim_neurips_baseline_settings_file()
|
||||
AirSimSettingsCreator().write_airsim_neurips_baseline_settings_file()
|
||||
|
|
|
|||
|
|
@ -3,39 +3,49 @@ import json
|
|||
import numpy as np
|
||||
import os
|
||||
|
||||
|
||||
def to_airsim_vector(np_arr):
|
||||
assert np.size(np_arr) == 3
|
||||
return airsim.Vector3r(np.float(np_arr[0]), np.float(np_arr[1]), np.float(np_arr[2]))
|
||||
return airsim.Vector3r(
|
||||
np.float(np_arr[0]), np.float(np_arr[1]), np.float(np_arr[2])
|
||||
)
|
||||
|
||||
|
||||
def to_airsim_vectors(np_arr):
|
||||
return [to_airsim_vector(np_arr[i, :]) for i in range(np.size(np_arr, 0))]
|
||||
|
||||
|
||||
# these clases are only meant to be settings generator.
|
||||
# for everything else, there's airsimdroneracinglab.Pose()
|
||||
class Position():
|
||||
def __init__(self, x = 0.0, y = 0.0, z = 0.0):
|
||||
class Position:
|
||||
def __init__(self, x=0.0, y=0.0, z=0.0):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.z = z
|
||||
|
||||
class Rotation():
|
||||
def __init__(self, yaw = 0.0, pitch = 0.0, roll = 0.0):
|
||||
|
||||
class Rotation:
|
||||
def __init__(self, yaw=0.0, pitch=0.0, roll=0.0):
|
||||
self.yaw = yaw
|
||||
self.pitch = pitch
|
||||
self.roll = roll
|
||||
|
||||
class Pose():
|
||||
|
||||
class Pose:
|
||||
def __init__(self, position, rotation):
|
||||
self.position = position
|
||||
self.rotation = rotation
|
||||
|
||||
|
||||
class AirSimSettingsCreator(object):
|
||||
def __init__(self, sim_mode = "Multirotor"):
|
||||
def __init__(self, sim_mode="Multirotor"):
|
||||
self.sim_mode = sim_mode
|
||||
self.settings_dict = {}
|
||||
|
||||
def add_minimal(self):
|
||||
self.settings_dict["SeeDocsAt"] = "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md"
|
||||
self.settings_dict[
|
||||
"SeeDocsAt"
|
||||
] = "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md"
|
||||
self.settings_dict["SettingsVersion"] = 1.2
|
||||
self.settings_dict["SimMode"] = self.sim_mode
|
||||
self.settings_dict["ClockSpeed"] = 1
|
||||
|
|
@ -50,28 +60,37 @@ class AirSimSettingsCreator(object):
|
|||
setting_key["Yaw"] = pose.rotation.yaw
|
||||
|
||||
def add_multirotor(self, vehicle_name, pose):
|
||||
assert(self.settings_dict["SimMode"] == "Multirotor")
|
||||
assert self.settings_dict["SimMode"] == "Multirotor"
|
||||
if "Vehicles" not in self.settings_dict.keys():
|
||||
self.settings_dict['Vehicles'] = {}
|
||||
self.settings_dict["Vehicles"] = {}
|
||||
|
||||
self.settings_dict['Vehicles'][vehicle_name] = {}
|
||||
self.settings_dict['Vehicles'][vehicle_name]["VehicleType"] = "SimpleFlight"
|
||||
self.set_pose(self.settings_dict['Vehicles'][vehicle_name], pose)
|
||||
self.settings_dict["Vehicles"][vehicle_name] = {}
|
||||
self.settings_dict["Vehicles"][vehicle_name]["VehicleType"] = "SimpleFlight"
|
||||
self.set_pose(self.settings_dict["Vehicles"][vehicle_name], pose)
|
||||
|
||||
def add_camera(self, vehicle_name, camera_name, relative_pose, image_type, image_width, image_height, fov_horizontal_degrees):
|
||||
def add_camera(
|
||||
self,
|
||||
vehicle_name,
|
||||
camera_name,
|
||||
relative_pose,
|
||||
image_type,
|
||||
image_width,
|
||||
image_height,
|
||||
fov_horizontal_degrees,
|
||||
):
|
||||
# fetch vehicle setting dict
|
||||
vehicle_setting = self.settings_dict['Vehicles'][vehicle_name]
|
||||
# initialize vehicle's camera setting dict to empty
|
||||
vehicle_setting['Cameras'] = {}
|
||||
vehicle_setting['Cameras'][camera_name] = {}
|
||||
camera_setting = vehicle_setting['Cameras'][camera_name]
|
||||
vehicle_setting = self.settings_dict["Vehicles"][vehicle_name]
|
||||
# initialize vehicle's camera setting dict to empty
|
||||
vehicle_setting["Cameras"] = {}
|
||||
vehicle_setting["Cameras"][camera_name] = {}
|
||||
camera_setting = vehicle_setting["Cameras"][camera_name]
|
||||
self.set_pose(camera_setting, relative_pose)
|
||||
capture_setting = {}
|
||||
capture_setting['Width'] = image_width
|
||||
capture_setting['Height'] = image_height
|
||||
capture_setting['ImageType'] = image_type
|
||||
capture_setting['FOV_Degrees'] = fov_horizontal_degrees
|
||||
camera_setting['CaptureSettings'] = [capture_setting]
|
||||
capture_setting["Width"] = image_width
|
||||
capture_setting["Height"] = image_height
|
||||
capture_setting["ImageType"] = image_type
|
||||
capture_setting["FOV_Degrees"] = fov_horizontal_degrees
|
||||
camera_setting["CaptureSettings"] = [capture_setting]
|
||||
|
||||
# default linux: /home/$USER/Documents/AirSim/settings.json
|
||||
# default windows: C:\\Users\\%USERNAME%\\Documents\\AirSim\\settings.json
|
||||
|
|
@ -88,8 +107,19 @@ class AirSimSettingsCreator(object):
|
|||
def write_airsim_neurips_baseline_settings_file(self):
|
||||
instance = self.__class__()
|
||||
instance.add_minimal()
|
||||
instance.add_multirotor(vehicle_name = "drone_1", pose = Pose(Position(), Rotation()))
|
||||
instance.add_camera(vehicle_name = "drone_1", camera_name = 'fpv_cam', relative_pose=Pose(Position(0.25, 0.0, 0.0), Rotation()),
|
||||
image_type = 0, image_width = 320, image_height = 240, fov_horizontal_degrees = 90)
|
||||
instance.add_multirotor(vehicle_name = "drone_2", pose = Pose(Position(), Rotation()))
|
||||
instance.write_airsim_settings_file()
|
||||
instance.add_multirotor(
|
||||
vehicle_name="drone_1", pose=Pose(Position(), Rotation())
|
||||
)
|
||||
instance.add_camera(
|
||||
vehicle_name="drone_1",
|
||||
camera_name="fpv_cam",
|
||||
relative_pose=Pose(Position(0.25, 0.0, 0.0), Rotation()),
|
||||
image_type=0,
|
||||
image_width=320,
|
||||
image_height=240,
|
||||
fov_horizontal_degrees=90,
|
||||
)
|
||||
instance.add_multirotor(
|
||||
vehicle_name="drone_2", pose=Pose(Position(), Rotation())
|
||||
)
|
||||
instance.write_airsim_settings_file()
|
||||
|
|
|
|||
|
|
@ -1,36 +1,55 @@
|
|||
from argparse import ArgumentParser
|
||||
import subprocess
|
||||
|
||||
class DockerImageBuilder():
|
||||
|
||||
class DockerImageBuilder:
|
||||
def __init__(self, args):
|
||||
self.args = args
|
||||
self.args = args
|
||||
|
||||
def build_docker_image(self):
|
||||
# if a base image is not specified, we use the Ubuntu 18, CUDA 10 image from NVIDIA
|
||||
docker_build_command = ['docker', 'build', '--network=host', \
|
||||
'-t', self.args.target_image, \
|
||||
'-f', self.args.dockerfile, \
|
||||
'--build-arg', 'BASE_IMAGE=' + self.args.base_image, \
|
||||
'.']
|
||||
docker_build_command = [
|
||||
"docker",
|
||||
"build",
|
||||
"--network=host",
|
||||
"-t",
|
||||
self.args.target_image,
|
||||
"-f",
|
||||
self.args.dockerfile,
|
||||
"--build-arg",
|
||||
"BASE_IMAGE=" + self.args.base_image,
|
||||
".",
|
||||
]
|
||||
|
||||
print(" ".join(docker_build_command))
|
||||
subprocess.call(docker_build_command)
|
||||
|
||||
|
||||
def main(args):
|
||||
docker_image_builder = DockerImageBuilder(args)
|
||||
docker_image_builder.build_docker_image()
|
||||
|
||||
if __name__=="__main__":
|
||||
parser = ArgumentParser(description='AirSim Drone Racing Lab docker image builder')
|
||||
parser.add_argument('--dockerfile', type=str, default='Dockerfile', help='path to docker file')
|
||||
parser.add_argument('--base_image', type=str, default="nvidia/cudagl:10.0-devel-ubuntu18.04", help='base image name AND tag, on top of which the target image is built')
|
||||
parser.add_argument('--target_image', type=str, help='desired name of target image name AND tag')
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = ArgumentParser(description="AirSim Drone Racing Lab docker image builder")
|
||||
parser.add_argument(
|
||||
"--dockerfile", type=str, default="Dockerfile", help="path to docker file"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--base_image",
|
||||
type=str,
|
||||
default="nvidia/cudagl:10.0-devel-ubuntu18.04",
|
||||
help="base image name AND tag, on top of which the target image is built",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--target_image", type=str, help="desired name of target image name AND tag"
|
||||
)
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
# if a target image name is not specified, let's call it adrl:SOURCE_IMAGE_TAG
|
||||
if not args.target_image:
|
||||
target_image_tag = args.base_image.split(":")[1]
|
||||
args.target_image = 'adrl' + ':' + target_image_tag
|
||||
target_image_tag = args.base_image.split(":")[1]
|
||||
args.target_image = "adrl" + ":" + target_image_tag
|
||||
|
||||
main(args)
|
||||
main(args)
|
||||
|
|
|
|||
|
|
@ -5,20 +5,23 @@ import time
|
|||
disqualified_racers = set()
|
||||
finished_racers = set()
|
||||
|
||||
|
||||
def open_file():
|
||||
list_of_files = glob.glob('*.log')
|
||||
list_of_files = glob.glob("*.log")
|
||||
latest_file = max(list_of_files, key=os.path.getctime)
|
||||
print("Opened file: " + latest_file)
|
||||
return open(latest_file, "r+")
|
||||
|
||||
|
||||
def follow(filename):
|
||||
filename.seek(0,2)
|
||||
filename.seek(0, 2)
|
||||
while True:
|
||||
line = filename.readline()
|
||||
if not line:
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
yield line
|
||||
yield line
|
||||
|
||||
|
||||
def process(line):
|
||||
tokens = line.split()
|
||||
|
|
@ -27,35 +30,49 @@ def process(line):
|
|||
print("Tokens: " + str(tokens))
|
||||
return
|
||||
|
||||
if tokens[3] == "disqualified" and tokens[4] == '1' and tokens[0] not in disqualified_racers:
|
||||
if (
|
||||
tokens[3] == "disqualified"
|
||||
and tokens[4] == "1"
|
||||
and tokens[0] not in disqualified_racers
|
||||
):
|
||||
disqualified_racers.add(tokens[0])
|
||||
handle_disqualified_racer(tokens[0])
|
||||
return
|
||||
|
||||
if tokens[3] == "finished" and tokens[4] == '1' and tokens[0] not in finished_racers:
|
||||
if (
|
||||
tokens[3] == "finished"
|
||||
and tokens[4] == "1"
|
||||
and tokens[0] not in finished_racers
|
||||
):
|
||||
finished_racers.add(tokens[0])
|
||||
handle_finished_racer(tokens[0])
|
||||
return
|
||||
|
||||
if tokens[3] == "gates_passed":
|
||||
handle_gate_passed(tokens[0], tokens[4])
|
||||
handle_gate_passed(tokens[0], tokens[4])
|
||||
|
||||
|
||||
def handle_disqualified_racer(racer_name):
|
||||
print(racer_name + " has been disqualified!")
|
||||
#Start a new race.
|
||||
# Start a new race.
|
||||
|
||||
|
||||
def handle_finished_racer(racer_name):
|
||||
print(racer_name + " has finished!")
|
||||
#Start a new race.
|
||||
# Start a new race.
|
||||
|
||||
|
||||
def handle_gate_passed(racer_name, gate_idx_passed):
|
||||
# log file gate indices are 1-indexed, not 0-indexed
|
||||
print("{} passed gate idx {}".format(racer_name, gate_idx_passed - 1))
|
||||
|
||||
|
||||
def main():
|
||||
f = open_file()
|
||||
for line in follow(f):
|
||||
process(line)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
main()
|
||||
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@
|
|||
"SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md",
|
||||
"SettingsVersion": 1.2,
|
||||
"SimMode": "Multirotor",
|
||||
"ViewMode": "NoDisplay",
|
||||
"ViewMode": "NoDisplay",
|
||||
"Vehicles": {
|
||||
"drone_1": {
|
||||
"Cameras": {
|
||||
|
|
@ -42,4 +42,4 @@
|
|||
"Z": 0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -2,16 +2,23 @@ import airsimdroneracinglab
|
|||
import threading
|
||||
import time
|
||||
|
||||
class ReproduceResetRaceCondition():
|
||||
def __init__(self, drone_name = "drone_1"):
|
||||
|
||||
class ReproduceResetRaceCondition:
|
||||
def __init__(self, drone_name="drone_1"):
|
||||
self.airsim_client = airsimdroneracinglab.MultirotorClient()
|
||||
self.airsim_client_2 = airsimdroneracinglab.MultirotorClient()
|
||||
self.airsim_client_3 = airsimdroneracinglab.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.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 repeat_timer(self, callback, period):
|
||||
|
|
@ -19,43 +26,54 @@ class ReproduceResetRaceCondition():
|
|||
callback()
|
||||
time.sleep(period)
|
||||
|
||||
def load_level(self, level_name, sleep_sec = 2.0):
|
||||
def load_level(self, level_name, sleep_sec=2.0):
|
||||
self.level_name = level_name
|
||||
self.airsim_client.simLoadLevel(self.level_name)
|
||||
self.airsim_client.confirmConnection() # failsafe
|
||||
time.sleep(sleep_sec) # let the environment load completely
|
||||
self.airsim_client.confirmConnection() # failsafe
|
||||
time.sleep(sleep_sec) # let the environment load completely
|
||||
|
||||
def reset(self):
|
||||
print(time.time(), 'called reset')
|
||||
print(time.time(), "called reset")
|
||||
self.airsim_client.reset()
|
||||
|
||||
def reset_race(self):
|
||||
print(time.time(), 'called simResetRace')
|
||||
print(time.time(), "called simResetRace")
|
||||
self.airsim_client_2.simResetRace()
|
||||
|
||||
def reset_and_reset_race(self):
|
||||
print(time.time(), 'called reset, followed by simResetRace')
|
||||
print(time.time(), "called reset, followed by simResetRace")
|
||||
self.airsim_client_3.reset()
|
||||
self.airsim_client_3.simResetRace()
|
||||
|
||||
def start_race(self, tier):
|
||||
print(time.time(), 'called start race')
|
||||
print(time.time(), "called start race")
|
||||
self.airsim_client.simStartRace(tier)
|
||||
|
||||
def initialize_drone(self):
|
||||
self.airsim_client.enableApiControl(vehicle_name=self.drone_name)
|
||||
self.airsim_client.arm(vehicle_name=self.drone_name)
|
||||
|
||||
# set default values for trajectory tracker gains
|
||||
traj_tracker_gains = airsimdroneracinglab.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)
|
||||
# set default values for trajectory tracker gains
|
||||
traj_tracker_gains = airsimdroneracinglab.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,
|
||||
)
|
||||
|
||||
self.airsim_client.setTrajectoryTrackerGains(traj_tracker_gains, vehicle_name=self.drone_name)
|
||||
self.airsim_client.setTrajectoryTrackerGains(
|
||||
traj_tracker_gains, vehicle_name=self.drone_name
|
||||
)
|
||||
time.sleep(0.2)
|
||||
|
||||
def start_threads(self):
|
||||
|
|
@ -74,9 +92,10 @@ class ReproduceResetRaceCondition():
|
|||
self.thread_reset_and_reset_race.join()
|
||||
print("Stopped threads.")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
reproducer = ReproduceResetRaceCondition('drone_1')
|
||||
reproducer.load_level('Qualifier_Tier_1')
|
||||
reproducer = ReproduceResetRaceCondition("drone_1")
|
||||
reproducer.load_level("Qualifier_Tier_1")
|
||||
reproducer.initialize_drone()
|
||||
reproducer.start_race(3)
|
||||
time.sleep(5)
|
||||
|
|
|
|||
Загрузка…
Ссылка в новой задаче