def _move_to(self, target, speed=5.0, yaw=0.0):
"""Due to bugs in controlling the drone(issue 1677 and 1292), the temporal solution is presented."""
target = (Vector3r(target[0], target[1], target[2])
if type(target) == list else target)
self._uav.moveToPositionAsync(
target.x_val,
target.y_val,
target.z_val,
speed,
yaw_mode=airsim.YawMode(False, yaw),
).join()
def take_snapshot(self):
# first hold our current position so drone doesn't try and keep flying while we take the picture.
pos = self.client.getMultirotorState().kinematics_estimated.position
self.client.moveToPositionAsync(pos.x_val, pos.y_val, self.z, 0.5, 10, airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(False, self.camera_heading)).join()
responses = self.client.simGetImages([airsim.ImageRequest(1, airsim.ImageType.Scene)]) #scene vision image in png format
response = responses[0]
filename = "photo_" + str(self.snapshot_index)
self.snapshot_index += 1
airsim.write_file(os.path.normpath(filename + '.png'), response.image_data_uint8)
print("Saved snapshot: {}".format(filename))
self.start_time = time.time() # cause smooth ramp up to happen again after photo is taken.
def move_by_velocity(self, v, duration=1.0):
if self.add_actuation_error:
v = add_gaussian_noise(self.gaussian_error_mean, self.gaussian_error_std, v)
if self.add_v_threshold:
v = limit_velocity(v, self.v_thres)
norm = np.linalg.norm(v)
if norm <= 0.5:
msg = self.vehicle_name + ", velocity magnitude smaller than 0.5, yaw control will not affect attitude."
#warnings.warn(msg)
#print(msg)
return self.client.moveByVelocityAsync(v[0], v[1], v[2], duration, drivetrain=self.drivetrain_type,
yaw_mode=airsim.YawMode(False, 0), vehicle_name=self.vehicle_name)
def flightcircle(self):
print("enter the thread of flightcircle.")
while (True):
# 获取无人机当前位置
state = self.client.simGetGroundTruthKinematics()
pos = np.array([[state.position.x_val], [state.position.y_val],
[state.position.z_val]])
# 计算径向速度的方向向量
dp = pos[0:2] - self.center[0:2]
#np.linalg.norm求的是二范数，正数
if np.linalg.norm(dp) - self.radius > 0.5:
vel_dir_1 = -dp
elif np.linalg.norm(dp) - self.radius < 0.5:
vel_dir_1 = dp
# 计算切向速度的方向向量
theta = math.atan2(dp[1, 0], dp[0, 0])
if self.clock_wise:
theta += math.pi / 2
else:
theta -= math.pi / 2
v_dir_2 = np.array([[math.cos(theta)], [math.sin(theta)]])
# 计算最终速度的方向向量
#0.01这个参数需要自己调整以便使得飞行的圆轨迹更加丝滑
v_dir = 0.01 * vel_dir_1 + v_dir_2
# 计算最终速度指令
v_cmd = self.speed * v_dir / np.linalg.norm(v_dir)
# 速度控制
drivetrain = airsim.DrivetrainType.ForwardOnly
yaw_mode = airsim.YawMode(False, 90)
#self.client.moveByVelocityZAsync(1, 0, self.height, 1, drivetrain=drivetrain, yaw_mode=yaw_mode)
self.client.moveByVelocityZAsync(v_cmd[0, 0],
v_cmd[1, 0],
self.height,
1,
drivetrain=drivetrain,
yaw_mode=yaw_mode).join
#self.client.moveByVelocityZAsync(v_cmd[0, 0], v_cmd[1, 0], self.height, 1)
# 画图
'''
point_reserve = [airsim.Vector3r(self.startpoint[0, 0], self.startpoint[1, 0], self.startpoint[2, 0])]
point = [airsim.Vector3r(pos[0, 0], pos[1, 0], pos[2, 0])]
point_end = pos + np.vstack((v_cmd, np.array([[0]])))
point_end = [airsim.Vector3r(point_end[0, 0], point_end[1, 0], point_end[2, 0])]
self.client.simPlotArrows(point, point_end, arrow_size=8.0, color_rgba=[0.0, 0.0, 1.0, 1.0])
self.client.simPlotLineList(point_reserve+point, color_rgba=[1.0, 0.0, 0.0, 1.0], is_persistent=True)
# 循环
pos_reserve = pos
'''
time.sleep(0.02)
if (self.running == False):
break
def orbit(self, args): # name, speed, x,y
if len(args) < 3:
print("need at least speed parameter and iterations")
return
if len(args) != 4: # Name, x,y
target_x = float(72.38) # X coordinate of turbine 1
target_y = float(48.92) # Y coordinate of turbine 1
self.client.enableApiControl(True)
self.client.moveToPositionAsync(
x=float(36.33),
y=float(24.32),
z=-float(17.33),
velocity=2,
drivetrain=airsim.DrivetrainType.ForwardOnly,
yaw_mode=airsim.YawMode(False, 0)).join()
self.client.hoverAsync().join()
airsim.time.sleep(2)
else:
target_x = float(args[3])
target_y = float(args[4])
speed = float(args[1])
iterations = int(args[2])
for i in range(iterations):
current_pos = self.client.getMultirotorState(
).kinematics_estimated.position
look_at_point = np.array([target_x, target_y])
current_pos_np = np.array([current_pos.x_val, current_pos.y_val])
angle = self.lookAt(look_at_point, np.array([1, 0]))
l = look_at_point - current_pos_np
radius = np.linalg.norm(l)
print("Radius:", radius)
# Have to make sure it is enabled:
self.client.enableApiControl(True)
self.client.rotateToYawAsync(angle, 20, 0).join()
print(self.client.getMultirotorState().kinematics_estimated.
orientation)
self.nav = OrbitNavigator(self.client,
radius=radius,
altitude=float(current_pos.z_val),
speed=speed,
iterations=1,
center=l)
self.nav.start()
print("Orbit ", i, "is done, climb to:", current_pos.x_val,
current_pos.y_val, current_pos.z_val - radius)
self.client.moveToPositionAsync(current_pos.x_val,
current_pos.y_val,
current_pos.z_val - radius, speed,
10).join()
def move(self, x, y, z, v):
"""
move the drone
:param x:
:param y:
:param z:
:param v:
:return:
"""
print("in move")
print(x, y, z, v)
default_yaw_mode = airsim.YawMode(is_rate=False)
self.client.moveToPositionAsync(x, y, z, v, yaw_mode=default_yaw_mode).join()
def _move_by_path(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
path = [
airsim.Vector3r(*position)
for position, _orientation in args.viewpoints
]
future = client.moveOnPathAsync(
path,
args.flight_velocity,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=-1.5), # FIXME
)
return future
def setCameraHeading(self, camera_heading, drone):
"""
Set camera orientation
"""
pos = self.getMultirotorState(drone).kinematics_estimated.position
self.client.moveByVelocityZAsync(
pos.x_val,
pos.y_val,
pos.z_val,
1,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(False, camera_heading),
vehicle_name=drone)
def move_and_change_camera_orientation(self, x, y, z, v, orientation: Quaternionr):
"""
move the drone
:param x:
:param y:
:param z:
:param v:
:param orientation:
:return:
"""
print(x, y, z, v)
default_yaw_mode = airsim.YawMode(is_rate=False)
self.client.moveToPositionAsync(x, y, z, v, yaw_mode=default_yaw_mode).join()
self.set_camera_orientation(orientation)
def move_to_pos(drone_name, x, y, z, yaw, speed):
cur_pos = get_cur_pos(vehicle_name=drone_name)
print("try to move: {} -> {}, yaw={}, speed={}".format(
cur_pos, (x, y, z), yaw, speed))
rc = client.moveToPositionAsync(
x,
y,
z,
speed,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw),
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
vehicle_name=drone_name).join()
cur_pos = get_cur_pos(vehicle_name=drone_name)
print(cur_pos)
def start(survey_path, x, y, z, z_min=-30, z_max=-60, velocity=50, quad=31):
#path = [airsim.Vector3r(i,i,self.z) for i in range(0,self.size,15)]
client = airsim.MultirotorClient()
# client.confirmConnection()
# client.enableApiControl(True)
scale_x = quad * (10 / x)
scale_y = quad * (10 / y)
path = [
airsim.Vector3r(p[0] * scale_x + scale_x / 2,
p[1] * scale_y + scale_y / 2,
z_min + p[2] * (z_max - z_min) / z)
for p in survey_path
]
#print("Started drone_survey...")
# client.armDisarm(True)
# client.takeoffAsync().join()
# landed = client.getMultirotorState().landed_state
# if landed == airsim.LandedState.Landed:
# print("taking off...")
# client.takeoffAsync().join()
# landed = client.getMultirotorState().landed_state
# if landed == airsim.LandedState.Landed:
# print("takeoff failed - check Unreal message log for details")
# return
# client.hoverAsync().join()
# time.sleep(2)
#
# after hovering we need to re-enabled api control for next leg of the trip
#client.enableApiControl(True)
# print ('Path(Unreal Coordinates):',path)
s = 1
try:
# client.simSetCameraOrientation("3", airsim.to_quaternion(0, 0, 0))
for i, p in enumerate(path, start=1):
#import pdb;pdb.set_trace();
# [(0,0,0),(2,2,0)]
# print(p.x_val,p.y_val,p.z_val)
client.moveToPositionAsync(
p.x_val,
p.y_val,
p.z_val,
velocity,
yaw_mode=airsim.YawMode(is_rate=False)).join()
#client.enableApiControl(True)
except:
errorType, value, traceback = sys.exc_info()
print("moveOnPath threw exception: " + str(value))
pass
def step(self, action_id):
self.client.enableApiControl(True)
self.client.armDisarm(True)
self.steps += 1
action = self.id2action(action_id)
if self.stepped_simulation:
self.client.simPause(False)
if self.drone:
# transform velocity according to the drone's orientation.... there is probably a way to do this more elegantly
yaw = airsim.to_eularian_angles(
self.client.simGetVehiclePose().orientation)[2]
v_x = (action[DroneActIdx.VX.value] * np.cos(yaw) -
action[DroneActIdx.VY.value] * np.sin(yaw))
v_y = (action[DroneActIdx.VX.value] * np.sin(yaw) +
action[DroneActIdx.VY.value] * np.cos(yaw))
self.client.moveByVelocityAsync(
v_x,
v_y,
action[DroneActIdx.VZ.value],
self.step_duration,
yaw_mode=airsim.YawMode(
is_rate=True,
yaw_or_rate=action[DroneActIdx.Yaw.value])).join()
# self.client.moveByAngleThrottleAsync(action[DroneActIdx.Pitch.value], action[DroneActIdx.Roll.value],
# action[DroneActIdx.Throttle.value], action[DroneActIdx.Yaw.value],
# self.step_duration).join()
else:
self.client.moveByVelocity(action[CarActIdx.VX.value],
action[CarActIdx.VY.value],
action[CarActIdx.VZ.value],
self.step_duration).join()
if self.stepped_simulation:
self.client.simContinueForTime(self.step_duration)
state = self._get_state()
step_reward = self._reward_function(state)
terminal = self._terminal_state(state)
self.acc_reward += step_reward
return state, step_reward, terminal
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 moveByVelocity(self, pos_msg):
self.client.enableApiControl(True, self.name)
self.client.armDisarm(True, self.name)
if pos_msg.linear.z != 0:
print(pos_msg)
return self.client.moveByVelocityAsync(
pos_msg.linear.x,
pos_msg.linear.y,
pos_msg.linear.z,
.25,
airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(False, 0),
vehicle_name=self.name)
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:
# Drone forward
self.velocity = airsim.Vector3r(self.speed, 0, 0)
elif action == 2:
# Drone backwards
self.velocity = airsim.Vector3r(-self.speed, 0, 0)
elif action == 3:
# Drone left
self.velocity = airsim.Vector3r(0, -self.speed, 0)
elif action == 4:
# Drone right
self.velocity = airsim.Vector3r(0, self.speed, 0)
elif action == 5:
# Drone up
self.velocity = airsim.Vector3r(0, 0, -self.speed / 2)
elif action == 6:
# Drone down
self.velocity = airsim.Vector3r(0, 0, self.speed / 2)
self.client.moveByVelocityAsync(
self.velocity.x_val, self.velocity.y_val, self.velocity.z_val,
self.duration, airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode()).join()
self.velocity = airsim.Vector3r(0, 0, 0)
self.client.moveByVelocityAsync(
self.velocity.x_val, self.velocity.y_val, self.velocity.z_val,
self.duration, airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode()).join()
def move_drone(drone_name, dx, dy, dz, yaw=0):
print_pos(drone_name)
# move the drong
cur_state = client.simGetGroundTruthKinematics(vehicle_name=drone_name)
cur_pos = cur_state.position
next_pos = airsim.Vector3r(cur_pos.x_val + dx, cur_pos.y_val + dy, cur_pos.z_val + dz)
log.info("try to move: {} -> {}".format(cur_pos, next_pos))
rc = client.moveToPositionAsync(
next_pos.x_val, next_pos.y_val, next_pos.z_val, 1,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw),
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
vehicle_name=drone_name).join()
print_pos(drone_name)
def move(self, z, x, y, duration=0.2, desired_velocity=1):
"""
Basic move command which just moves the drone forward
"""
vx, vy, _ = self.transformToEarthFrame([desired_velocity * x, desired_velocity * y, 0], [self.client.simGetVehiclePose().orientation.w_val,\
self.client.simGetVehiclePose().orientation.x_val,\
self.client.simGetVehiclePose().orientation.y_val,self.client.simGetVehiclePose().orientation.z_val])
self.client.moveByVelocityZAsync(
vx=vx,
vy=vy,
z=z,
yaw_mode=airsim.YawMode(True, 0),
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
duration=duration).join()
self.client.hoverAsync().join()
def center(self, in_frame, z, dir=1, duration=0.2):
"""
Specify protocols for getting package centered in camera view. With a Field of view of 90 deg.
As it is now: drone does a 360 sweep and if no package is found, moves to a lower level and checks again.
dir is the direction you will turn in fine tuning (1 = clockwise, -1 = counter clockwise)
"""
# If the package isn't in frame, rotate and try to get it in frame
if not in_frame:
# Rotate and capture
self.client.moveByVelocityZAsync(
0, 0, z, duration, airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(True, 90)).join()
self.mem += 1
# Lower the drone based on how many runs it's done
if self.mem % 16 == 0:
self.client.moveByVelocityZAsync(
0, 0, z + 1, duration,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(False, 0)).join() # Move down by 5 units
self.mem = 0
# If the package is in frame, do small turns until it's centered
else:
if dir == 0:
self.client.moveByVelocityZAsync(
0, 0, z, duration,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(True, 0)).join()
else:
#TODO: If the distace of the max is less towards one side, move in that direction
self.client.moveByVelocityZAsync(
0, 0, z, duration,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(True, dir * 20)).join()
def _move_by_positions(client: airsim.MultirotorClient,
args: argparse.Namespace) -> None:
for position, orientation in args.viewpoints:
_pitch, _roll, yaw = airsim.to_eularian_angles(
airsim.Quaternionr(*orientation))
client.moveToPositionAsync(
*position,
args.flight_velocity,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw),
).join()
client.hoverAsync().join()
time.sleep(2)
def move_drone(drone_name, dx, dy, dz, yaw, speed):
cur_pos = get_cur_pos(vehicle_name=drone_name)
next_pos = airsim.Vector3r(cur_pos.x_val + dx, cur_pos.y_val + dy,
cur_pos.z_val + dz)
print("try to move: {} -> {}, yaw={}, speed={}".format(
cur_pos, next_pos, yaw, speed))
rc = client.moveToPositionAsync(
next_pos.x_val,
next_pos.y_val,
next_pos.z_val,
speed,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=yaw),
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
vehicle_name=drone_name).join()
cur_pos = get_cur_pos(vehicle_name=drone_name)
print(cur_pos)
def moveToPosition(self, pos_msg):
# self.client.enableApiControl(True, self.name)
# self.client.armDisarm(True, self.name)
x, y, z = pos_msg.pose.position.x, pos_msg.pose.position.y, pos_msg.pose.position.z
print("Move to Position")
# print(x, y, z)
speed = 7 # vel.x + vel.y + vel.z^2 blah
return self.client.moveToPositionAsync(
x,
y,
z,
speed,
timeout_sec=10,
drivetrain=airsim.DrivetrainType.ForwardOnly,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=0),
vehicle_name=self.name)
def moveToPosition(self, pos_msg):
# self.client.enableApiControl(True, self.name)
# self.client.armDisarm(True, self.name)
x, y, z = pos_msg.pose.position.x, pos_msg.pose.position.y, pos_msg.pose.position.z
# print(x, y, z)
speed = 2 # vel.x + vel.y + vel.z^2 blah
return self.client.moveToPositionAsync(
x,
y,
z,
speed,
timeout_sec=10,
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
yaw_mode=airsim.YawMode(is_rate=False,
yaw_or_rate=pos_msg.pose.orientation.w),
vehicle_name=self.name)
def move_to_position(self, args: list):
print("Move received")
if len(args) != 5:
print("Move needs 5 args")
return
self.client.enableApiControl(True)
print("Move args:", float(args[1]), float(args[2]), float(args[3]),
float(args[4]))
self.client.moveToPositionAsync(
x=float(args[1]),
y=float(args[2]),
z=float(args[3]),
velocity=float(args[4]),
drivetrain=airsim.DrivetrainType.ForwardOnly,
yaw_mode=airsim.YawMode(False, 0)).join()
self.client.hoverAsync().join()
print("Moved!")
def trajectory_callback(spline_marker_array_msg):
last_traj_num = len(spline_marker_array_msg.markers)
following_path = []
global velocity
for pose in spline_marker_array_msg.markers[last_traj_num - 1].points:
print("[NED] Adding point (%f, %f, %f)" % (pose.x, -pose.y, -pose.z))
following_path.append(airsim.Vector3r(pose.x, -pose.y, -pose.z))
client.moveOnPathAsync(following_path, velocity, 3e+38,
airsim.DrivetrainType.ForwardOnly,
airsim.YawMode(False, 0), -1, 1).join()
client.enableApiControl(False)
def _move_drone(cls,
client,
drone_name,
dx,
dy,
dz,
cur_yaw,
speed,
screenshot_helper=None,
mininet_helper=None):
# move the drong
cur_state = client.getMultirotorState(vehicle_name=drone_name)
cur_pos = cur_state.kinematics_estimated.position
next_pos = airsim.Vector3r(cur_pos.x_val + dx, cur_pos.y_val + dy,
cur_pos.z_val + dz)
log.info("try to move: {} -> {}, yaw={}, speed={}".format(
cur_pos, next_pos, cur_yaw, speed))
rc = client.moveToPositionAsync(
next_pos.x_val,
next_pos.y_val,
next_pos.z_val,
speed,
yaw_mode=airsim.YawMode(is_rate=False, yaw_or_rate=cur_yaw),
drivetrain=airsim.DrivetrainType.MaxDegreeOfFreedom,
vehicle_name=drone_name).join()
# pose = client.simGetObjectPose(drone_name)
# cur_pos = pose.position
# next_pos = airsim.Vector3r(
# cur_pos.x_val + dx, cur_pos.y_val + dy, cur_pos.z_val + dz)
# pose.position = next_pos
# rc = client.simSetObjectPose(drone_name, pose, True)
log.info("rc: {}".format(rc))
if mininet_helper is not None:
mininet_helper.move_drone(cur_pos, next_pos)
# is collision?
collision_info = client.simGetCollisionInfo(vehicle_name=drone_name)
if collision_info.has_collided:
log.error('collided! collision_info={}'.format(collision_info))
client.enableApiControl(False, drone_name)
return False
if screenshot_helper is not None:
image_file_list = screenshot_helper.do_screenshot(is_display=False)
if mininet_helper is not None:
mininet_helper.send_image(image_file_list[0])
return True
def run(self):
# --> Reset Drone to starting position
self.client.reset()
# --> Enable API control and take off
self.client.enableApiControl(True)
self.client.armDisarm(True)
self.client.moveToPositionAsync(0, 0, -2, 3).join()
# --> Set path
result = self.client.moveOnPathAsync([
airsim.Vector3r(0, -40, 2),
airsim.Vector3r(-10, -72, 0),
airsim.Vector3r(-25, -65, 4),
airsim.Vector3r(-40, -65, 0),
airsim.Vector3r(-62, -55, 0),
airsim.Vector3r(-65, 30, 0),
airsim.Vector3r(-50, 45, 4),
airsim.Vector3r(-35, 55, 5),
airsim.Vector3r(-10, 40, 5),
airsim.Vector3r(-0, 25, 4),
airsim.Vector3r(0, 0, -2),
airsim.Vector3r(0, 0, -2),
airsim.Vector3r(0, 0, -2),
], 16, 150, airsim.DrivetrainType.ForwardOnly,
airsim.YawMode(False,
0), 20, 1).join()
# self.client.moveToPositionAsync(0, -20, -2, 10).join()
# time.sleep(2)
#
# self.client.moveToPositionAsync(0, -20, 4, 10).join()
# time.sleep(2)
# self.client.moveToPositionAsync(0, -28.5, -2, 1).join()
# time.sleep(2)
# self.client.moveToPositionAsync(0, -28.5, -6, 1)
# self.client.moveToPositionAsync(0, -28, -2, 1).join()
# time.sleep(2)
# self.client.moveToPositionAsync(0, -28, -2, 1).join()
# self.client.moveToPositionAsync(-2, -26.5, -2, 2).join()
# print("1")
# self.client.moveToPositionAsync(-40, -60, -2, 16, drivetrain=airsim.DrivetrainType.ForwardOnly).join()
print("2")
def __move_on_box(client: airsim.MultirotorClient, z: float, side: float,
velocity: float) -> None:
# NOTE airsim.DrivetrainType.MaxDegreeOfFreedom allows controlling the drone yaw independently
# from the direction it is flying, this way we make it always point to the inside of the box
duration = side / velocity
vx_vy_yaw = [(velocity, 0, 90), (0, velocity, 180), (-velocity, 0, 270),
(0, -velocity, 0)]
print(f"[ff] Moving on box...", end=" ", flush=True)
for vx, vy, yaw in vx_vy_yaw:
client.simPrintLogMessage(
f"moving by velocity vx={vx}, vy={vy}, yaw={yaw}")
client.moveByVelocityZAsync(vx, vy, z, duration,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(False, yaw)).join()
time.sleep(4)
client.hoverAsync().join()
client.landAsync().join()
print(f"done.")
def enter_keyboard_control(self):
print("You entered the keyboard mode. Press 't' to return.")
kc = KeyController()
z = self.client.getMultirotorState(
).kinematics_estimated.position.z_val
self.client.enableApiControl(True)
while kc.listener.running:
self.client.cancelLastTask()
self.client.enableApiControl(True)
keys = kc.get_key_pressed()
if 'h' in keys:
self.client.hoverAsync()
else:
quad_vel = self.client.getMultirotorState(
).kinematics_estimated.linear_velocity
self.vx = self.handle_key_pressed(keys_to_check=['w', 's'],
pressed_keys=keys,
current_vel=quad_vel.x_val)
self.vy = self.handle_key_pressed(keys_to_check=['d', 'a'],
pressed_keys=keys,
current_vel=quad_vel.y_val)
z = self.handle_height(keys_to_check=['z', 'x'],
pressed_keys=keys,
current_height=z)
self.yaw = self.handle_rotation(keys_to_check=['e', 'q'],
pressed_keys=keys)
print(
"current vel: \n vx:{0}, nvx:{1}\n vy:{2}, nvy:{3}\n vz:{4}, nvz:{5}\n"
.format(quad_vel.x_val, self.vx, quad_vel.y_val, self.vy,
quad_vel.z_val, self.vz))
current_pos = self.client.getMultirotorState(
).kinematics_estimated.position
print("current pos: \n x:{0}, y:{1}\n z:{2}\n".format(
current_pos.x_val, current_pos.y_val, current_pos.z_val))
self.client.moveByVelocityZAsync(
self.vx, self.vy, z, 0.1,
airsim.DrivetrainType.MaxDegreeOfFreedom,
airsim.YawMode(True, self.yaw)).join()
# self.client.moveByVelocityAsync(self.vx, self.vy, self.vz, 0.1, airsim.DrivetrainType.MaxDegreeOfFreedom,
# airsim.YawMode(True, self.yaw)).join()
# airsim.time.sleep(0.2)
print("'t' has been pressed and the console control is back")
self.client.hoverAsync().join()
def enter_keyboard_control(self):
print("You entered the keyboard mode. Press 't' to return.")
kc = KeyController()
self.client.enableApiControl(True)
while kc.thread.isAlive():
self.client.cancelLastTask()
self.client.enableApiControl(True)
keys = kc.get_key_pressed()
quad_vel = self.client.getMultirotorState().kinematics_estimated.linear_velocity
self.vx = self.handle_key_pressed(keys_to_check=['w', 's'], pressed_keys=keys, current_vel=quad_vel.x_val)
self.vy = self.handle_key_pressed(keys_to_check=['d', 'a'], pressed_keys=keys, current_vel=quad_vel.y_val)
self.vz = self.handle_key_pressed(keys_to_check=['e', 'q'], pressed_keys=keys, current_vel=quad_vel.z_val)
print("current: \n vx:{0}, nvx:{1}\n vy:{2}, nvy:{3}\n vz:{4}, nvz:{5}\n".format(quad_vel.x_val, self.vx,
quad_vel.y_val, self.vy,
quad_vel.z_val, self.vz))
self.client.moveByVelocityAsync(self.vx, self.vy, self.vz, 0.1, airsim.DrivetrainType.ForwardOnly,
airsim.YawMode(False, 0)).join()
# airsim.time.sleep(0.2)
print("'t' has been pressed and the console control is back")
self.client.hoverAsync().join()
def flyToPosition(self, x: float, y: float, z: float, v: float):
"""
Fly the drone to position
Args:
x : float - x coordinate
y : float - y coordinate
z : float - z coordinate
v : float - the velocity which the drone fly to position
Returns:
none
"""
self.future = self.client.moveToPositionAsync(
x,
y,
z,
v,
drivetrain=airsim.DrivetrainType.ForwardOnly,
yaw_mode=airsim.YawMode(False, 0.0))
