def reset(self):
self.client.reset()
self.client.enableApiControl(True)
self.client.armDisarm(True)
# Move the TargetActor to initial position
self.client.simSetObjectPose("TargetActor", self.initialPose, True)
# Change initial position
self._move_target("TargetActor")
# Spawn the drone at random position around frontal position
offset = airsim.Vector3r(np.random.uniform(-2, 2),
np.random.uniform(0, 1),
np.random.uniform(-0.5, 0.3))
ori = self.client.simGetObjectPose("TargetActor").orientation
while np.isnan(ori.w_val):
#print("FAIL")
ori = self.client.simGetObjectPose("TargetActor").orientation
offset = self._transform_to_frame(offset, ori)
dronePos = self.frontalPos + offset
self.client.simSetVehiclePose(
airsim.Pose(dronePos, airsim.to_quaternion(0, 0, 0)), True)
self.client.moveByVelocityZAsync(0, 0, dronePos.z_val,
self.duration).join()
time.sleep(0.05)
# Look at target direction
self._look_at_target()
self.client.simSetCameraOrientation("bottom_center",
airsim.to_quaternion(1.40, 0, 0))
# Reset variables
self.start_time = time.time()
return self._observe()
def _take_action(self, action):
# Baselines stuff
action = np.clip(action, [
-self.maxSpeed, -self.maxSpeed, -self.maxSpeed, -self.maxAngle,
-self.maxAngle
], [
+self.maxSpeed, +self.maxSpeed, +self.maxSpeed, +self.maxAngle,
+self.maxAngle
])
# Drone
# action[0] -> forwards/backwards
# action[1] -> left/right
# action[2] -> up/down
v = airsim.Vector3r(action[0], action[1], action[2])
# Get the drone's orientation
#ori = self.client.simGetGroundTruthKinematics().orientation
ori = self.client.simGetCameraInfo("front_center").pose.orientation
v = self._transform_to_frame(v, ori)
self.client.moveByVelocityAsync(
v.x_val, v.y_val, v.z_val - 0.09, self.duration,
airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode()).join()
z = self.client.simGetGroundTruthKinematics().position.z_val
self.client.moveByVelocityZAsync(0, 0, z, self.duration).join()
# Camera
# action[3] -> up/down
# action[4] -> left/right
qRot = airsim.to_quaternion(action[3], 0, 0)
self.qCam = self.qCam * qRot
qRot = airsim.to_quaternion(0, 0, action[4])
self.qCam = qRot * self.qCam
self.client.simSetCameraOrientation("front_center", self.qCam)
def _take_action(self, action):
# Baselines stuff
#action = np.clip(action, [-self.maxSpeed,-self.maxSpeed,-self.maxSpeed,-self.maxAngle,-self.maxAngle], [+self.maxSpeed,+self.maxSpeed,+self.maxSpeed,+self.maxAngle,+self.maxAngle])
# Baselines stuff
action = np.float64(action)
#print(action)
# Drone
# action[0] -> forwards/backwards
# action[1] -> left/right
# action[2] -> up/down
self.client.moveByVelocityAsync(
action[0], action[1], action[2] / 2, self.duration,
airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode()).join()
z = self.client.simGetGroundTruthKinematics().position.z_val
self.client.moveByVelocityZAsync(0, 0, z, self.duration).join()
# Camera
# action[3] -> up/down
# action[4] -> left/right
qRot = airsim.to_quaternion(action[3], 0, 0)
self.qCam = self.qCam * qRot
qRot = airsim.to_quaternion(0, action[4], 0)
self.qCam = qRot * self.qCam
self.client.simSetCameraOrientation("bottom_center", self.qCam)
def _setup_my_cameras(self):
"""
Helper function to set the left, right, forward, and back cameras up
on the vehicle as I've see fit.
:returns: nada
"""
left_cam_orientation = airsim.to_quaternion(pitch=-0.17,
yaw=-1.04,
roll=0.0)
forward_cam_orientation = airsim.to_quaternion(pitch=-0.17,
yaw=0.0,
roll=0.0)
right_cam_orientation = airsim.to_quaternion(pitch=-0.17,
yaw=1.04,
roll=0.0)
backward_cam_orientation = airsim.to_quaternion(pitch=-0.17,
yaw=0.0,
roll=0.0)
# creates a panoram-ish camera set-up
self.client.simSetCameraOrientation(self.left_cam_name,
left_cam_orientation)
self.client.simSetCameraOrientation(self.right_cam_name,
right_cam_orientation)
self.client.simSetCameraOrientation(self.forward_cam_name,
forward_cam_orientation)
def _look_at_target(self):
# Get the direction of the target object
cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position
targetDirection = self.targetPos - cameraPos
if targetDirection.get_length() > 0:
# The default camera pitch
defaultDirection = airsim.Vector3r(0, 0, 1)
# Calculate the angle between the two
pitchTheta = np.arccos(
targetDirection.dot(defaultDirection) /
(targetDirection.get_length() * defaultDirection.get_length()))
#targetDirection.z_val = 0 # for yaw vector
# The default camera yaw
defaultDirection = airsim.Vector3r(1, 0, 0)
# Calculate the angle between the two
yawTheta = np.arccos(
targetDirection.dot(defaultDirection) /
(targetDirection.get_length() * defaultDirection.get_length()))
if targetDirection.y_val < defaultDirection.y_val:
yawTheta = -yawTheta
else:
pitchTheta = 0
yawTheta = 0
# Set camera pitch
self.qCam = airsim.to_quaternion(pitchTheta, 0, 0)
# Set camera yaw
self.qCam = airsim.to_quaternion(0, yawTheta, 0) * self.qCam
self.client.simSetCameraOrientation("bottom_center", self.qCam)
def _take_action(self, action):
#TO_DO ZOOM
"""camera_info = self.client.simGetCameraInfo("bottom_center")
print("CameraInfo %s: %s" % ("bottom_center", pprint.pformat(camera_info)))
drone_info = self.client.simGetGroundTruthKinematics()
print("DroneInfo %s: %s" % ("drone", pprint.pformat(drone_info)))"""
if action == 0:
pass
elif action == 1:
# Camera up
qRot = airsim.to_quaternion(0.0872, 0, 0)
self.qCam = self.qCam * qRot
elif action == 2:
# Camera down
qRot = airsim.to_quaternion(-0.0872, 0, 0)
self.qCam = self.qCam * qRot
elif action == 3:
# Camera left
qRot = airsim.to_quaternion(0, -0.0872, 0)
self.qCam = qRot * self.qCam
elif action == 4:
# Camera right
qRot = airsim.to_quaternion(0, 0.0872, 0)
self.qCam = qRot * self.qCam
self.client.simSetCameraOrientation("bottom_center", self.qCam)
def add_offset_to_pose(pose, pos_offset=1, yaw_offset=np.pi*2, enable_offset=True):
if enable_offset:
position = airsim.Vector3r(pos_offset*np.random.rand()+pose[0], pos_offset*np.random.rand()+pose[1], -pose[2])
orientation = airsim.to_quaternion(0, 0, yaw_offset*np.random.rand()+pose[3])
pose_with_offset = airsim.Pose(position, orientation)
return pose_with_offset
else:
position = airsim.Vector3r(pose[0], pose[1], -pose[2])
orientation = airsim.to_quaternion(0, 0, pose[3])
pose_without_offset = airsim.Pose(position, orientation)
return pose_without_offset
def main():
global nmap, road_points, path_in_list, path_in_environment
# finish = turn.execute_turns(client, nmap, road_points, path_in_list, path_in_environment)
if len(sys.argv) == 2:
client = airsim.CarClient()
client.confirmConnection()
print('Connect succcefully！')
client.enableApiControl(True)
car_controls = airsim.CarControls()
client.simSetTimeOfDay(True, "2019-08-19 21:00:00", True, 1, 60, False)
client.simSetCameraOrientation(
2, airsim.to_quaternion(0, 0, -math.pi / 4))
client.simSetCameraOrientation(1,
airsim.to_quaternion(0, 0, math.pi / 4))
start_point = [13, 13]
target_point = [0, 22]
nmap, road_points, path_in_list, path_in_environment = turn.init_path_planning(
start_point, target_point)
if sys.argv[1] == 'test':
print('start testing')
testNetwork(client, car_controls, list(path_in_environment[-1]))
elif sys.argv[1] == 'train':
print('start training')
trainNetwork(client, car_controls)
else:
print('unkown input argument, please run this file as follow!')
print('python DQN_airsim.py train OR python DQN_airsim.py test')
elif len(sys.argv) == 3:
client = airsim.CarClient()
client.confirmConnection()
print('Connect succcefully！')
if sys.argv[1] == 'show_map' and sys.argv[2] == 'train':
print('display map')
show_map.show_map(client, [])
elif sys.argv[1] == 'show_map' and sys.argv[2] == 'test':
print('display map')
show_map.show_map(client, path_in_environment)
else:
print('unkown input argument, please run this file as follow!')
print(
'python DQN_airsim.py show_map train OR python DQN_airsim.py show_map test'
)
else:
print('please include an argument train or test or show_map')
def reset(self):
self.client.reset()
self.client.enableApiControl(True)
self.client.armDisarm(True)
# Spawn the drone at random position
self.client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(np.random.randint(0, 16),
np.random.randint(-10, 11),
np.random.randint(-5, -3)),
airsim.to_quaternion(0, 0, 0)), True)
time.sleep(0.05)
# Get the direction of the target object
cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position
targetDirection = self.targetPos - cameraPos
#targetDirection.z_val = 0 # for yaw vector
if targetDirection.get_length() > 0:
# The default camera pitch
defaultDirection = airsim.Vector3r(0, 0, 1)
# Calculate the angle between the two
pitchTheta = np.arccos(
targetDirection.dot(defaultDirection) /
(targetDirection.get_length() * defaultDirection.get_length()))
# The default camera yaw
defaultDirection = airsim.Vector3r(1, 0, 0)
# Calculate the angle between the two
yawTheta = np.arccos(
targetDirection.dot(defaultDirection) /
(targetDirection.get_length() * defaultDirection.get_length()))
if targetDirection.y_val < defaultDirection.y_val:
yawTheta = -yawTheta
else:
pitchTheta = 0
yawTheta = 0
# Set camera pitch
self.qCam = airsim.to_quaternion(pitchTheta, 0, 0)
# Set camera yaw
self.qCam = airsim.to_quaternion(0, yawTheta, 0) * self.qCam
self.client.simSetCameraOrientation("bottom_center", self.qCam)
# Perform trajectory
self._run_trajectory()
return self._observe()
def take_action(self, action, num_actions, phase):
# Set Paramaters
fov_v = 45 * np.pi / 180
fov_h = 80 * np.pi / 180
r = 0.4
ignore_collision = False
sqrt_num_actions = np.sqrt(num_actions)
posit = self.client.simGetVehiclePose()
pos = posit.position
orientation = posit.orientation
quat = (orientation.w_val, orientation.x_val, orientation.y_val,
orientation.z_val)
eulers = euler_from_quaternion(quat)
alpha = eulers[2]
theta_ind = int(action[0] / sqrt_num_actions)
psi_ind = action[0] % sqrt_num_actions
theta = fov_v / sqrt_num_actions * (theta_ind -
(sqrt_num_actions - 1) / 2)
psi = fov_h / sqrt_num_actions * (psi_ind - (sqrt_num_actions - 1) / 2)
# print('Theta: ', theta * 180 / np.pi, end='')
# print(' Psi: ', psi * 180 / np.pi)
if phase == 'train':
noise_theta = (fov_v / sqrt_num_actions) / 6
noise_psi = (fov_h / sqrt_num_actions) / 6
psi = psi + random.uniform(-1, 1) * noise_psi
theta = theta + random.uniform(-1, 1) * noise_theta
x = pos.x_val + r * np.cos(alpha + psi)
y = pos.y_val + r * np.sin(alpha + psi)
z = pos.z_val + r * np.sin(
theta) # -ve because Unreal has -ve z direction going upwards
self.client.simSetVehiclePose(airsim.Pose(
airsim.Vector3r(x, y, z),
airsim.to_quaternion(0, 0, alpha + psi)),
ignore_collison=ignore_collision)
elif phase == 'infer':
r_infer = 0.5
vx = r_infer * np.cos(alpha + psi)
vy = r_infer * np.sin(alpha + psi)
vz = r_infer * np.sin(theta)
# TODO
# Take average of previous velocities and current to smoothen out drone movement.
self.client.moveByVelocityAsync(
vx=vx,
vy=vy,
vz=vz,
duration=1,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False,
yaw_or_rate=180 * (alpha + psi) /
np.pi))
def get_topview_image(hight=100, start_pos=[0, 0], drone=None, tmp_dir="airsim_drone"):
drone.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(start_pos[0], start_pos[1], -hight),
airsim.to_quaternion(0, 0, 0)), True, vehicle_name='Drone1')
image_responses = drone.client.simGetImages([
airsim.ImageRequest("bottom_center", airsim.ImageType.DepthPlanner, True)], vehicle_name="Drone1")
image_response = image_responses[0]
img1d = image_response.image_data_float
img2d = np.reshape(img1d, (image_response.height, image_response.width))
filename = os.path.join(tmp_dir, "depth_" + str(hight) + '.png')
imageio.imwrite(os.path.normpath(os.path.join(tmp_dir, "depth_" + str(hight) + '.png')),
generate_contrast_viz(img2d, hight))
# plt.imshow(img2d)
# plt.show()
real_image_responses = drone.client.simGetImages([
airsim.ImageRequest("bottom_center", airsim.ImageType.Scene, False, False)], vehicle_name="Drone1")
img1d = np.fromstring(real_image_responses[0].image_data_uint8, dtype=np.uint8)
img_rgb = img1d.reshape(real_image_responses[0].height, real_image_responses[0].width, 3)
# plt.imshow(img_rgb)
# plt.show()
# airsim.write_png(os.path.normpath(os.path.join(tmp_dir, "real_" + str(hight) + '.png')), img_rgb)
imageio.imwrite(os.path.normpath(os.path.join(tmp_dir, "real_" + str(hight) + '.png')), img_rgb)
return img_rgb, filename
def reset(client, scene=None):
"""
Called from the reset method in AirsimEnv. This method resets the AirSim simulation, respawns the UAV etc.
:param client: AirsimEnv object
:param scene: The AirSim scene
:return: None
"""
client.reset()
if scene is not None:
time.sleep(0.2)
pose = client.simGetVehiclePose()
start_pos = valid_spawn(scene)
pose.position.x_val = start_pos[0]
pose.position.y_val = start_pos[1]
pose.position.z_val = 0
pitch, roll, yaw = airsim.to_eularian_angles(pose.orientation)
yaw = np.random.rand() * 2 * np.pi
pose.orientation = airsim.to_quaternion(pitch, roll, yaw)
client.simSetVehiclePose(pose, True)
time.sleep(0.2)
client.enableApiControl(True)
client.armDisarm(True)
hover(client)
custom_takeoff(client)
hover(client)
def get_start_pose(self, random=True, verbose=True):
# 在路上选择一个起始位置和方向
if not random: # 固定选择默认的起始位置
position = np.array([0., 0.])
yaw = 0.
else: # 随机选择一个位置
if not hasattr(self, 'roads_without_corners'):
self.roads_without_corners = self.get_roads(
include_corners=False)
# 计算位置
road_index = np.random.choice(len(self.roads_without_corners))
p, q = self.roads_without_corners[road_index]
t = np.random.uniform(0.3, 0.7)
position = t * p + (1. - t) * q
# 计算朝向
if np.isclose(p[0], q[0]): # 与 Y 轴平行
yaws = [0.5 * math.pi, -0.5 * math.pi]
elif np.isclose(p[1], q[1]): # 与 X 轴平行
yaws = [0., math.pi]
yaw = np.random.choice(yaws)
if verbose:
print('起始位置 = {}, 方向 = {}'.format(position, yaw))
position = airsim.Vector3r(position[0], position[1], -0.6)
orientation = airsim.to_quaternion(pitch=0., roll=0., yaw=yaw)
pose = airsim.Pose(position, orientation)
return pose
def test_car_rand_pose(vehicle_client, car_client):
# get the fixed location to test the orientation randomness
starting_points_fixed, _ = get_random_pose()
i = 0
while i < 30:
# print('setting position')
starting_points, starting_direction = get_random_pose()
# set car location and orientation
vehicle_client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(starting_points[0], starting_points[1], starting_points[2]),
airsim.to_quaternion(starting_direction[0], starting_direction[1],
starting_direction[2])), True)
# test the car orientation
# print(starting_direction)
# car_client.simSetVehiclePose(
# airsim.Pose(airsim.Vector3r(starting_points_fixed[0], starting_points_fixed[1], starting_points_fixed[2]),
# airsim.to_quaternion(starting_direction[0], starting_direction[1],
# starting_direction[2] + 0.01)), True)
# print('wait for momentum die out')
car_controls = airsim.CarControls()
car_controls.steering = 0
car_controls.throttle = 0
car_controls.brake = 1
car_client.setCarControls(car_controls)
time.sleep(4)
i += 1
def initial_positions(name, initZ=0, num_agents = 1):
name = name+'()'
orig_ip, levels, crash_threshold = eval(name)
ip_each_drone = int(np.floor(len(orig_ip) / num_agents))
reset_array = {}
reset_array_raw = {}
level_names = {}
for agents in range(num_agents):
name_agent = "drone" + str(agents)
ind = ip_each_drone * agents
player_start_unreal = orig_ip[ind]
reset_array[name_agent] = []
reset_array_raw[name_agent] = []
level_names[name_agent] = []
physical_player_start = orig_ip[0]
for i in range(ip_each_drone):
x1 = (orig_ip[i+ind][0]-player_start_unreal[0])/100
y1 = (orig_ip[i+ind][1]-player_start_unreal[1])/100
x_raw = (orig_ip[i+ind][0]-physical_player_start[0])/100
y_raw = (orig_ip[i+ind][1]-physical_player_start[1])/100
# z1 = 0
z1 = initZ # in case of computervision mode
pitch = 0
roll = 0
yaw = orig_ip[i+ind][2]*np.pi/180
pp = airsim.Pose(airsim.Vector3r(x1, y1, z1), airsim.to_quaternion(pitch, roll, yaw))
reset_array[name_agent].append(pp)
reset_array_raw[name_agent].append([x_raw, y_raw, z1, yaw*180/np.pi, roll*180/np.pi, pitch*180/np.pi])
level_names[name_agent].append(levels[ind+i])
return reset_array, reset_array_raw, level_names, crash_threshold
def __init__(self):
self.client = airsim.MultirotorClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
self.client.armDisarm(True)
orientation = airsim.to_quaternion(-np.pi / 6, 0, 0)
self.client.simSetCameraOrientation('0', orientation)
def calculate_camera(self, v_identifier):
"""
Calculate camera heading
"""
current_pos_state = self.client.getMultirotorState(
).kinematics_estimated
current_pos_local = np.asarray([
current_pos_state.position.x_val, current_pos_state.position.y_val,
current_pos_state.position.z_val
])
# print("now:"+str(current_pos_local))
camera_angle = self.calculate_camera_to_center(current_pos_local)
global g_current_orientation_pitch, g_current_orientation_roll, g_current_orientation_yaw
g_current_orientation_pitch = camera_angle[0]
g_current_orientation_roll = 0
g_current_orientation_yaw = camera_angle[
2] - airsim.to_eularian_angles(current_pos_state.orientation)[2]
#g_current_orientation_yaw = camera_angle[2]
# log_file("camera",
# v_identifier+":desired orientation:" + str(g_current_orientation_pitch) + ","
# + str(g_current_orientation_yaw))
self.client.simSetCameraOrientation(
"front_center",
airsim.to_quaternion(g_current_orientation_pitch,
g_current_orientation_roll,
g_current_orientation_yaw))
def setCamNum(self, num):
# self.client.confirmConnection()
# self.client.enableApiControl(True, "Camera1")
# self.client.armDisarm(True, "Camera1")
#print("CamNum Callback:")
self.camNum = num.data
print("Switching to Camera: {}".format(self.camNum))
if self.camNum is not 7:
print("Cam Num is not 7")
self.pose.position.x_val = self.camCoords[
self.camNum][0] - self.offset_x
self.pose.position.y_val = self.camCoords[
self.camNum][1] - self.offset_y
self.pose.position.z_val = self.camCoords[self.camNum][2]
#self.pose.position.w_val = self.camCoords[self.camNum][3]
# self.pose.orientation.
#self.pose.orientation.w_val = self.camCoords[self.camNum][3];
self.pose.orientation = airsim.to_quaternion(
0, 0, self.camCoords[self.camNum][3])
print(self.pose)
#print(self.pose);
self.client.simSetVehiclePose(self.pose,
True,
vehicle_name="Camera1").join()
def reset(self):
self.cur_step = 0
self.trajectory.clear_memory()
cur_pos = self.get_cur_position()
if math.isnan(cur_pos[0]) or math.isnan(cur_pos[1]) or math.isnan(cur_pos[2]):
self.client.reset()
self.client.enableApiControl(True)
self.client.armDisarm(True)
orientation = airsim.to_quaternion(-np.pi / 6, 0, 0)
self.client.simSetCameraOrientation('0', orientation)
# set the starting position of the drone to be at 4 meters away from the human
rel_pos = self.local_to_world(np.array([0, -2, -4]), 1)
position = self.client.simGetObjectPose(self.HUMAN_ID).position
position.z_val -= 1.7 / 2
position.x_val += rel_pos[0]
position.y_val += rel_pos[1]
position.z_val += rel_pos[2]
heading = self.client.simGetObjectPose(self.HUMAN_ID).orientation
pose = airsim.Pose(position, heading)
self.client.simSetVehiclePose(pose, True)
# use songxiaocheng's airsim (https://github.com/songxiaocheng/AirSim)
self.client.moveToPositionAsync(position.x_val, position.y_val, position.z_val, 1).join()
# use official airsim
# self.client.takeoffAsync().join()
print("===== start position: ", self.v2t(position))
def AirView(W, X, Y, Z, skeleton_recv, dir=1, rate=1):
pp = pprint.PrettyPrinter(indent=4)
client = airsim.VehicleClient()
client.confirmConnection()
# airsim.wait_key('Press any key to start the tracking')
x_init, y_init, z_init = 0, 0, -1.6
while True:
sk = skeleton_recv.recv()
# print('AirSimCV received:', sk)
if isinstance(sk, list) and len(sk) == 25:
FacePos = sk[0]
x_shift = -FacePos[2] / 50
y_shift = FacePos[0] / 212
z_shift = FacePos[1] / 256
#print("received HeadPose:", HeadPose[0])
n_w, n_qx, n_qy, n_qz = W.value, X.value, Y.value, Z.value
if dir:
client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(x_init + x_shift,
y_init + rate * y_shift,
z_init + rate * z_shift),
airsim.Quaternionr(n_qx, n_qy, n_qz, n_w)), True)
else:
client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(x_init - x_shift,
y_init - rate * y_shift,
z_init - rate * z_shift),
airsim.Quaternionr(n_qx, n_qy, n_qz, n_w)), True)
elif sk == "Break":
print("Tracking terminating...")
client.simSetPose(
airsim.Pose(airsim.Vector3r(0, 0, 0),
airsim.to_quaternion(0.0, 0, 0)), True)
break
elif sk == 'Empty':
i = 1
client.simSetPose(
airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)),
True)
def setCameraAngle(self, camera_angle, drone, cam=0):
"""
Set camera angle
"""
pos = self.client.simSetCameraOrientation(
cam,
airsim.to_quaternion(camera_angle * math.pi / 180, 0, 0),
vehicle_name=drone) # radians
def airpub():
## Start ROS ---------------------------------------------------------------
rospy.init_node('geo_mapping', anonymous=False)
rate = rospy.Rate(10)
## Publishers --------------------------------------------------------------
# image publishers
depth_pub = rospy.Publisher("airsim/depth", Image, queue_size=1)
# camera paramters publisher
rgb_cam_pub = rospy.Publisher("airsim/camera_info",
CameraInfo,
queue_size=1)
depth_cam_pub = rospy.Publisher("airsim/depth/camera_info",
CameraInfo,
queue_size=1)
# odometry publisher
odom_pub = rospy.Publisher("odom", Odometry, queue_size=1)
# pose publisher
pose_pub = rospy.Publisher("airsim/pose", PoseStamped, queue_size=1)
# curent position publisher
current_pose_pub = rospy.Publisher("airsim/current_pose",
Vector3,
queue_size=1)
## Main --------------------------------------------------------------------
# connect to the AirSim simulator
client = airsim.CarClient()
client.confirmConnection()
client.enableApiControl(True)
client.simSetCameraOrientation(0, airsim.to_quaternion(0, 0, 0))
# client.simSetCameraOrientation(0, airsim.to_quaternion(-math.pi/2, 0, 0))
while not rospy.is_shutdown():
camera_info_msg = get_camera_params()
simPose = get_sim_pose(client)
odom_msg = convert_posestamped_to_odom_msg(simPose)
rgb_msg, depth_msg = get_image_messages(client)
current_pose = get_curr_pose(client)
# header message
simPose.header.stamp = rospy.Time.now()
odom_msg.header.stamp = simPose.header.stamp
camera_info_msg.header.stamp = simPose.header.stamp
depth_msg.header = camera_info_msg.header
# publish message
current_pose_pub.publish(current_pose)
pose_pub.publish(simPose)
publish_tf_msg(simPose)
odom_pub.publish(odom_msg)
depth_cam_pub.publish(camera_info_msg)
depth_pub.publish(depth_msg)
# log PoseStamped message
rospy.loginfo(simPose)
# sleeps until next cycle
rate.sleep()
def start(self):
client = airsim.VehicleClient('', self.port)
client.confirmConnection()
print("1111111111111111")
client.simSetCameraOrientation(
"0", airsim.to_quaternion(0.261799 * 3, 0, 0))
print("22222222222222222")
return "complit"
def as_quaternion(self) -> Quaternionr:
""" Returns a quaternion representing the rotation angles.
Note: AirSim's `Quaternionr` expresses coordinates in WXYZ order.
"""
return airsim.to_quaternion(
pitch=self.pitch,
roll=self.roll,
yaw=self.yaw,
)
def reset(self):
self.client.reset()
self.client.enableApiControl(True)
self.client.armDisarm(True)
# Spawn the drone at random position
self.velocity = airsim.Vector3r(0,0,0)
self.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(np.random.uniform(-1,0),np.random.uniform(-1,1),np.random.uniform(-3,-1)), airsim.to_quaternion(0, 0, 0)), True)
self.client.moveByVelocityAsync(self.velocity.x_val, self.velocity.y_val, self.velocity.z_val, 1, airsim.DrivetrainType.MaxDegreeOfFreedom, airsim.YawMode())
time.sleep(0.05)
# Get the direction of the target object
cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position
targetDirection = self.targetPos - cameraPos
if targetDirection.get_length() > 0:
# The default camera pitch
defaultDirection = airsim.Vector3r(0,0,1)
# Calculate the angle between the two
pitchTheta = np.arccos(targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length()))
#targetDirection.z_val = 0 # for yaw vector
# The default camera yaw
defaultDirection = airsim.Vector3r(1,0,0)
# Calculate the angle between the two
yawTheta = np.arccos(targetDirection.dot(defaultDirection) / (targetDirection.get_length() * defaultDirection.get_length()))
if targetDirection.y_val < defaultDirection.y_val:
yawTheta = -yawTheta
else:
pitchTheta = 0
yawTheta = 0
# Set camera pitch
self.qCam = airsim.to_quaternion(pitchTheta, 0, 0)
# Set camera yaw
self.qCam = airsim.to_quaternion(0, yawTheta, 0) * self.qCam
self.client.simSetCameraOrientation("bottom_center", self.qCam)
self.start_time = time.time()
#self._move_to_target()
return self._observe()
def reset(self):
self.client.reset()
self.client.enableApiControl(True)
self.client.armDisarm(True)
# Move the currently active TargetActor to its initial pose
self.client.simSetObjectPose("TargetActor" + str(self.activeMesh),
self.initialPose[self.activeMesh], True)
# Pick a new TargetActor for this episode
self.activeMesh = np.random.randint(0, self.numPeople - 4) # Train
#self.activeMesh = np.random.randint(self.numPeople-4,self.numPeople) # Test
#self.activeMesh = 4
# Change TargetActor pose
self._move_target("TargetActor" + str(self.activeMesh))
# Spawn the drone at random position around frontal position
offset = airsim.Vector3r(np.random.uniform(-1, 1),
np.random.uniform(0, 0.5),
np.random.uniform(-0.2, 0.1))
#offset = airsim.Vector3r(0, 0, -0.06)
ori = self._safe_simGetObjectPose("TargetActor" +
str(self.activeMesh)).orientation
offset = self._transform_to_frame(offset, ori)
dronePos = self.frontalPos + offset
self.client.simSetVehiclePose(
airsim.Pose(dronePos, airsim.to_quaternion(0, 0, 0)), True)
self.client.moveByVelocityZAsync(0, 0, dronePos.z_val,
self.duration).join()
time.sleep(0.05)
# Look at target direction
self._look_at_target()
self.client.simSetCameraOrientation("bottom_center",
airsim.to_quaternion(1.40, 0, 0))
# Reset variables
self.targetReached = 0
self.start_time = time.time()
return self._observe()
def __init__(self, control_mode, max_steering, max_throttle, max_brakes):
""" Initializes the AISGame object.
Args:
control_mode (string): The control mode to used with the joystick.
"""
self.client = airsim.CarClient()
self.client.confirmConnection()
self.client.enableApiControl(True)
# Camera 2 facing left or right? (negative for left)
cam_dir = -1
# Change camera direction (in radians)
self.client.simSetCameraOrientation(
1, airsim.to_quaternion(0, 0, cam_dir * 0.523599))
# self.client.simSetCameraOrientation(2,
# airsim.to_quaternion(0,
# 0,
# -0.523599))
# Internally represents the vehicle control state
self.vehicle_controls = VehicleControl(control_mode, max_steering,
max_throttle, max_brakes)
self._timer = None
self.save_timer = None
self._display = None
self._main_image = None
self._seg_image = None
self._is_on_reverse = False
self._position = None
self.counter = 0
self.color_map = {}
self.val_map = {}
self.set_segmentation_ids()
self.recording = False
self.request_stop_recording = False
self.record_path = None
self.save_counter = 0
self.csv_data = []
self.last_pos = np.zeros(3)
pygame.joystick.init()
self.joysticks = [
pygame.joystick.Joystick(x)
for x in range(pygame.joystick.get_count())
]
for j in self.joysticks:
j.init()
print("Found %d joysticks" % (len(self.joysticks)))
def airpub():
## Start ROS ---------------------------------------------------------------
rospy.init_node('airsim_img_publisher', anonymous=False)
loop_rate = rospy.get_param('~loop_rate', 10)
rate = rospy.Rate(loop_rate)
## Publishers --------------------------------------------------------------
# image publishers
rgb_pub = rospy.Publisher("airsim/rgb/image_raw", Image, queue_size=1)
depth_pub = rospy.Publisher("airsim/depth", Image, queue_size=1)
# camera paramters publisher
rgb_cam_pub = rospy.Publisher("airsim/camera_info",
CameraInfo,
queue_size=1)
depth_cam_pub = rospy.Publisher("airsim/depth/camera_info",
CameraInfo,
queue_size=1)
# odometry publisher
odom_pub = rospy.Publisher("odom", Odometry, queue_size=1)
# pose publisher
pose_pub = rospy.Publisher("airsim/pose", PoseStamped, queue_size=1)
## Main --------------------------------------------------------------------
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.simSetCameraOrientation(0, airsim.to_quaternion(0, 0, 0))
# client.simSetCameraOrientation(0, airsim.to_quaternion(-math.pi/2, 0, 0))
while not rospy.is_shutdown():
camera_info_msg = get_camera_params()
sim_pose_msg = get_sim_pose(client)
odom_msg = convert_posestamped_to_odom_msg(sim_pose_msg)
rgb_msg, depth_msg = get_image_messages(client)
# header message
sim_pose_msg.header.stamp = rospy.Time.now()
odom_msg.header.stamp = sim_pose_msg.header.stamp
camera_info_msg.header.stamp = sim_pose_msg.header.stamp
rgb_msg.header = camera_info_msg.header
depth_msg.header = camera_info_msg.header
# publish message
pose_pub.publish(sim_pose_msg)
publish_tf_msg(sim_pose_msg)
odom_pub.publish(odom_msg)
rgb_cam_pub.publish(camera_info_msg)
depth_cam_pub.publish(camera_info_msg)
rgb_pub.publish(rgb_msg)
depth_pub.publish(depth_msg)
rate.sleep()
def _take_action(self, action):
if action == 0:
# Do nothing
pass
elif action == 1:
# Tilt drone camera up
qRot = airsim.to_quaternion(0.0872, 0, 0)
self.qCam = self.qCam * qRot
elif action == 2:
# Tilt drone camera down
qRot = airsim.to_quaternion(-0.0872, 0, 0)
self.qCam = self.qCam * qRot
elif action == 3:
# Pan drone camera left
qRot = airsim.to_quaternion(0, -0.0872, 0)
self.qCam = qRot * self.qCam
elif action == 4:
# Pan drone camera right
qRot = airsim.to_quaternion(0, 0.0872, 0)
self.qCam = qRot * self.qCam
self.client.simSetCameraOrientation("bottom_center", self.qCam)
def OrbitAnimal(cx, cy, radius, speed, altitude, camera_angle, animal):
"""
@param cx: The x position of our orbit starting location
@param cy: The x position of our orbit starting location
@param radius: The radius of the orbit circle
@param speed: The speed the drone should more, it's hard to take photos when flying fast
@param altitude: The altidude we want to fly at, dont fly too high!
@param camera_angle: The angle of the camera
@param animal: The name of the animal, used to prefix the photos
"""
x = cx - radius
y = cy
# set camera angle
client.simSetCameraOrientation(0, airsim.to_quaternion(
camera_angle * math.pi / 180, 0, 0)) # radians
# move the drone to the requested location
print("moving to position...")
client.moveToPositionAsync(
x, y, z, 1, 60, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(False, 0)).join()
pos = client.getMultirotorState().kinematics_estimated.position
dx = x - pos.x_val
dy = y - pos.y_val
yaw = airsim.to_eularian_angles(
client.getMultirotorState().kinematics_estimated.orientation)[2]
# keep the drone on target, it's windy out there!
print("correcting position and yaw...")
while abs(dx) > 1 or abs(dy) > 1 or abs(yaw) > 0.1:
client.moveToPositionAsync(
x, y, z, 0.25, 60, drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom, yaw_mode=airsim.YawMode(False, 0)).join()
pos = client.getMultirotorState().kinematics_estimated.position
dx = x - pos.x_val
dy = y - pos.y_val
yaw = airsim.to_eularian_angles(
client.getMultirotorState().kinematics_estimated.orientation)[2]
print("yaw is {}".format(yaw))
print("location is off by {},{}".format(dx, dy))
o = airsim.to_eularian_angles(
client.getMultirotorState().kinematics_estimated.orientation)
print("yaw is {}".format(o[2]))
# let's orbit around the animal and take some photos
nav = drone_orbit.OrbitNavigator(photo_prefix=animal, radius=radius, altitude=altitude, speed=speed, iterations=1, center=[
cx - pos.x_val, cy - pos.y_val], snapshots=30, image_dir="./drone_images/")
nav.start()
# https://github.com/Microsoft/AirSim/blob/master/docs/image_apis.md#computer-vision-mode
import setup_path
import airsim
import pprint
import os
import time
pp = pprint.PrettyPrinter(indent=4)
client = airsim.VehicleClient()
client.confirmConnection()
airsim.wait_key('Press any key to set camera-0 gimble to 15-degree pitch')
client.simSetCameraOrientation("0", airsim.to_quaternion(0.261799, 0, 0)); #radians
airsim.wait_key('Press any key to get camera parameters')
for camera_name in range(5):
camera_info = client.simGetCameraInfo(str(camera_name))
print("CameraInfo %d: %s" % (camera_name, pp.pprint(camera_info)))
airsim.wait_key('Press any key to get images')
for x in range(3): # do few times
z = x * -20 - 5 # some random number
client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(z, z, z), airsim.to_quaternion(x / 3.0, 0, x / 3.0)), True)
responses = client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.DepthVis),
airsim.ImageRequest("1", airsim.ImageType.DepthPerspective, True),
airsim.ImageRequest("2", airsim.ImageType.Segmentation),
def moveUAV(client,pos,yaw):
client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(pos[0], pos[1], pos[2]), airsim.to_quaternion(0, 0, yaw)), True)
camera_info = client.simGetCameraInfo(str(camera_id))
print("CameraInfo %d: %s" % (camera_id, pp.pprint(camera_info)))
airsim.wait_key('Press any key to get images')
tmp_dir = os.path.join(tempfile.gettempdir(), "airsim_drone")
print ("Saving images to %s" % tmp_dir)
try:
for n in range(3):
os.makedirs(os.path.join(tmp_dir, str(n)))
except OSError:
if not os.path.isdir(tmp_dir):
raise
for x in range(50): # do few times
#xn = 1 + x*5 # some random number
client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(x, 0, -2), airsim.to_quaternion(0, 0, 0)), True)
time.sleep(0.1)
responses = client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.Scene),
airsim.ImageRequest("1", airsim.ImageType.Scene),
airsim.ImageRequest("2", airsim.ImageType.Scene)])
for i, response in enumerate(responses):
if response.pixels_as_float:
print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_float), pprint.pformat(response.camera_position)))
airsim.write_pfm(os.path.normpath(os.path.join(tmp_dir, str(x) + "_" + str(i) + '.pfm')), airsim.get_pfm_array(response))
else:
print("Type %d, size %d, pos %s" % (response.image_type, len(response.image_data_uint8), pprint.pformat(response.camera_position)))
airsim.write_file(os.path.normpath(os.path.join(tmp_dir, str(i), str(x) + "_" + str(i) + '.png')), response.image_data_uint8)
import setup_path
import airsim
import time
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)
# MultirotorClient.wait_key('Press any key to takeoff')
print("Taking off")
client.takeoffAsync().join()
print("Ready")
for i in range(5):
client.moveToPositionAsync(float(-50.00), float( 50.26), float( -20.58), float( 3.5))
time.sleep(6)
client.simSetCameraOrientation("0", airsim.to_quaternion(0.5, 0.5, 0.1))
client.moveToPositionAsync(float(50.00), float( -50.26), float( -10.58), float( 3.5))
time.sleep(6)
client.simSetCameraOrientation("0", airsim.to_quaternion(-0.5, -0.5, -0.1))
client.reset()
airsim.wait_key('Press any key to set skin age to 1')
client.simCharSetSkinAgeing(1)
airsim.wait_key('Press any key to set skin color to 0.9')
client.simCharSetSkinDarkness(0.9)
#airsim.wait_key('Press any key to set face expression')
#client.simCharSetFaceExpression("BlendShapeNode_Smile", 1);
airsim.wait_key('Press any key to set bone pose')
client.reset()
jaw_pose = airsim.Pose()
jaw_pose.position = airsim.Vector3r(0.002, 0.001, -0.003)
jaw_pose.orientation = airsim.to_quaternion(0, 0, -.15)
client.simCharSetBonePose( "Jaw", jaw_pose);
airsim.wait_key('Press any key to set preset')
client.reset()
for x in range(0, 10, 3):
client.simCharSetFacePreset("FACS_0" + str(x), 5);
time.sleep(1)
airsim.wait_key('Press any key to set multiple presets')
presets = {"Phoneme_l":0.5, "Phoneme_ae": 1, "Phoneme_ooo":0.0}
client.simCharSetFacePresets(presets)
airsim.wait_key('Press any key to turn head around')
client.reset()
for pitch in range(-5, 5, 5):