def test_calcular_pose_relativa(self):
pose = self.dron.calcularPoseRelativa(5., 0., 0., self.pose0)
self.assertPose(airsim.Pose(airsim.Vector3r(5, 0, 0)), pose)
pose = self.dron.calcularPoseRelativa(5., 90, 0., self.pose0)
self.assertPose(airsim.Pose(airsim.Vector3r(0, 5, 0)), pose)
pose = self.dron.calcularPoseRelativa(5., 90, 90., self.pose0)
self.assertPose(airsim.Pose(airsim.Vector3r(0, 0, 5)), pose)
def setup_my_cameras(self, client):
"""
Helper function to set the left, right, forward, and back cameras up
on the vehicle as I've see fit.
:param client: airsim.CarClient() object that
already connected to the sim
:returns: nada
"""
# pitch, roll, yaw ; each is in radians where
# 15 degrees = 0.261799 (don't ask me why they used radians...)
# 10 degrees = 0.174533, 60 degrees = 1.0472, and 180 degrees = 3.14159
# reason for +- 0.70: forward camera FOV is 90 degrees or 1.57 radians;
# 0.7 is roughly half that and seem to work well, so I'm sticking with it
# NOTE: these images are reflected over a vertical line: left in the image
# is actually right in the simulation...should be ok for CNN since it is
# spatially invariant, but if not, then come back and change these
self.client.simSetCameraOrientation(self.left_cam_name,
airsim.Vector3r(0.0, 0.0, -0.68))
self.client.simSetCameraOrientation(self.right_cam_name,
airsim.Vector3r(0.0, 0.0, 0.68))
self.client.simSetCameraOrientation(self.forward_cam_name,
airsim.Vector3r(0.0, 0.0, 0.0))
self.client.simSetCameraOrientation(self.backward_cam_name,
airsim.Vector3r(0.0, 0.0, 11.5))
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 getStartPos(self, json_data):
if self.player_name != "":
json_car = json_data["Vehicles"][self.player_name]
pos_setting = airsim.Vector3r(float(json_car["X"]), float(json_car["Y"]), 0)
else:
pos_setting = airsim.Vector3r(0, 0, 0)
return pos_setting
def _get_reward(self):
done = False
cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position
# Camera reward
# Get target direction
targetDirection = self.targetPos - cameraPos
# Get current camera direction vector
defaultDirection = airsim.Vector3r(1,0,0)
defaultDirection = defaultDirection.to_Quaternionr()
ori = self.client.simGetCameraInfo("bottom_center").pose.orientation
lookDirection = ori.conjugate() * defaultDirection * ori
lookDirection = airsim.Vector3r(lookDirection.x_val,-lookDirection.y_val,-lookDirection.z_val)
# Calculate the angle between the two
theta = np.arccos(targetDirection.dot(lookDirection) / (targetDirection.get_length() * lookDirection.get_length()))
# Calculate final reward
reward = (0.4 - theta)
# Detect target reached
if self.episode_duration < (time.time()-self.start_time) and theta < 0.2:
reward = 100
#done = True
# Detect out of bounds
if theta > 0.7:
reward = -100
done = True
# Detect collision
if self.client.simGetCollisionInfo().object_id != -1:
#reward = -100
done = True
return reward, done
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
if args.verbose:
print(
f"[ff] HomeGeoPoint: {Vec3.from_GeoPoint(client.getHomeGeoPoint())}\n"
)
print(
f"[ff] VehiclePose.position: {Vec3.from_Vector3r(client.simGetVehiclePose().position)}\n"
)
initial_state = client.getMultirotorState()
if initial_state.landed_state == airsim.LandedState.Landed:
print(f"[ff] Taking off")
client.takeoffAsync(timeout_sec=8).join()
else:
client.hoverAsync().join() # airsim.LandedState.Flying
#__move_on_path(client, args.flight_path, args.flight_velocity)
#__move_on_box(client, z=-20, side=20, velocity=args.flight_velocity)
if not args.use_viewpoints:
future = client.moveOnPathAsync([
airsim.Vector3r(coord.x, coord.y, coord.z)
for coord in args.flight_path
], args.flight_velocity)
else:
import viewpoints
future = client.moveOnPathAsync(
[airsim.Vector3r(*position) for position in viewpoints.Positions],
args.flight_velocity)
print(f"[ff] Press [space] to take pictures")
ch, img_count, img_responses = msvcrt.getch(), 0, []
while ch == b' ':
img_responses.extend(
client.simGetImages([
airsim.ImageRequest(
ff.CameraName.bottom_center,
airsim.ImageType.Scene,
False,
True # compressed PNG image
)
]))
img_count += 1
print(f" {img_count} pictures taken", end="\r")
ch = msvcrt.getch()
print()
print(f"[ff] Waiting for drone to finish path...", end=" ", flush=True)
future.join()
print(f"done.")
for i, response in enumerate(img_responses):
airsim.write_file(os.path.join(args.output_folder, f"out_{i}.png"),
response.image_data_uint8)
time.sleep(4)
print(f"[ff] Drone reset")
client.reset()
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 _get_reward(self, action):
# Drone reward
cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position
# Calculate Euclidean distance from target position
dist = self.frontalPos.distance_to(cameraPos)
# Camera reward
# Get target direction
targetDirection = self.targetPos - cameraPos
# Get current camera direction vector
defaultDirection = airsim.Vector3r(1, 0, 0)
defaultDirection = defaultDirection.to_Quaternionr()
ori = self.client.simGetCameraInfo("bottom_center").pose.orientation
lookDirection = ori.conjugate() * defaultDirection * ori
lookDirection = airsim.Vector3r(lookDirection.x_val,
-lookDirection.y_val,
-lookDirection.z_val)
# Calculate the angle between the two
theta = np.arccos(
targetDirection.dot(lookDirection) /
(targetDirection.get_length() * lookDirection.get_length()))
# Calculate reward
done = False
reward = (1 - dist)
# Detect target reached
if dist < 0.5 and theta < 0.2:
reward = reward + 100
action_clipped = np.clip(action, [
-self.maxSpeed, -self.maxSpeed, -self.maxSpeed, -self.maxAngle,
-self.maxAngle
], [
+self.maxSpeed, +self.maxSpeed, +self.maxSpeed, +self.maxAngle,
+self.maxAngle
])
if np.array_equal(action, action_clipped) == False:
reward = -100
done = True
# Detect out of bounds
if dist > 2 or theta > 0.7:
reward = -100
done = True
# Detect collision
if self.client.simGetCollisionInfo().object_id != -1:
reward = -100
done = True
return reward, done
def moveAngulo(self, angulo, z=10):
self.client.moveOnPathAsync(
[airsim.Vector3r(0, -253, z),
airsim.Vector3r(125, -253, z),
airsim.Vector3r(125, 0, z),
airsim.Vector3r(0, 0, z),
airsim.Vector3r(0, 0, -20)],
12,
120,
airsim.DrivetrainType.ForwardOnly,
airsim.YawMode(False, 0),
20,
1).join()
self.client.moveToPositionAsync(0, 0, z, 1).join()
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 fly(client: airsim.MultirotorClient, args) -> None:
if args.verbose:
print(f"[ff] HomeGeoPoint:\n{client.getHomeGeoPoint()}\n")
print(f"[ff] VehiclePose:\n{client.simGetVehiclePose()}\n")
#print(f"[ff] MultirotorState:\n{client.getMultirotorState()}\n")
home_UE4 = Vec3.from_GeoPoint(client.getHomeGeoPoint())
ground_offset_NED = Vec3.from_Vector3r(client.simGetVehiclePose().position)
#assert ground_offset_NED.x == ground_offset_NED.y == 0 # assumes the drone is at PlayerStart
#player_start = Vec3.from_Vector3r(
# client.simGetObjectPose(client.simListSceneObjects("PlayerStart.*")[0]).position
#)
print(f"[ff] Taking off")
client.takeoffAsync(timeout_sec=8).join()
fligh_path = BLOCKS_PATH # TODO add option to change on argparse
print(f"[ff] Moving on path...", flush=True)
client.moveOnPathAsync(path=[
airsim.Vector3r(coord.x, coord.y, coord.z) for coord in fligh_path
],
velocity=5).join()
print(f"[ff] Landing", flush=True)
client.landAsync().join()
# print(f"[ff] Going home", flush=True)
# client.goHomeAsync().join()
print(f"[ff] Done")
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, 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 takeAction(self, action):
position = self.client.simGetVehiclePose().position
yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)[2]
if action == 0:
yaw_change = 0.0
elif action == 1:
yaw_change = 30.0
elif action == 2:
yaw_change = -30.0
action_yaw = yaw + yaw_change
vx = math.cos(action_yaw)
vy = math.sin(action_yaw)
v_norm = np.linalg.norm([vx, vy])
vx = vx / v_norm * self.MOVE_RATE
vy = vy / v_norm * self.MOVE_RATE
new_x = position.x_val + vx
new_y = position.y_val + vy
new_pose = airsim.Pose(airsim.Vector3r(new_x, new_y, self.HEIGHT),
airsim.utils.to_quaternion(0, 0, action_yaw))
self.client.simSetVehiclePose(new_pose, False)
# time.sleep(self.CV_SLEEP_TIME)
collided = self.checkCollision()
return collided
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 fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
initial_pose = client.simGetVehiclePose()
initial_state = client.getMultirotorState()
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
if initial_state.landed_state == airsim.LandedState.Landed:
print("[ff] Taking off")
client.takeoffAsync(timeout_sec=8).join()
# else:
# client.hoverAsync().join() # airsim.LandedState.Flying
path = [airsim.Vector3r(*position) for position, _orientation in args.viewpoints]
future = client.moveOnPathAsync(
path,
velocity=2,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=-1.5), # FIXME
)
_take_pictures_loop(client)
future.join()
client.reset()
print("[ff] Drone reset")
def convert_ned_to_enu(pos_ned, orientation_ned):
pos_enu = airsim.Vector3r(pos_ned.x_val, -pos_ned.y_val, -pos_ned.z_val)
orientation_enu = airsim.Quaternionr(orientation_ned.w_val,
-orientation_ned.z_val,
orientation_ned.x_val,
orientation_ned.y_val)
return pos_enu, orientation_ned
def _get_reward(self):
#time.sleep(0.7)
done = False
# Drone reward
cameraPos = self.client.simGetCameraInfo("bottom_center").pose.position
frontalPos = self.targetPos - airsim.Vector3r(1, 0, 0.4)
# Calculate Euclidean distance from target position
dist = frontalPos.distance_to(cameraPos)
# Calculate final reward
reward = (1 - dist)
# Detect target reached
if dist < 0.5:
reward = 100
done = True
# Detect out of bounds
if dist > 2:
reward = -100
done = True
# Detect collision
if self.client.simGetCollisionInfo().object_id != -1:
reward = -100
done = True
return reward, done
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 __init__(self, ip_address, control_type, step_length, image_shape,
goal):
super().__init__(image_shape)
self.step_length = step_length
self.control_type = control_type
self.image_shape = image_shape
self.goal = airsim.Vector3r(goal[0], goal[1], goal[2])
if self.control_type is 'discrete':
self.action_space = spaces.Discrete(7)
if self.control_type is 'continuous':
self.action_space = spaces.Box(low=-5, high=5, shape=(3, ))
else:
print(
"Must choose a control type {'discrete','continuous'}. Defaulting to discrete."
)
self.action_space = spaces.Discrete(7)
self.state = {"position": np.zeros(3), "collision": False}
self.drone = airsim.MultirotorClient(ip=ip_address)
self._setup_flight()
self.image_request = airsim.ImageRequest('front_center',
airsim.ImageType.Scene, False,
False)
def fly(client: airsim.MultirotorClient, args: argparse.Namespace) -> None:
initial_pose = client.simGetVehiclePose()
if args.verbose:
ff.print_pose(initial_pose, airsim.to_eularian_angles)
points, poses, names = [], [], []
for position, orientation in args.viewpoints:
position = airsim.Vector3r(*position)
orientation = airsim.Quaternionr(*orientation) # xyzw
points.append(position)
poses.append(airsim.Pose(position, orientation))
names.append(
ff.to_xyz_str(position) + "\n" + ff.to_xyzw_str(orientation))
plot_linestrips = True # else plot_transforms
curr_press_keys = set()
key_combination = {keyboard.Key.space, keyboard.Key.shift}
def on_press(key):
nonlocal plot_linestrips, curr_press_keys, key_combination
if key in key_combination:
curr_press_keys.add(key)
if curr_press_keys == {keyboard.Key.space}:
if (plot_linestrips := not plot_linestrips):
client.simPlotLineStrip(points, thickness=3, duration=10)
else:
client.simPlotTransforms(poses,
thickness=3,
scale=40,
duration=10)
def _transform_to_frame(self, vector, q):
# Create a pure quaternion p out of vector
vector = vector.to_Quaternionr()
# q is the vector's orientation with regard to the world frame
# Pre-multiply vector with q and post-multiply it with the conjugate q*
qv = q * vector * q.conjugate()
return airsim.Vector3r(qv.x_val, qv.y_val, qv.z_val)
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 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 add_wind(self):
w1 = np.random.randint(0, 3)
w2 = np.random.randint(0, 3)
w3 = np.random.randint(0, 3)
wind = airsim.Vector3r(w1, w2, w3)
print(f'add wind vector = {wind}')
self.cl.simSetWind(wind)
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 move_on_path(self, args: list):
print("MoveOnPath received")
if len(args) % 3 != 2:
print("Move needs 3 args per position args")
return
# Have to make sure it is enabled:
self.client.enableApiControl(True)
iterations = (len(args) - 2) / 3
path = []
for i in range(int(iterations)):
point = airsim.Vector3r(float(args[(i * 3) + 1]),
float(args[(i * 3) + 2]),
float(args[(i * 3) + 3]))
path.append(point)
if self.verbatim:
print("path point added", str(point))
try:
result = self.client.moveOnPathAsync(path, float(args[-1]), 120,
airsim.DrivetrainType.ForwardOnly, airsim.YawMode(False, 0),
20,
1).join()
except:
errorType, value, traceback = airsim.sys.exc_info()
print("moveOnPath threw exception: " + str(value))
pass
self.client.hoverAsync().join()
print("Path moved!")
def geodeticToNedFast(cls, geo, home):
d_lat = geo.latitude - home.latitude
d_lon = geo.longitude - home.longitude
d_alt = home.altitude - geo.altitude
x = cls.degreesToRadians(d_lat) * cls._EARTH_RADIUS
y = cls.degreesToRadians(d_lon) * (
cls._EARTH_RADIUS * math.cos(cls.degreesToRadians(geo.latitude)))
return airsim.Vector3r(x, y, d_alt)
def move(self, pos, yaw, offset_x=0, offset_y=0):
# pos Z coordinate is overridden to -1 (or 0, need to test)
self.client.enableApiControl(True, self.name)
# self.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(pos[0]+offset_x, pos[1]+offset_y, -1), airsim.to_quaternion(0, 0, yaw)), True, vehicle_name=self.name)
self.client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(pos[0]+offset_x, pos[1]+offset_y, -1)), True, vehicle_name=self.name)
time.sleep(0.3)
self.client.enableApiControl(False, self.name)
self.client.armDisarm(False, self.name)
def __move_on_path(client: airsim.MultirotorClient, path: List[Vec3],
velocity: float) -> None:
print(f"[ff] Moving on path...", end=" ", flush=True)
client.moveOnPathAsync(
[airsim.Vector3r(coord.x, coord.y, coord.z) for coord in path],
velocity,
).join()
print(f"done.")
