def telloControllerThread():
tello = Tello('', 8889)
north = 0
west = 0
south = 0
east = 0
landing = False
while not receiving:
pass
try:
tello.takeoff()
except Exception:
print("error occured taking off")
while True:
print("height")
print(tello.get_height())
if tello.get_height() < 1 and not landing:
resp = tello.move_up(0.2)
print("response for moving up")
print(resp)
if north < 1:
print("moving forward")
resp = tello.move_forward(0.2)
print("response for moving forward")
print(resp)
north = north + 0.2
elif west < 1:
print("moving left")
resp = tello.move_left(0.2)
print("response for moving left")
print(resp)
west = west + 0.2
elif south < 1:
print("moving backward")
resp = tello.move_backward(0.2)
print("response for moving backward")
print(resp)
south = south + 0.2
elif east < 1:
print("moving right")
resp = tello.move_right(0.2)
print("response for moving east")
print(resp)
east = east + 0.2
else:
landing = True
tello.land()
# controls as given in intro() function
if k == ord('t'):
t.takeoff(response=False)
taken_off = True
if k == ord('m'):
if MANUAL_MODE == True:
MANUAL_MODE = False
else:
MANUAL_MODE = True
if k == ord('q'):
running = False
if taken_off:
t.land()
if k == 27:
print("Shutting down motors...")
t.emergency()
break
if k == 49:
if S != 30:
print("Setting speed to 'low'")
S = 30
else:
print("Speed already set to low")
if k == 50:
if S != 65:
class FaceTracker:
"""Constants"""
FB_S = 15 # FWD/BWD Speed of the drone
LR_S = 25 # LFT/RGHT Speed of the drone
UD_S = 25
CW_S = 25 # CW/CCW Speed of the drone
FPS = 20 # Frames per second of the pygame window display
def __init__(self, height, width):
"""Start loading"""
namedWindow("drone")
self.paper = imread("./resources/Tello.png")
self.height = height
self.width = width
self.fv = FaceVector(height, width)
self.tello = Tello()
# Drone velocities between -100~100
self.for_back_velocity = 20
self.left_right_velocity = 20
self.up_down_velocity = 20
self.yaw_velocity = 20
self.speed = 20
self.send_rc_control = False
self.should_stop = False
self.mode = "keyboard"
def run(self, show=False, debug=False):
"""Connecting block"""
if not self.tello.connect():
return
print("Connected")
if not self.tello.set_speed(self.speed):
return
print("Speeds set")
"""Stream start block"""
if not self.tello.streamoff():
return
if not self.tello.streamon():
return
cap = self.tello.server.get_video_capture()
print("Stream started")
"""Main loop"""
while not self.should_stop:
vec = None
"""Frame reading block"""
if self.mode == "tracking" or show or debug:
try:
r, frame = cap.read()
except ValueError:
continue
if r:
"""Creating target directions vector"""
if self.mode == "tracking" or debug:
vec, frame = self.fv.direction_vector_3d_with_returning_image(frame)
"""Frame plotting(requires from argument: bool:SHOW)"""
if show or debug:
frame = self.fv.text_addition(frame, vec)
imshow("drone", frame)
if (not show) and (not debug):
imshow("drone", self.paper)
key = waitKey(5) & 0xff
"""Keyboard commands getting"""
self.check_key(key)
if self.mode == "tracking":
"""Driving block"""
if vec is None:
vec = [0, 0, 0]
print(vec)
"""Setting velocities depending from directions vector VEC"""
if vec[0] != 0 or vec[1] != 0:
"""Moving in 2D space at first"""
# set left/right velocity
self.left_right_velocity = -self.LR_S * vec[0]
# set up/down velocity
self.up_down_velocity = self.UD_S * vec[1]
# set forward/backward velocity
else:
"""Then moving forward/backward"""
self.for_back_velocity = self.FB_S * vec[2]
# set yaw clockwise velocity
self.yaw_velocity = self.CW_S * 0
"""Send move commands"""
self.update()
sleep(1 / self.FPS)
self.tello.end()
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.move_with_velocities(self.left_right_velocity, self.for_back_velocity,
self.up_down_velocity, self.yaw_velocity)
def check_key(self, key):
"""
Moves Tello through the keyboard keys.
- T / G : Takeoff / Land(Ground).
- W / S : Up / Down.
- A / D : CW / CCW.
- I / K : Forward / Backward.
- J / L : Left / Right.
- SPACE : Start / Stop face tracking.
- Q : Quit.
"""
if key == ord('q'): # stop
self.should_stop = True
elif key == ord('t'): # takeoff
print("Flying")
self.tello.takeoff()
self.send_rc_control = True
elif key == ord('g'): # land
print("Landing")
self.tello.land()
self.send_rc_control = False
elif key == ord('i'): # forward
self.tello.forward(50)
elif key == ord('k'): # backward
self.tello.back(50)
elif key == ord('j'): # left
self.tello.left(50)
elif key == ord('l'): # right
self.tello.right(50)
elif key == ord('w'): # up
self.tello.up(50)
elif key == ord('s'): # down
self.tello.down(50)
elif key == ord('a'): # cw
self.tello.rotate_cw(30)
elif key == ord('d'): # ccw
self.tello.rotate_ccw(30)
elif key == ord(' '): # change mode
self.change_mode()
"""Realise mode changing event"""
def change_mode(self):
if self.mode == "tracking":
self.mode = "keyboard"
self.tello.stop()
elif self.mode == "keyboard":
self.mode = "tracking"
while True:
try:
tello = Tello(local_ip[i], local_port, imperial, wait_time + 1)
except OSError:
i = i + 1
if i == 2:
break
else:
print(local_ip[i], "Is wrong ip")
try:
Current_battery = tello.get_battery()
except NameError:
sys.exit("No ip found")
if Current_battery >= Lowest_battery:
print("Battery is fine")
tello.takeoff()
time.sleep(wait_time) # delay for 1 seconds
tello.flip('b')
time.sleep(wait_time) # delay for 1 seconds
tello.rotate_cw(180)
time.sleep(wait_time)
tello.set_speed(2)
time.sleep(1)
tello.move_forward(0.1)
time.sleep(wait_time)
print('moved')
time.sleep(wait_time)
tello.land()
else:
print("Battery need charging!")
class Drone:
def __init__(self, local_ip, local_port):
self.drone = Tello(local_ip, local_port)
def close(self):
self.drone.close()
def dame_imagen(self):
return self.drone.get_image()
def dame_objeto(self):
return self.drone.get_object(showImageFiltered=True)
# ------------ EXTRA ------------
def despegar(self):
return self.drone.takeoff()
def aterrizar(self):
return self.drone.land()
# ------------ CONTROL EN VELOCIDAD ------------
def to_rc(self, v):
return math.floor((100 * v) / V_MAX)
def to_rc_rot(self, w):
return math.floor((100 * w) / W_MAX)
# Las 4 funciones: velocidad, avanzar, retroceder y lateral_izquierda
# son bloqueantes, avanzan hasta una determinada posición y paran
# y después ejecutan la siguiente instrucción (no hay movimiento contínuo)
#def velocidad(self, vel):
# return self.set_speed(vel)
#def avanzar(self, vel):
# self.drone.forward_speed(vel)
#def retroceder(self, vel):
# self.drone.backward_speed(vel)
#def lateral_izquierda(self, vel):
# self.drone.left_speed(vel)
# Modo RadioControl, movimiento constante con rangos de velocidades
# -100 a 100 (simulando un stick analógico). He hecho pruebas y la velocidad
# máxima de avance (con vx = 100) es de aprox. 1.5 m/s (igual que si pilotas en la app
# del móvil) aunque en los foros y la documentación pone que es máximo 1 m/s.
# La velocidad máxima teórica del Tello es de 10 m/s pero está capado (hay que tocar el
# firmware).
# Con el API solo se puede ir en modo lento (velocidad max de 1 m/s), y con la app
# del movil se puede poner el modo rápido que tiene una velocidad maxima de 3 m/s.
def _rc(self, vy, vx, vz, rot):
self.drone.set_velocities(vy, vx, vz, rot)
def avanzar(self, vx):
vx = self.to_rc(vx)
self._rc(0, vx, 0, 0)
def retroceder(self, vx):
vx = self.to_rc(vx) * (-1)
self._rc(0, vx, 0, 0)
def lateral_izquierda(self, vy):
vy = self.to_rc(vy)
self._rc(vy, 0, 0, 0)
def lateral_derecha(self, vy):
vy = self.to_rc(vy) * (-1)
self._rc(vy, 0, 0, 0)
def subir(self, vz):
vz = self.to_rc(vz)
self._rc(0, 0, vz, 0)
def bajar(self, vz):
vz = self.to_rc(vz) * (-1)
self._rc(0, 0, vz, 0)
def girar_izquierda(self, rot):
rot = self.to_rc_rot(rot)
self._rc(0, 0, 0, rot)
def girar_derecha(self, rot):
rot = self.to_rc_rot(rot) * (-1)
self._rc(0, 0, 0, rot)
def movimiento_libre(self, vx, vy, vz, rot):
self._rc(self.to_rc(vy), self.to_rc(vx), self.to_rc(vz),
self.to_rc_rot(rot))
def parar(self):
self.drone.stop()
# ------------ CONTROL EN POSICION ------------
def girar_derecha_hasta(self, grados):
return self.drone.rotate_cw(grados)
#def girar_derecha(self, grados):
# return self.rotate_cw(grados)
def girar_izquierda_hasta(self, grados):
return self.drone.rotate_ccw(grados)
#def girar_izquierda(self, grados):
# return self.rotate_ccw(grados)
def retrocecer_hasta(self, distancia):
return self.drone.move_backward(distancia)
def bajar_hasta(self, distancia):
return self.drone.move_down(distancia)
def avanzar_hasta(self, distancia):
return self.drone.move_forward(distancia)
def izquierda_hasta(self, distancia):
return self.drone.move_left(distancia)
def derecha_hasta(self, distancia):
return self.drone.move_right(distancia)
def subir_hasta(self, distancia):
return self.drone.move_up(distancia)
# ------ METODOS INFORMATIVOS ------
def altura_actual(self):
return self.drone.get_height()
def bateria_restante(self):
return self.drone.get_battery()
def tiempo_de_vuelo(self):
return self.drone.get_flight_time()
def velocidad_actual(self):
return self.drone.get_speed()
class Follower:
def __init__(self):
pygame.init()
pygame.display.set_caption("Tello video stream")
self.screen = pygame.display.set_mode([960, 720])
# Drone velocities between -100~100
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = 0
self.speed = 10
self.tello = Tello()
self.send_rc_control = False
pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
self.personTracker = PersonTracker()
self.Px, self.Ix, self.Dx = 0.10, 0, -0 #D gain should be negative.
self.Py, self.Iy, self.Dy = 0.1, 0, -0
self.Pz, self.Iz, self.Dz = 0.25, 0, -0.001
self.prev_err_x, self.prev_err_y, self.prev_err_z = None, None, None
self.accum_err_x, self.accum_err_y, self.accum_err_z = 0, 0, 0
self.found_person = False
self.manual = True
self.iter = 0
def run(self):
self.tello.connect()
self.tello.set_speed(self.speed)
# In case streaming is on. This happens when we quit this program without the escape key.
self.tello.streamoff()
self.tello.streamon()
frame_read = self.tello.get_frame_read()
should_stop = False
imprinted = False
emergency_counter = 0
while not should_stop:
for event in pygame.event.get():
if event.type == pygame.USEREVENT + 1:
self.update()
elif event.type == pygame.QUIT:
should_stop = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
should_stop = True
else:
self.keydown(event.key)
elif event.type == pygame.KEYUP:
self.keyup(event.key)
if frame_read.stopped:
break
self.screen.fill([0, 0, 0])
frame = frame_read.frame
if self.iter > 240:
humanBox, imprinted = self.personTracker.findMyHooman(frame)
if humanBox is None and self.send_rc_control and not self.manual:
emergency_counter += 1
if emergency_counter >= 120: #missed human for 120 frames; 1 second
print("ENGAGING EMERGENCY HOVER.")
cv2.putText(frame, "EMERGENCY HOVER", (700, 130),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0),
2)
self.emergencyHover()
elif humanBox is not None:
if self.found_person == False:
self.found_person = True
emergency_counter = 0
#PID LOOP:
desired_pos = (frame.shape[1] // 2, frame.shape[0] // 2
) #format x,y
if humanBox is not None:
xmin = int(humanBox[1] * frame.shape[1])
ymin = int(humanBox[0] * frame.shape[0])
xmax = int(humanBox[3] * frame.shape[1])
ymax = int(humanBox[2] * frame.shape[0])
centerHumanPosition = (np.mean(
(xmax, xmin)), np.mean((ymax, ymin))) #format x,y
#draw bounding box
cv2.rectangle(frame, (xmin, ymin), (xmax, ymax),
(255, 0, 0), 2) #blue
#draw target coord
cv2.circle(frame, (int(
centerHumanPosition[0]), int(centerHumanPosition[1])),
10, (255, 0, 0), 1) #blue
# print("z width: {}".format(np.abs(xmax-xmin)))
#draw desired coord
cv2.circle(frame, desired_pos, 10, (0, 0, 255), 1) #red
if self.send_rc_control and not self.manual:
self.update_control(centerHumanPosition, desired_pos,
xmax, xmin)
text = "Battery: {}%".format(self.tello.get_battery())
cv2.putText(frame, text, (5, 720 - 5), cv2.FONT_HERSHEY_SIMPLEX, 1,
(0, 0, 255), 2)
heightText = "Height:{}".format(self.tello.get_height())
cv2.putText(frame, heightText, (720 - 5, 40),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
manualText = "Manual: {}".format(self.manual)
if self.manual:
cv2.putText(frame, manualText, (720 - 5, 70),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
else:
cv2.putText(frame, manualText, (720 - 5, 70),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
imprintedTxt = "Imprinted: {}".format(imprinted)
if imprinted:
cv2.putText(frame, imprintedTxt, (720 - 5, 100),
cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 2)
else:
cv2.putText(frame, imprintedTxt, (720 - 5, 100),
cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
frame = np.rot90(frame)
frame = np.flipud(frame)
frame = pygame.surfarray.make_surface(frame)
self.screen.blit(frame, (0, 0))
pygame.display.update()
if self.iter <= 240:
self.iter += 1
time.sleep(1 / FPS)
# Call it always before finishing. To deallocate resources.
self.tello.end()
def keydown(self, key):
""" Update velocities based on key pressed
Arguments:
key: pygame key
"""
if key == pygame.K_UP: # set forward velocity
self.for_back_velocity = S
elif key == pygame.K_DOWN: # set backward velocity
self.for_back_velocity = -S
elif key == pygame.K_LEFT: # set left velocity
self.left_right_velocity = -S
elif key == pygame.K_RIGHT: # set right velocity
self.left_right_velocity = S
elif key == pygame.K_w: # set up velocity
self.up_down_velocity = S
elif key == pygame.K_s: # set down velocity
self.up_down_velocity = -S
elif key == pygame.K_a: # set yaw counter clockwise velocity
self.yaw_velocity = -S
elif key == pygame.K_d: # set yaw clockwise velocity
self.yaw_velocity = S
elif key == pygame.K_m: # set yaw clockwise velocity
self.manual = not self.manual
self.yaw_velocity = 0
self.up_down_velocity = 0
self.left_right_velocity = 0
print("MANUAL MODE IS NOW: {}".format(self.manual))
def keyup(self, key):
""" Update velocities based on key released
Arguments:
key: pygame key
"""
if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
self.for_back_velocity = 0
elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
self.left_right_velocity = 0
elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
self.up_down_velocity = 0
elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
self.yaw_velocity = 0
elif key == pygame.K_t: # takeoff
self.tello.takeoff()
self.send_rc_control = True
elif key == pygame.K_l: # land
not self.tello.land()
self.iter = 0
self.send_rc_control = False
def update(self):
""" Update routine. Send velocities to Tello."""
if self.send_rc_control:
self.tello.send_rc_control(self.left_right_velocity,
self.for_back_velocity,
self.up_down_velocity,
self.yaw_velocity)
def update_control(self, curr_lateral_pos, desired_later_pos, xmax, xmin):
#Three error directions. Two lateral, one forward/backward. How to calc forward/back error?
err_x = curr_lateral_pos[0] - desired_later_pos[
0] #if positive, we're to the left of where we want to be, so want positive control. (CHECK THIS)
err_y = desired_later_pos[1] - curr_lateral_pos[
1] #if positive, we're below where we want to be. (CHECK THIS)
#hardcode desired box width. Must test!!
desired_width = 350
curr_width = np.abs(xmax -
xmin) #check. is this actually the width dim?
err_z = desired_width - curr_width #if negative, too close; want backwards--> positive gain
# print("Err z: {}".format(err_z))
if self.prev_err_x == None:
derivative_x_input = 0
derivative_y_input = 0
derivative_z_input = 0
else:
derivative_x_input = (err_x - self.prev_err_x) / (1 / FPS)
derivative_y_input = (err_y - self.prev_err_y) / (1 / FPS)
derivative_z_input = (err_z - self.prev_err_z) / (1 / FPS)
#clip derivative errors to avoid noise
derivative_x_input = np.clip(derivative_x_input, -11000, 11000)
derivative_y_input = np.clip(derivative_y_input, -11000, 11000)
derivative_z_input = np.clip(derivative_z_input, -11000, 11000)
self.accum_err_x += err_x
self.accum_err_y += err_y
self.accum_err_z += err_z
self.prev_err_x = err_x
self.prev_err_y = err_y
self.prev_err_z = err_z
# print("derr_z: {}".format(derivative_z_input))
# self.left_right_velocity = self.Px*err_x+self.Dx*derivative_x_input+self.Ix*self.accum_err_x
self.yaw_velocity = self.Px * err_x + self.Dx * derivative_x_input + self.Ix * self.accum_err_x
self.up_down_velocity = self.Py * err_y + self.Dy * derivative_y_input + self.Iy * self.accum_err_y
self.for_back_velocity = self.Pz * err_z + self.Dy * derivative_z_input + self.Iz * self.accum_err_z
#limit velocity to 2*S.
# self.left_right_velocity = np.clip(self.left_right_velocity, -S*2, S*2)
self.yaw_velocity = int(np.clip(self.yaw_velocity, -S, S))
self.up_down_velocity = int(np.clip(self.up_down_velocity, -S, S))
self.for_back_velocity = int(np.clip(self.for_back_velocity, -S, S))
#Send new velocities to robot.
self.update()
def emergencyHover(self):
print("Cannot find hooman. I am lonely doggo. Hovering and rotating.")
self.found_person = False
self.for_back_velocity = 0
self.left_right_velocity = 0
self.up_down_velocity = 0
self.yaw_velocity = int(S // 2)
self.update()
from tello import Tello
import time
local_ip = '192.168.10.3'
local_port = 8889
imperial = False
drone = Tello(local_ip, local_port, imperial)
drone.takeoff()
time.sleep(7)
drone.set_speed(2)
time.sleep(1)
drone.move_down(3)
time.sleep(5)
drone.move_forward(10)
time.sleep(10)
drone.rotate_cw(180)
time.sleep(5)
drone.move_forward(10)
time.sleep(10)
drone.land()
print("Hokey Pokey complete! :)")
print('Flight time: %s' % drone.get_flight_time())
def main(_argv):
# Telloクラスを使って，droneというインスタンス(実体)を作る
drone = Tello('', 8889, command_timeout=.01)
# 人検知，接近用のインスタンス，フラグ，トラッカータイプ
default = Default(drone) # 人探索用のインスタンス作成
# track_type = "KCF" # トラッカーのタイプ，ユーザーが指定
# 処理の開始
drone.send_command('command') # SDKモードを開始
current_time = time.time() # 現在時刻の保存変数
pre_time = current_time # 5秒ごとの'command'送信のための時刻変数
drone.subscribe() # 対話開始
# ここでアレクサに「救助アプリを開いて」と言うと離陸し対話を受け付ける
# 強制離陸する場合
# drone.takeoff()
time.sleep(0.5) # 通信が安定するまでちょっと待つ
# drone.takeoff() # 自動で離陸しているが，ここはAlexaを使用して離陸させた方が良いかも(対話を開始するタイミングをトリガーさせるためにも)
# openpose_flag = 0:
#Ctrl+cが押されるまでループ
try:
while True:
if drone.start_flag:
if drone.status == 'default':
# デフォルト状態でホバリングし，常に人を認識する．認識した時，statusを'approach'に変更する
print(drone.status)
frame, bbox = default.detect() # 人を探し，検知したら領域をbboxに保存
if drone.detect_flag: # 人を検知後statusをapproachに変更
drone.to_communicate()
approach = Approach(
drone, frame,
bbox) # Approachクラスのインスタンスを作成，トラッカーの初期化
continue
# デバッグ用
# time.sleep(1)
# print(drone.status)
# drone.to_approach()
if drone.status == 'approach':
# 認識した人に近づく．近づき終わったらstatusを'communicate'に変更する
print(drone.status)
approach.approach() # 検知した人を追跡．結果を返す
# 人を追跡できているか，または接近できたかどうかの判定
if drone.detect_flag and drone.approach_flag: # 接近できていればstatusをcommunicateへ変更
# drone.to_communicate()
break
elif not drone.detect_flag: # 追跡が失敗したらdefaultへ戻る
drone.to_default()
del approach # Approachクラスのインスタンスを削除
else: # 例外処理
print("なんかエラーっぽいよ")
print("detect:" + str(drone.detect_flag))
print("approach:" + str(drone.approach_flag))
time.sleep(10)
# デバッグ用
# time.sleep(1)
# drone.to_communicate()
if drone.status == 'communicate':
# 人と対話する．対話が正常終了したらstatusを'default'に戻す．対話に失敗した場合はstatusを'judingpose'に
speak.mp3play(
'./ProcessVoice/speech_20191223054237114.mp3')
# デバッグ用
# drone.to_default()
# drone.to_judingpose()
# 対話時間
time.sleep(5)
drone.send_command('command') # 'command'送信
time.sleep(5)
drone.send_command('command') # 'command'送信
time.sleep(5)
drone.send_command('command') # 'command'送信
if drone.status == 'communicate': # 無言だった場合
drone.status = 'judingpose' # 人の姿勢を検出する
if drone.status == 'judingpose':
# 人の姿勢を検出する．姿勢推定を行い人の状態の判定後，人に話しかけ，statusを'default'に戻す
speak.mp3play('../openpose/jirikidehinanndekinai.mp3')
time.sleep(1)
speak.mp3play('../openpose/jirikidehinanndekiru.mp3')
time.sleep(1)
while True:
frame = drone.read() # 映像を1フレーム取得
if frame is None or frame.size == 0: # 中身がおかしかったら無視
continue
# (B)ここから画像処理
image = cv2.cvtColor(
frame, cv2.COLOR_RGB2BGR) # OpenCV用のカラー並びに変換する
small_image = cv2.resize(image,
dsize=(480,
360)) # 画像サイズを半分に変更
run_function.openpose(small_image)
if run_load_human_model.add_label(
"../openpose/uncho.csv") == 3:
speak.mp3play('../openpose/hinansitekudasai.mp3')
else:
speak.mp3play('../openpose/kyuujyowoyobimasu.mp3')
# デバッグ用
time.sleep(1)
print(drone.status)
drone.to_default()
# 以下(X)(Y)(Z)は便宜的に記載した．システムで必要な処理ではない
# (X)ウィンドウに表示
# cv2.imshow('OpenCV Window', small_image) # ウィンドウに表示するイメージを変えれば色々表示できる
# (Y)OpenCVウィンドウでキー入力を1ms待つ
# key = cv2.waitKey(1)
# if key == 27: # k が27(ESC)だったらwhileループを脱出，プログラム終了
# break
# elif key == ord('t'):
# drone.takeoff() # 離陸
# elif key == ord('l'):
# drone.land() # 着陸
# elif key == ord('w'):
# drone.move_forward(0.3) # 前進
# elif key == ord('s'):
# drone.move_backward(0.3) # 後進
# elif key == ord('a'):
# drone.move_left(0.3) # 左移動
# elif key == ord('d'):
# drone.move_right(0.3) # 右移動
# elif key == ord('q'):
# drone.rotate_ccw(20) # 左旋回
# elif key == ord('e'):
# drone.rotate_cw(20) # 右旋回
# elif key == ord('r'):
# drone.move_up(0.3) # 上昇
# elif key == ord('f'):
# drone.move_down(0.3) # 下降
# elif key == ord('o'):
# image_path = "../openpose/images/" # openpose
# image = cv2.imwrite(image_path+"input.jpg",small_image)
# os.system('python ../openpose/openpose.py')
# (Z)5秒おきに'command'を送って、死活チェックを通す
current_time = time.time() # 現在時刻を取得
if current_time - pre_time > 5.0: # 前回時刻から5秒以上経過しているか？
drone.send_command('command') # 'command'送信
pre_time = current_time # 前回時刻を更新
except (KeyboardInterrupt, SystemExit): # Ctrl+cが押されたら離脱
print("SIGINTを検知")
# telloクラスを削除
drone.land()
del drone
