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 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 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 convert_uavmvs_to_airsim_pose(camera: TrajectoryCamera,
translation=None,
scaling=None):
import airsim
assert camera.kind in [TRAJ, CSV]
position = convert_uavmvs_to_airsim_position(camera.position, translation,
scaling)
qw, qx, qy, qz = map(float, camera._rotation_into(CSV))
# qx = -qx # XXX
qy = -qy
qz = -qz
orientation = airsim.Quaternionr(qx, qy, qz, qw)
length = orientation.get_length()
assert 0.99 <= length <= 1.01, orientation
# orientation /= length
# NOTE ignore translation and scaling for orientation (we'd only care for rotation)
pose = airsim.Pose(position, orientation)
del airsim
return pose
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 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 get_reward_status(self) :
reward = 0
is_done = False
# Penalty for collision
if self.is_in_collision :
reward += -5
# Extra Penalty for laziness - not moving or extremely slow to encourage exploration and travel faster
# Speed
if abs(self.current_state[4]) < (1.0/MAX_SPEED) :
reward += -1
# Penalty for driving in reverse to force car to prefer driving forward
if self.car_controls.manual_gear == -1 :
reward += -1
# Intermediate penalty to encourage to reach goal in smaller amount of time
reward += - 1
# Intermediate reward proportional to distance from way point.
tcar_state = self.client.simGetVehiclePose(vehicle_name="TargetCar")
tcar_car_pos = [tcar_state.position.x_val, tcar_state.position.y_val, tcar_state.position.z_val]
dist = self._euclidean_distance(self.car_pos, tcar_car_pos)
reward += ((self.max_goal_distance - dist) / (self.max_goal_distance - self.goal_threshold))
# Intermediate reward for facing the direction of the way point. Inversely penalizes if facing the opposite direction
tcar_yaw = self._get_yaw_angle_rad(self.car_pos, tcar_car_pos)
reward += math.cos( tcar_yaw - airsim.utils.to_eularian_angles(self.client.getCarState().kinematics_estimated.orientation)[2] )
# Check if maximum time steps reached
if self.time_steps_left == 0 :
is_done = True
else :
if dist <= self.goal_threshold :
self.current_goal += 1
# Last way point reached
if self.current_goal >= len(self.goals) :
is_done = True
reward += 25
# Intermediate way point reached
else :
reward += 10
# set to next way point
if self.current_goal < len(self.goals) :
pose = airsim.Pose(self.goals[self.current_goal][0], self.goals[self.current_goal][1])
self.client.simSetVehiclePose(pose, True, vehicle_name="TargetCar")
self.is_done = is_done
print("Goal Distance:" + str(dist))
self.goal_distance = dist
return reward, is_done
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
class TestDroneController(TestCase):
pose0 = airsim.Pose()
cliente = DatabaseGenerator.crearCliente()
dron = DroneController.DroneController('Drone1', cliente)
def assertPose(self, poseEsperada, pose):
self.assertAlmostEqual(poseEsperada.position.x_val,
pose.position.x_val, delta=1e-10)
self.assertAlmostEqual(poseEsperada.position.y_val,
pose.position.y_val, delta=1e-10)
self.assertAlmostEqual(poseEsperada.position.z_val,
pose.position.z_val, delta=1e-10)
self.assertAlmostEqual(poseEsperada.orientation.w_val,
pose.orientation.w_val, delta=1e-10)
self.assertAlmostEqual(poseEsperada.orientation.x_val,
pose.orientation.x_val, delta=1e-10)
self.assertAlmostEqual(poseEsperada.orientation.y_val,
pose.orientation.y_val, delta=1e-10)
self.assertAlmostEqual(poseEsperada.orientation.z_val,
pose.orientation.z_val, delta=1e-10)
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 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 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 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 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 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 setPosition(self, point, direction):
self.car_controls.position = airsim.Vector3r(point[0], point[1], 0)
self.car_controls.heading = airsim.utils.to_quaternion(0, 0, direction)
self.car_controls.pose = airsim.Pose(self.car_controls.position,
self.car_controls.heading)
self.client.simSetVehiclePose(self.car_controls.pose,
True,
vehicle_name=self.car_name)
def get_reward_status(self):
reward = 0
is_done = False
# Penalty for collision
if self.is_in_collision:
reward += (-1 * 10)
# Penalty for driving in reverse to force car to prefer driving forward
if self.car_controls.manual_gear == -1:
reward += (-0.1 * 20)
# Penalty for stepping for braking
if self.car_controls.brake >= 0.4:
reward += (-0.1 * 20)
# Intermediate reward for movign towards the way point
tcar_state = self.client.simGetVehiclePose(vehicle_name="TargetCar")
tcar_car_pos = [
tcar_state.position.x_val, tcar_state.position.y_val,
tcar_state.position.z_val
]
dist = self._euclidean_distance(self.car_pos, tcar_car_pos)
dist_oldPos = self._euclidean_distance(self.car_pos,
self.pose_waypoint)
reward += ((self.max_goal_distance - dist) * 20 /
(self.max_goal_distance - self.goal_threshold))
reward += (-1 * (100 - dist_oldPos) / 100 * 10)
if self.time_steps_left == 0:
is_done = True
else:
if dist <= self.goal_threshold:
self.current_goal += (1 * 10)
# Last way point reached
if self.current_goal >= len(self.goals):
is_done = True
# reward for arriving faster
reward += (self.time_steps_left * 10)
else:
# set to next way point
pose = airsim.Pose(self.goals[self.current_goal][0],
self.goals[self.current_goal][1])
self.client.simSetVehiclePose(pose,
True,
vehicle_name="TargetCar")
self.pose_waypoint = pose
self.is_done = is_done
print("Goal Distance:" + str(dist))
self.goal_distance = dist
return reward, is_done
def location(client):
return client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(x_val=71.37133026123047,
y_val=173.48350524902344,
z_val=-0.6527218222618103),
airsim.Quaternionr(w_val=0.7069156765937805,
x_val=2.5558463676134124e-05,
y_val=-1.7646530977799557e-05,
z_val=-0.7072978019714355)), True)
def location(client):
return client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(x_val=resources.x_location,
y_val=resources.y_location,
z_val=resources.z_location),
airsim.Quaternionr(w_val=resources.w_orientation,
x_val=resources.x_orientation,
y_val=resources.y_orientation,
z_val=resources.z_orientation)), True)
def location(client):
return client.simSetVehiclePose(
airsim.Pose(
airsim.Vector3r(x_val=60.330116271972656,
y_val=-272.38421630859375,
z_val=-0.6507290601730347),
airsim.Quaternionr(w_val=0.7091798782348633,
x_val=4.51675005024299e-05,
y_val=-2.079392288578674e-06,
z_val=0.7050275802612305)), True)
def _move_by_vehicle_poses(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
raise Warning("simSetVehiclePose() is meant for ComputerVision mode")
# NOTE check https://github.com/microsoft/AirSim/pull/2324
for position, orientation in args.viewpoints:
pose = airsim.Pose(airsim.Vector3r(*position),
airsim.Quaternionr(*orientation))
client.simSetVehiclePose(pose, ignore_collision=True)
client.hoverAsync().join()
time.sleep(2)
def initialCarControlSettings(self):
self.car_controls.is_manual_gear=False
self.car_controls.gear_immediate=True
self.client.setCarControls(self.car_controls)
position = airsim.Vector3r(0.0 , 0.0, 0.0)
heading = airsim.utils.to_quaternion(0.0, 0.0, np.random.uniform(0.0,6.5))
pose = airsim.Pose(position, heading)
self.client.simSetVehiclePose(pose, True)
def convert_ned_to_enu(pos_ned, orientation_ned):
position_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)
pose_enu = airsim.Pose(position_enu, orientation_enu)
return pose_enu
def get_image(client, pos, q):
x, y, z = pos
qw, qx, qy, qz = q.elements
client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(x, y, z),
airsim.Quaternionr(qx, qy, qz, qw)), True)
responses = client.simGetImages([
airsim.ImageRequest("0", airsim.ImageType.Scene),
])
return responses[0]
def connect_drone():
print(
'------------------------------ Drone ------------------------------')
client = airsim.MultirotorClient(timeout_value=10)
client.confirmConnection()
old_posit = client.simGetVehiclePose()
client.simSetVehiclePose(airsim.Pose(airsim.Vector3r(0, 0, -2.2),
old_posit.orientation),
ignore_collison=True)
return client, old_posit
def reset_pose(self):
#print ("resetting pose")
x = np.random.uniform(self.x_min, self.x_max)
y = np.random.uniform(self.y_min, self.y_max)
print("New pose: " + str(x) + ',' + str(y))
yaw = np.random.uniform(-np.pi, np.pi)
position = airsim.Vector3r(x, y, self.z_height)
heading = airsim.utils.to_quaternion(self.pitch, self.roll, yaw)
pose = airsim.Pose(position, heading)
self.client.simSetVehiclePose(pose, True) #Set yaw angle
return self.waypoint(x, y, yaw, heading)
def init_way_points(self):
self.current_goal = 0
self.goals = []
# randomize
arr = np.arange(len(self.possible_way_points))
np.random.shuffle(arr)
for i in range(0, self.way_points):
self.goals.append(self.possible_way_points[arr[i]])
# set initial way point
pose = airsim.Pose(self.goals[0][0], self.goals[0][1])
self.client.simSetVehiclePose(pose, True, vehicle_name="TargetCar")
def runSingleCar(id: int):
client = airsim.CarClient()
client.confirmConnection()
vehicle_name = f"Car_{id}"
pose = airsim.Pose(airsim.Vector3r(0, 7 * id, 0),
airsim.Quaternionr(0, 0, 0, 0))
print(f"Creating {vehicle_name}")
success = client.simAddVehicle(vehicle_name, "Physxcar", pose)
if not success:
print(f"Falied to create {vehicle_name}")
return
# Sleep for some time to wait for other vehicles to be created
time.sleep(1)
# driveCar(vehicle_name, client)
print(f"Driving {vehicle_name} for a few secs...")
client.enableApiControl(True, vehicle_name)
car_controls = airsim.CarControls()
# go forward
car_controls.throttle = 0.5
car_controls.steering = 0
client.setCarControls(car_controls, vehicle_name)
time.sleep(3) # let car drive a bit
# Go forward + steer right
car_controls.throttle = 0.5
car_controls.steering = 1
client.setCarControls(car_controls, vehicle_name)
time.sleep(3)
# go reverse
car_controls.throttle = -0.5
car_controls.is_manual_gear = True
car_controls.manual_gear = -1
car_controls.steering = 0
client.setCarControls(car_controls, vehicle_name)
time.sleep(3)
car_controls.is_manual_gear = False # change back gear to auto
car_controls.manual_gear = 0
# apply brakes
car_controls.brake = 1
client.setCarControls(car_controls, vehicle_name)
time.sleep(3)
def connect_drone(ip_address='127.0.0.0', phase='infer'):
print('------------------------------ Drone ------------------------------')
client = airsim.MultirotorClient(ip=ip_address, timeout_value=10)
client.confirmConnection()
old_posit = client.simGetVehiclePose()
if phase == 'train':
client.simSetVehiclePose(
airsim.Pose(airsim.Vector3r(0, 0, 0), old_posit.orientation),
ignore_collison=True)
elif phase == 'infer':
client.enableApiControl(True)
client.armDisarm(True)
client.takeoffAsync().join()
return client, old_posit
