#!/usr/bin/env python
# NOTE: Line numbers of this example are referenced in the user guide.
# Don't forget to update the user guide after every modification of this example.
import csv
import cv2
import math
import os
import queue
import shlex
import subprocess
import tempfile
import threading
import traceback
import olympe
from olympe.messages.ardrone3.Piloting import TakeOff, Landing
from olympe.messages.ardrone3.Piloting import moveBy
from olympe.messages.ardrone3.PilotingState import FlyingStateChanged
from olympe.messages.ardrone3.PilotingSettings import MaxTilt
from olympe.messages.ardrone3.GPSSettingsState import GPSFixStateChanged
olympe.log.update_config({"loggers": {"olympe": {"level": "WARNING"}}})
DRONE_IP = "10.202.0.1"
class StreamingExample(threading.Thread):
def __init__(self):
# Create the olympe.Drone object from its IP address
self.drone = olympe.Drone(DRONE_IP)
self.tempd = tempfile.mkdtemp(prefix="olympe_streaming_test_")
print("Olympe streaming example output dir: {}".format(self.tempd))
self.h264_frame_stats = []
self.h264_stats_file = open(
os.path.join(self.tempd, 'h264_stats.csv'), 'w+')
self.h264_stats_writer = csv.DictWriter(
self.h264_stats_file, ['fps', 'bitrate'])
self.h264_stats_writer.writeheader()
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
super().__init__()
super().start()
def start(self):
# Connect the the drone
self.drone.connect()
# You can record the video stream from the drone if you plan to do some
# post processing.
self.drone.set_streaming_output_files(
h264_data_file=os.path.join(self.tempd, 'h264_data.264'),
h264_meta_file=os.path.join(self.tempd, 'h264_metadata.json'),
# Here, we don't record the (huge) raw YUV video stream
# raw_data_file=os.path.join(self.tempd,'raw_data.bin'),
# raw_meta_file=os.path.join(self.tempd,'raw_metadata.json'),
)
# Setup your callback functions to do some live video processing
self.drone.set_streaming_callbacks(
raw_cb=self.yuv_frame_cb,
h264_cb=self.h264_frame_cb,
start_cb=self.start_cb,
end_cb=self.end_cb,
flush_raw_cb=self.flush_cb,
)
# Start video streaming
self.drone.start_video_streaming()
def stop(self):
# Properly stop the video stream and disconnect
self.drone.stop_video_streaming()
self.drone.disconnect()
self.h264_stats_file.close()
def yuv_frame_cb(self, yuv_frame):
"""
This function will be called by Olympe for each decoded YUV frame.
:type yuv_frame: olympe.VideoFrame
"""
yuv_frame.ref()
self.frame_queue.put_nowait(yuv_frame)
def flush_cb(self):
with self.flush_queue_lock:
while not self.frame_queue.empty():
self.frame_queue.get_nowait().unref()
return True
def start_cb(self):
pass
def end_cb(self):
pass
def h264_frame_cb(self, h264_frame):
"""
This function will be called by Olympe for each new h264 frame.
:type yuv_frame: olympe.VideoFrame
"""
# Get a ctypes pointer and size for this h264 frame
frame_pointer, frame_size = h264_frame.as_ctypes_pointer()
# For this example we will just compute some basic video stream stats
# (bitrate and FPS) but we could choose to resend it over an another
# interface or to decode it with our preferred hardware decoder..
# Compute some stats and dump them in a csv file
info = h264_frame.info()
frame_ts = info["ntp_raw_timestamp"]
if not bool(info["h264"]["is_sync"]):
if len(self.h264_frame_stats) > 0:
while True:
start_ts, _ = self.h264_frame_stats[0]
if (start_ts + 1e6) < frame_ts:
self.h264_frame_stats.pop(0)
else:
break
self.h264_frame_stats.append((frame_ts, frame_size))
h264_fps = len(self.h264_frame_stats)
h264_bitrate = (
8 * sum(map(lambda t: t[1], self.h264_frame_stats)))
self.h264_stats_writer.writerow(
{'fps': h264_fps, 'bitrate': h264_bitrate})
def show_yuv_frame(self, window_name, yuv_frame):
# the VideoFrame.info() dictionary contains some useful information
# such as the video resolution
info = yuv_frame.info()
height, width = info["yuv"]["height"], info["yuv"]["width"]
# yuv_frame.vmeta() returns a dictionary that contains additional
# metadata from the drone (GPS coordinates, battery percentage, ...)
# convert pdraw YUV flag to OpenCV YUV flag
cv2_cvt_color_flag = {
olympe.PDRAW_YUV_FORMAT_I420: cv2.COLOR_YUV2BGR_I420,
olympe.PDRAW_YUV_FORMAT_NV12: cv2.COLOR_YUV2BGR_NV12,
}[info["yuv"]["format"]]
# yuv_frame.as_ndarray() is a 2D numpy array with the proper "shape"
# i.e (3 * height / 2, width) because it's a YUV I420 or NV12 frame
# Use OpenCV to convert the yuv frame to RGB
cv2frame = cv2.cvtColor(yuv_frame.as_ndarray(), cv2_cvt_color_flag)
# Use OpenCV to show this frame
cv2.imshow(window_name, cv2frame)
cv2.waitKey(1) # please OpenCV for 1 ms...
def run(self):
window_name = "Olympe Streaming Example"
cv2.namedWindow(window_name, cv2.WINDOW_NORMAL)
main_thread = next(
filter(lambda t: t.name == "MainThread", threading.enumerate())
)
while main_thread.is_alive():
with self.flush_queue_lock:
try:
yuv_frame = self.frame_queue.get(timeout=0.01)
except queue.Empty:
continue
try:
self.show_yuv_frame(window_name, yuv_frame)
except Exception:
# We have to continue popping frame from the queue even if
# we fail to show one frame
traceback.print_exc()
finally:
# Don't forget to unref the yuv frame. We don't want to
# starve the video buffer pool
yuv_frame.unref()
cv2.destroyWindow(window_name)
def fly(self):
# Takeoff, fly, land, ...
print("Takeoff if necessary...")
self.drone(
FlyingStateChanged(state="hovering", _policy="check")
| FlyingStateChanged(state="flying", _policy="check")
| (
GPSFixStateChanged(fixed=1, _timeout=10, _policy="check_wait")
>> (
TakeOff(_no_expect=True)
& FlyingStateChanged(
state="hovering", _timeout=10, _policy="check_wait")
)
)
).wait()
self.drone(MaxTilt(40)).wait().success()
for i in range(3):
print("Moving by ({}/3)...".format(i + 1))
self.drone(moveBy(10, 0, 0, math.pi, _timeout=20)).wait().success()
print("Landing...")
self.drone(
Landing()
>> FlyingStateChanged(state="landed", _timeout=5)
).wait()
print("Landed\n")
def postprocessing(self):
# Convert the raw .264 file into an .mp4 file
h264_filepath = os.path.join(self.tempd, 'h264_data.264')
mp4_filepath = os.path.join(self.tempd, 'h264_data.mp4')
subprocess.run(
shlex.split('ffmpeg -i {} -c:v copy -y {}'.format(
h264_filepath, mp4_filepath)),
check=True
)
# Replay this MP4 video file using the default video viewer (VLC?)
# subprocess.run(
# shlex.split('xdg-open {}'.format(mp4_filepath)),
# check=True
# )
if __name__ == "__main__":
streaming_example = StreamingExample()
# Start the video stream
streaming_example.start()
# Perform some live video processing while the drone is flying
streaming_example.fly()
# Stop the video stream
streaming_example.stop()
# Recorded video stream postprocessing
streaming_example.postprocessing()
