def takeoff_with_moveOnSpline(self, takeoff_height=2.0):
if self.level_name == "ZhangJiaJie_Medium":
takeoff_height = 1.0
# time.sleep(1)
start_position = self.airsim_client.simGetVehiclePose(
vehicle_name=self.drone_name).position
start_position.x_val += randf(0, 1)
start_position.y_val += randf(-1, 0)
# start_position.z_val += randf(0, 1)
# start_position.x_val += -2
# start_position.y_val += 2
# start_position.z_val += -1
takeoff_waypoint = airsim.Vector3r(
start_position.x_val, start_position.y_val,
start_position.z_val - takeoff_height)
if (self.plot_transform):
self.airsim_client.plot_transform(
[airsim.Pose(takeoff_waypoint, airsim.Quaternionr())],
vehicle_name=self.drone_name)
self.airsim_client.moveToPositionAsync(takeoff_waypoint.x_val,
takeoff_waypoint.y_val,
takeoff_waypoint.z_val,
1).join()
def takeoff_with_moveOnSpline(self, takeoff_height=0.1):
if self.level_name == "ZhangJiaJie_Medium":
takeoff_height = -3
# time.sleep(1)
start_position = self.airsim_client.simGetVehiclePose(
vehicle_name=self.drone_name).position
takeoff_waypoint = airsim.Vector3r(start_position.x_val,
start_position.y_val,
-takeoff_height)
if (self.plot_transform):
self.airsim_client.plot_transform(
[airsim.Pose(takeoff_waypoint, airsim.Quaternionr())],
vehicle_name=self.drone_name)
self.airsim_client.moveOnSplineAsync(
[takeoff_waypoint],
vel_max=15.0,
acc_max=5.0,
add_position_constraint=True,
add_velocity_constraint=False,
viz_traj=self.viz_traj,
vehicle_name=self.drone_name).join()
def pose_drone(airsim_client,
object_name,
xyz,
axisrot,
theta,
ignore_collision=False,
pause_sim=True):
# need to pause sim otherwise physics happens
airsim_client.simPause(pause_sim)
quat = math_utils.rotvec2quat(axisrot, theta)
pose = airsim.Pose(position_val={
'x_val': xyz[0],
'y_val': xyz[1],
'z_val': xyz[2]
},
orientation_val={
'w_val': quat[0],
'x_val': quat[1],
'y_val': quat[2],
'z_val': quat[3]
})
pose = airsim_client.simSetVehiclePose(pose,
ignore_collision,
vehicle_name=object_name)
return pose
def takeoff_with_moveOnSpline(self, takeoff_height = 0.1):
if self.level_name == "ZhangJiaJie_Medium":
takeoff_height = 0.3
takeoff_waypoint = airsim.Vector3r(0, 0, -takeoff_height)
if(self.plot_transform):
self.airsim_client.plot_transform([airsim.Pose(takeoff_waypoint, airsim.Quaternionr())], vehicle_name=self.drone_name)
self.airsim_client.moveOnSplineAsync([takeoff_waypoint], vel_max=15.0, acc_max=5.0, add_curr_odom_position_constraint=True,
add_curr_odom_velocity_constraint=False, viz_traj=self.viz_traj, vehicle_name=self.drone_name).join()
def odometry_callback(self):
# get uncompressed fpv cam image
drone_state = self.airsim_client_odom.getMultirotorState(
vehicle_name='drone_2')
# in world frame:
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
pose = airsim.Pose(position_val=position, orientation_val=orientation)
t = time.clock()
if len(self.pose_history) == 0:
self.pose_history.append(pose_to_dict(pose, t))
else:
if (t - self.t0) >= 0.05 and self.pose_history[-1] != pose:
self.pose_history.append(pose_to_dict(pose, t))
self.t0 = t
def generate_data(self, csv_path=None, gate_lim=10):
# load json or use attribute
history_df = self.pose_history_df if csv_path is None else pd.read_csv(
csv_path, sep=',')
# At each position, place camera/drone
for row in history_df.itertuples():
print(f"Processing t = {row.t}...")
# At each position, place camera/drone
pose = airsim.Pose(position_val={
'x_val': row.x,
'y_val': row.y,
'z_val': row.z
},
orientation_val={
'w_val': row.qw,
'x_val': row.qx,
'y_val': row.qy,
'z_val': row.qz
})
self.client.simSetVehiclePose(pose,
ignore_collison=True,
vehicle_name=self.vehicle_name)
img_name = self.sess_name + f"_{row.t:0.3f}_0deg"
# Take initial images
self.get_labeled_images(img_name)
# TODO: place drone 2 randomly and take images again
# TODO: Random rotation among 4 quadrants of drone POV...
# use airsim.utils.to_eularian_angles(pose.orientation), adjust, use airsim.utils.to_quaternion(roll, pitch, yaw)
# euler_ang0 = airsim.utils.to_eularian_angles(pose.orientation)
# TODO: Randomize gate sizes?
# TODO: place drone 2 randomly and take images again
# Update master list of sim seg ID's
self.seg_id_sess = np.unique(self.seg_id_sess)
master_set = set(self.seg_id_list)
for seg_id in self.seg_id_sess:
if seg_id not in master_set:
self.seg_id_list.append(seg_id)
filename = self.sim_seg_id_list_path.name.replace(
'.json', '_updated.json')
with open(Path(self.sim_seg_id_list_path.parent, filename), 'w') as f:
json.dump(self.seg_id_list, f)
def pose_object(airsim_client,
object_name,
xyz,
axisrot,
theta,
teleport=True):
quat = math_utils.rotvec2quat(axisrot, theta)
pose = airsim.Pose(position_val={
'x_val': xyz[0],
'y_val': xyz[1],
'z_val': xyz[2]
},
orientation_val={
'w_val': quat[0],
'x_val': quat[1],
'y_val': quat[2],
'z_val': quat[3]
})
pose = airsim_client.simSetObjectPose(object_name, pose, teleport=teleport)
return pose