def _connect_impl(self):
# Use default values for connection json. If we want to changes values
# (or add new info), we just need to add them in req (using json format)
# For instance:
req = bytes('{ "%s": "%s", "%s": "%s", "%s": "%s"}' % (
"arstream2_client_stream_port", PDRAW_LOCAL_STREAM_PORT,
"arstream2_client_control_port", PDRAW_LOCAL_CONTROL_PORT,
"arstream2_supported_metadata_version", "1"), 'utf-8')
device_id = b""
device_conn_cfg = od.struct_arsdk_device_conn_cfg(
ctypes.create_string_buffer(b"arsdk-ng"), ctypes.create_string_buffer(b"desktop"),
ctypes.create_string_buffer(bytes(device_id)), ctypes.create_string_buffer(req))
# Send connection command
self._connect_future = Future(self._thread_loop)
res = od.arsdk_device_connect(
self._device.arsdk_device,
device_conn_cfg,
self._device_cbs_cfg,
self._thread_loop.pomp_loop
)
if res != 0:
self.logger.error(f"Error while connecting: {res}")
self._connect_future.set_result(False)
else:
self.logger.info("Connection in progress...")
return self._connect_future
def _send_command_impl(self, message, args, quiet=False):
"""
Must be run from the pomp loop
"""
argv = message._encode_args(*args.values())
# Check if we are already sending a command.
# if it the case, wait for the lock to be released
# Define an Arsdk structure
cmd = od.struct_arsdk_cmd()
# Find the description of the command in libarsdk.so
command_desc = od.struct_arsdk_cmd_desc.in_dll(
od._libraries["libarsdk.so"], message.g_arsdk_cmd_desc) # pylint: disable=E1101
# argv is an array of struct_arsdk_value
argc = argv._length_
# Encode the command
res = od.arsdk_cmd_enc_argv(ctypes.pointer(cmd),
ctypes.pointer(command_desc), argc, argv)
if res != 0:
self.logger.error(
f"Error while encoding command {message.fullName}: {res}")
else:
self.logger.debug(f"Command {message.fullName} has been encoded")
# cmd_itf must exist to send command
if self._cmd_itf is None:
raise RuntimeError(
"[sendcmd] Error cmd interface seems to be destroyed")
# Send the command
send_command_future = Future(self._thread_loop)
send_status_userdata = ctypes.pointer(
ctypes.py_object((send_command_future, message, args)))
self._send_status_userdata[id(
send_command_future)] = send_status_userdata
res = od.arsdk_cmd_itf_send(self._cmd_itf, ctypes.pointer(cmd),
self._send_status, send_status_userdata)
if res != 0:
self.logger.error(f"Error while sending command: {res}")
send_command_future.set_result(False)
return send_command_future
event = ArsdkMessageEvent(message, args)
# Update the currently monitored expectations
self._scheduler.process_event(event)
log_msg = f"{event} has been sent to the device"
if quiet:
self.logger.debug(log_msg)
else:
self.logger.info(log_msg)
return send_command_future
def _ready_to_play(self, pdraw, demuxer, ready, userdata):
self.logger.info(f"_ready_to_play({ready}) called")
self._is_ready_to_play = bool(ready)
if self._is_ready_to_play:
if self._play_resp_future.done():
self._play_resp_future = Future(self.pdraw_thread_loop)
self._play_impl()
if self.start_callback is not None:
self.callbacks_thread_loop.run_async(self.start_callback)
else:
if self.end_callback is not None:
self.callbacks_thread_loop.run_async(self.end_callback)
def _disconnection_impl(self):
f = Future(self._thread_loop)
if not self.connected:
return f.set_result(True)
self._disconnect_future = f
res = od.arsdk_device_disconnect(self._device.arsdk_device)
if res != 0:
self.logger.error(
f"Error while disconnecting from device: {self._ip_addr} ({res})")
self._disconnect_future.set_result(False)
else:
self.logger.info(
f"disconnected from device: {self._ip_addr}")
return self._disconnect_future
def pause(self):
"""
Pause the currently playing video
"""
if self.pdraw is None:
self.logger.error("Error Pdraw interface seems to be destroyed")
self._pause_resp_future.set_result(False)
return self._pause_resp_future
if self._pause_resp_future.done():
self._pause_resp_future = Future(self.pdraw_thread_loop)
if self.state is PdrawState.Playing:
self.pdraw_thread_loop.run_async(self._pause_impl)
elif self.state is PdrawState.Closed:
# Pause an closed stream is OK
self._pause_resp_future.set_result(True)
else:
self.logger.error(
f"Cannot pause stream from the {self.state} state")
self._pause_resp_future.set_result(False)
return self._pause_resp_future
def _open_stream(self):
"""
Opening pdraw stream using an rtsp url
"""
self._open_resp_future = Future(self.pdraw_thread_loop)
if self.state not in (PdrawState.Error, PdrawState.Stopped,
PdrawState.Closed, PdrawState.Created):
self.logger.warning(f"Cannot open stream from {self.state}")
self._open_resp_future.set_result(False)
return self._open_resp_future
self.state = PdrawState.Opening
if not self.pdraw and not self._pdraw_new():
self._open_resp_future.set_result(False)
return self._open_resp_future
ret = self._open_url()
if not ret:
self._open_resp_future.set_result(False)
return self._open_resp_future
def _stop(self):
if self._stop_resp_future.done():
self._stop_resp_future = Future(self.pdraw_thread_loop)
if not self.pdraw:
self._stop_resp_future.set_result(True)
return self._stop_resp_future
res = od.pdraw_stop(self.pdraw)
if res != 0:
self.logger.error(f"cannot stop pdraw session {res}")
self._stop_resp_future.set_result(False)
if self.callbacks_thread_loop.stop():
self.logger.info("pdraw callbacks thread loop stopped")
# cleanup some FDs from the callbacks thread loop that might
# have been lost
for stream in self.streams.values():
if stream['video_queue_event'] is not None:
self.logger.warning(
"cleanup leftover pdraw callbacks eventfds")
self.callbacks_thread_loop.remove_event_from_loop(
stream['video_queue_event'])
stream['video_queue_event'] = None
return self._stop_resp_future
def async_connect(self, *, timeout=None, later=False, retry=1):
if timeout is None:
timeout = 6 * retry
# If not already connected to a device
if self.connected:
f = Future(self._thread_loop)
f.set_result(True)
return f
if self._connected_future is None:
if not later:
self._connected_future = self._thread_loop.run_async(
self._do_connect, timeout, retry)
else:
self._connected_future = self._thread_loop.run_later(
self._do_connect, timeout, retry)
def _on_connect_done(f):
self._connected_future = None
self._connected_future.add_done_callback(_on_connect_done)
return self._connected_future
def close(self):
"""
Close a playing or paused video stream session
"""
if self.state is PdrawState.Closed:
f = Future(self.pdraw_thread_loop)
f.set_result(True)
if self.state in (PdrawState.Opened, PdrawState.Paused,
PdrawState.Playing, PdrawState.Error):
self.logger.debug(f"pdraw closing from the {self.state} state")
if self._close_resp_future.done():
self._close_resp_future = Future(self.pdraw_thread_loop)
self._close_resp_future.add_done_callback(
self._on_close_resp_done)
f = self._close_resp_future
self.state = PdrawState.Closing
self.pdraw_thread_loop.run_async(self._close_stream)
elif self.state is not PdrawState.Closing:
f = Future(self.pdraw_thread_loop)
f.set_result(False)
else:
f = self._close_resp_future
return f
def async_disconnect(self, *, timeout=5):
"""
Disconnects current device (if any)
Blocks until it is done or abandoned
:rtype: bool
"""
if not self.connected:
f = Future(self._thread_loop)
f.set_result(True)
self.logger.debug("Already disconnected")
return f
if self._connected_future is not None:
f = Future(self._thread_loop)
f.set_result(False)
self.logger.warning(
"Cannot disconnect while a connection is in progress")
return f
self.logger.debug("We are not disconnected yet")
disconnected = self._thread_loop.run_async(self._disconnection_impl)
return disconnected
class ControllerBase(CommandInterfaceBase):
def __init__(self,
ip_addr,
*,
name=None,
drone_type=0,
proto_v_min=1,
proto_v_max=3,
is_skyctrl=None,
video_buffer_queue_size=8,
media_autoconnect=True):
self._logger_scope = "controller"
self._ip_addr_str = str(ip_addr)
self._ip_addr = ip_addr.encode('utf-8')
self._is_skyctrl = is_skyctrl
self._piloting = False
self._piloting_command = PilotingCommand()
super().__init__(
name=name,
drone_type=drone_type,
proto_v_min=1,
proto_v_max=3)
self._connected_future = None
self._last_disconnection_time = None
# Setup piloting commands timer
self._piloting_timer = self._thread_loop.create_timer(
self._piloting_timer_cb)
def _recv_message_type(self):
return messages.ArsdkMessageType.CMD
def _create_backend(self, name, proto_v_min, proto_v_max):
self._backend = CtrlBackend(
name=name, proto_v_min=proto_v_min, proto_v_max=proto_v_max
)
def _declare_callbacks(self):
"""
Define all callbacks
"""
super()._declare_callbacks()
self._device_cbs_cfg = od.struct_arsdk_device_conn_cbs.bind({
"connecting": self._connecting_cb,
"connected": self._connected_cb,
"disconnected": self._disconnected_cb,
"canceled": self._canceled_cb,
"link_status": self._link_status_cb,
})
@callback_decorator()
def _create_command_interface(self):
"""
Create a command interface to send command to the device
"""
cmd_itf = od.POINTER_T(od.struct_arsdk_cmd_itf)()
res = od.arsdk_device_create_cmd_itf(
self._device.arsdk_device,
self._cmd_itf_cbs,
ctypes.pointer(cmd_itf))
if res != 0:
self.logger.error(f"Error while creating command interface: {res}")
cmd_itf = None
else:
self.logger.info("Command interface has been created")
return cmd_itf
@callback_decorator()
def _connecting_cb(self, _arsdk_device, arsdk_device_info, _user_data):
"""
Notify connection initiation.
"""
device_name = od.string_cast(arsdk_device_info.contents.name)
self.logger.info(f"Connecting to device: {device_name}")
@callback_decorator()
def _connected_cb(self, _arsdk_device, arsdk_device_info, _user_data):
"""
Notify connection completion.
"""
device_name = od.string_cast(arsdk_device_info.contents.name)
self.set_device_name(device_name)
self.logger.info(f"Connected to device: {device_name}")
json_info = od.string_cast(arsdk_device_info.contents.json)
try:
json_info = json.loads(json_info)
self.logger.info(pprint.pformat(json_info))
except ValueError:
self.logger.error(f'json contents cannot be parsed: {json_info}')
self.connected = True
if self._connect_future is not None and not self._connect_future.done():
self._connect_future.set_result(True)
@callback_decorator()
def _disconnected_cb(self, _arsdk_device, arsdk_device_info, _user_data):
"""
Notify disconnection.
"""
device_name = od.string_cast(arsdk_device_info.contents.name)
self.logger.info(f"Disconnected from device: {device_name}")
self.connected = False
if self._disconnect_future is not None and not self._disconnect_future.done():
self._disconnect_future.set_result(True)
self._thread_loop.run_later(self._on_device_removed)
@callback_decorator()
def _canceled_cb(self, _arsdk_device, arsdk_device_info, reason, _user_data):
"""
Notify connection cancellation. Either because 'disconnect' was
called before 'connected' callback or remote aborted/rejected the
request.
"""
device_name = od.string_cast(arsdk_device_info.contents.name)
reason = od.string_cast(
od.arsdk_conn_cancel_reason_str(reason))
self.logger.info(
f"Connection to device: {device_name} has been canceled for reason: {reason}")
if self._connect_future is not None and not self._connect_future.done():
self._connect_future.set_result(False)
self._thread_loop.run_later(self._on_device_removed)
@callback_decorator()
def _link_status_cb(self, _arsdk_device, _arsdk_device_info, status, _user_data):
"""
Notify link status. At connection completion, it is assumed to be
initially OK. If called with KO, user is responsible to take action.
It can either wait for link to become OK again or disconnect
immediately. In this case, call arsdk_device_disconnect and the
'disconnected' callback will be called.
"""
self.logger.info(f"Link status: {status}")
# If link has been lost, we must start disconnection procedure
if status == od.ARSDK_LINK_STATUS_KO:
# the device has been disconnected
self.connected = False
self._thread_loop.run_later(self._on_device_removed)
@callback_decorator()
def _disconnection_impl(self):
f = Future(self._thread_loop)
if not self.connected:
return f.set_result(True)
self._disconnect_future = f
res = od.arsdk_device_disconnect(self._device.arsdk_device)
if res != 0:
self.logger.error(
f"Error while disconnecting from device: {self._ip_addr} ({res})")
self._disconnect_future.set_result(False)
else:
self.logger.info(
f"disconnected from device: {self._ip_addr}")
return self._disconnect_future
def _synchronize_clock(self):
date_time = datetime.datetime.now(
get_localzone()).strftime("%Y%m%dT%H%M%S%z")
if not self._is_skyctrl:
current_date_time = common.Common.CurrentDateTime
else:
current_date_time = skyctrl.Common.CurrentDateTime
res = self(current_date_time(datetime=date_time, _timeout=0.5))
def _on_sync_done(res):
if not res.success():
msg = "Time synchronization failed for {}".format(
self._ip_addr)
self.logger.warning(msg)
else:
self.logger.info("Synchronization of {} at {}".format(
self._ip_addr, date_time))
res.add_done_callback(_on_sync_done)
@callback_decorator()
def _start_piloting_impl(self):
delay = 100
period = 25
ok = self._thread_loop.set_timer(self._piloting_timer, delay, period)
if ok:
self._piloting = True
self.logger.info(
"Piloting interface has been correctly launched")
else:
self.logger.error("Unable to launch piloting interface")
return self._piloting
@callback_decorator()
def _stop_piloting_impl(self):
# Stop the drone movements
self._piloting_command.set_default_piloting_command()
time.sleep(0.1)
ok = self._thread_loop.clear_timer(self._piloting_timer)
if ok:
# Reset piloting state value to False
self._piloting = False
self.logger.info("Piloting interface stopped")
else:
self.logger.error("Unable to stop piloting interface")
@callback_decorator()
def _piloting_timer_cb(self, timer, _user_data):
self.logger.debug(f"piloting timer callback: {timer}")
if self.connected:
self._send_piloting_command()
def _send_piloting_command(self):
# When piloting time is 0 => send default piloting commands
if self._piloting_command.piloting_time:
# Check if piloting time since last pcmd order has been reached
diff_time = (
datetime.datetime.now() -
self._piloting_command.initial_time
)
if diff_time.total_seconds() >= self._piloting_command.piloting_time:
self._piloting_command.set_default_piloting_command()
# Flag to activate movement on roll and pitch. 1 activate, 0 deactivate
if self._piloting_command.roll or (
self._piloting_command.pitch
):
activate_movement = 1
else:
activate_movement = 0
self._send_command_impl(
ardrone3.Piloting.PCMD,
dict(
flag=activate_movement,
roll=self._piloting_command.roll,
pitch=self._piloting_command.pitch,
yaw=self._piloting_command.yaw,
gaz=self._piloting_command.gaz,
timestampAndSeqNum=0,
),
quiet=True,
)
def start_piloting(self):
"""
Start interface to send piloting commands
:rtype: bool
"""
if self._piloting:
self.logger.debug("Piloting interface already started")
return True
f = self._thread_loop.run_async(self._start_piloting_impl)
try:
ok = f.result_or_cancel(timeout=2)
except (FutureTimeoutError, CancelledError):
self.logger.error("Unable to launch piloting interface")
return False
if not ok:
self.logger.error("Unable to launch piloting interface")
return False
self.logger.info("Piloting started")
return True
def stop_piloting(self):
"""
Stop interface to send piloting commands
:rtype: bool
"""
# Check piloting interface is running
if not self._piloting:
self.logger.debug("Piloting interface already stopped")
return True
f = self._thread_loop.run_async(self._stop_piloting_impl)
try:
ok = f.result_or_cancel(timeout=2)
except (FutureTimeoutError, CancelledError):
self.logger.error("Unable to stop piloting interface")
return False
if not ok:
self.logger.error("Unable to stop piloting interface")
return False
self.logger.info("Piloting stopped")
return True
def piloting(self, roll, pitch, yaw, gaz, piloting_time):
"""
Send manual piloting commands to the drone.
This function is a non-blocking.
:type roll: int
:param roll: roll consign for the drone (must be in [-100:100])
:type pitch: int
:param pitch: pitch consign for the drone (must be in [-100:100])
:type yaw: int
:param yaw: yaw consign for the drone (must be in [-100:100])
:type gaz: int
:param gaz: gaz consign for the drone (must be in [-100:100])
:type piloting_time: float
:param piloting_time: The time of the piloting command
:rtype: bool
"""
if not self.start_piloting():
return False
self._piloting_command.update_piloting_command(roll, pitch, yaw, gaz, piloting_time)
return True
def piloting_pcmd(self, roll, pitch, yaw, gaz, piloting_time):
warn(
"ControllerBase.piloting_pcmd is deprecated, "
"please use ControllerBase.piloting instead",
DeprecationWarning
)
return self.piloting(roll, pitch, yaw, gaz, piloting_time)
async def _async_discover_device(self):
# Try to identify the device type we are attempting to connect to...
discovery = DiscoveryNet(self._backend, ip_addr=self._ip_addr)
device = await discovery.async_get_device()
if device is None:
self.logger.info(f"Net discovery failed for {self._ip_addr}")
self.logger.info(f"Trying 'NetRaw' discovery for {self._ip_addr} ...")
assert discovery.stop()
discovery = DiscoveryNetRaw(self._backend, ip_addr=self._ip_addr)
device = await discovery.async_get_device()
if device is None:
discovery.stop()
return device, discovery
async def _async_get_device(self):
if self._device is not None:
return True
device, discovery = await self._async_discover_device()
if device is None:
self.logger.info(f"Unable to discover the device: {self._ip_addr}")
return False
# Save device related info
self._device = device
self._discovery = discovery
self._device_type = self._device.type
if self._is_skyctrl is None:
if self._device_type in SKYCTRL_DEVICE_TYPE_LIST:
self._is_skyctrl = True
else:
self._is_skyctrl = False
return True
@callback_decorator()
def _connect_impl(self):
# Use default values for connection json. If we want to changes values
# (or add new info), we just need to add them in req (using json format)
# For instance:
req = bytes('{ "%s": "%s", "%s": "%s", "%s": "%s"}' % (
"arstream2_client_stream_port", PDRAW_LOCAL_STREAM_PORT,
"arstream2_client_control_port", PDRAW_LOCAL_CONTROL_PORT,
"arstream2_supported_metadata_version", "1"), 'utf-8')
device_id = b""
device_conn_cfg = od.struct_arsdk_device_conn_cfg(
ctypes.create_string_buffer(b"arsdk-ng"), ctypes.create_string_buffer(b"desktop"),
ctypes.create_string_buffer(bytes(device_id)), ctypes.create_string_buffer(req))
# Send connection command
self._connect_future = Future(self._thread_loop)
res = od.arsdk_device_connect(
self._device.arsdk_device,
device_conn_cfg,
self._device_cbs_cfg,
self._thread_loop.pomp_loop
)
if res != 0:
self.logger.error(f"Error while connecting: {res}")
self._connect_future.set_result(False)
else:
self.logger.info("Connection in progress...")
return self._connect_future
async def _do_connect(self, timeout, retry):
grace_period = 5.
if self._last_disconnection_time is not None:
last_disconnection = (time.time() - self._last_disconnection_time)
if last_disconnection < grace_period:
await self._thread_loop.asleep(grace_period - last_disconnection)
# the deadline does not include the grace period
deadline = time.time() + timeout
backoff = 2.
for i in range(retry):
if self.connected:
# previous connection attempt timedout but we're connected..
break
if deadline < time.time():
self.logger.error(f"'{self._ip_addr_str} connection timed out")
return False
self.logger.debug(f"Discovering device {self._ip_addr_str} ...")
if not await self._async_get_device():
self.logger.debug(f"Discovering device {self._ip_addr_str} failed")
await self._thread_loop.asleep(backoff)
backoff *= 2.
continue
self.logger.debug(f"Connecting device {self._ip_addr_str} ...")
connected = await self._connect_impl()
if not connected:
self.logger.debug(f"Connecting device {self._ip_addr_str} failed")
await self._thread_loop.asleep(backoff)
backoff *= 2.
continue
# We're connected
break
else:
self.logger.error(f"'{self._ip_addr_str} connection retries failed")
return False
# Create the arsdk command interface
if self._cmd_itf is None:
self.logger.debug(f"Creating cmd_itf for {self._ip_addr_str} ...")
self._cmd_itf = self._create_command_interface()
if self._cmd_itf is None:
self.logger.error(f"Creating cmd_itf for {self._ip_addr_str} failed")
self.disconnect()
return False
if self.connected:
return await self._on_connected(deadline)
else:
return False
async def _send_states_settings_cmd(self, command):
res = await self(command)
if not res.success():
self.logger.error(
f"Unable get device state/settings: {command} "
f"for {self._ip_addr}")
self.disconnect()
return False
return True
async def _on_connected(self, deadline):
if not self._ip_addr_str.startswith("10.202") and (
not self._ip_addr_str.startswith("127.0")):
self._synchronize_clock()
# We're connected to the device, get all device states and settings if necessary
if not self._is_skyctrl:
all_states_settings_commands = [
common.Common.AllStates(), common.Settings.AllSettings()]
else:
all_states_settings_commands = [
skyctrl.Common.AllStates(), skyctrl.Settings.AllSettings()]
# Get device specific states and settings
timeout = deadline - time.time()
for states_settings_command in all_states_settings_commands:
res = await self._thread_loop.await_for(
timeout,
self._send_states_settings_cmd, states_settings_command
)
if not res:
return False
if self._is_skyctrl:
# If the skyctrl is connected to a drone get the drone states and settings too
if self(drone_manager.connection_state(
state="connected", _policy="check")):
all_states = await self(
common.CommonState.AllStatesChanged() &
common.SettingsState.AllSettingsChanged(),
)
if not all_states.success():
self.logger.error("Unable get connected drone states and/or settings")
return False
# Enable airsdk mission support from the drone
if not await self(mission.custom_msg_enable()):
self.logger.error("Failed to send mission.custom_msg_enable")
return False
else:
# Enable airsdk mission support from the drone
if not await self(mission.custom_msg_enable()):
self.logger.error("Failed to send mission.custom_msg_enable")
return False
# Process the ConnectedEvent
event = ConnectedEvent()
self.logger.info(str(event))
self._scheduler.process_event(event)
return True
def async_connect(self, *, timeout=None, later=False, retry=1):
if timeout is None:
timeout = 6 * retry
# If not already connected to a device
if self.connected:
f = Future(self._thread_loop)
f.set_result(True)
return f
if self._connected_future is None:
if not later:
self._connected_future = self._thread_loop.run_async(
self._do_connect, timeout, retry)
else:
self._connected_future = self._thread_loop.run_later(
self._do_connect, timeout, retry)
def _on_connect_done(f):
self._connected_future = None
self._connected_future.add_done_callback(_on_connect_done)
return self._connected_future
def connect(self, *, timeout=None, retry=1):
"""
Make all step to make the connection between the device and the controller
:param timeout: the global connection timeout in seconds (including the
retried connection attempt duration when `retry` > 1)
:param retry: the number of connection attempts (default to `1`)
:rtype: bool
"""
connected_future = self.async_connect(timeout=timeout, retry=retry)
try:
connected_future.result_or_cancel(timeout=timeout)
except (FutureTimeoutError, CancelledError):
self.logger.error(f"'{self._ip_addr_str} connection timed out")
# If the connection timedout we must disconnect
self.disconnect()
return self.connected
def _on_device_removed(self):
if self._discovery:
self._discovery.stop()
if self._piloting:
self._stop_piloting_impl()
self._disconnection_impl()
self._last_disconnection_time = time.time()
self._piloting_command = PilotingCommand()
super()._on_device_removed()
def _reset_instance(self):
self._piloting = False
self._piloting_command = PilotingCommand()
self._device = None
self._device_name = None
self._discovery = None
self._connected_future = None
super()._reset_instance()
def __init__(
self,
name=None,
device_name=None,
server_addr=None,
buffer_queue_size=8,
pdraw_thread_loop=None,
):
"""
:param name: (optional) pdraw client name (used by Olympe logs)
:type name: str
:param device_name: (optional) the drone device name
(used by Olympe logs)
:type device_name: str
:param buffer_queue_size: (optional) video buffer queue size
(defaults to 8)
:type buffer_queue_size: int
"""
super().__init__(name, device_name, "pdraw")
if pdraw_thread_loop is None:
self.own_pdraw_thread_loop = True
self.pdraw_thread_loop = PompLoopThread(self.logger)
self.pdraw_thread_loop.start()
else:
self.own_pdraw_thread_loop = False
self.pdraw_thread_loop = pdraw_thread_loop
self.callbacks_thread_loop = PompLoopThread(
self.logger, parent=self.pdraw_thread_loop)
self.callbacks_thread_loop.start()
self.buffer_queue_size = buffer_queue_size
self.pomp_loop = self.pdraw_thread_loop.pomp_loop
self._open_resp_future = Future(self.pdraw_thread_loop)
self._close_resp_future = Future(self.pdraw_thread_loop)
self._close_resp_future.add_done_callback(self._on_close_resp_done)
self._play_resp_future = Future(self.pdraw_thread_loop)
self._pause_resp_future = Future(self.pdraw_thread_loop)
self._stop_resp_future = Future(self.pdraw_thread_loop)
self._state = PdrawState.Created
self._state_lock = threading.Lock()
self._state_wait_events = {k: list() for k in PdrawState}
self.pdraw = od.POINTER_T(od.struct_pdraw)()
self.pdraw_demuxer = od.POINTER_T(od.struct_pdraw_demuxer)()
self.streams = defaultdict(StreamFactory)
self.session_metadata = {}
self.outfiles = {
od.VDEF_FRAME_TYPE_CODED: {
'data': None,
'meta': None,
'info': None,
},
od.VDEF_FRAME_TYPE_RAW: {
'data': None,
'meta': None,
'info': None,
},
}
self.frame_callbacks = {
(od.VDEF_FRAME_TYPE_CODED, od.VDEF_CODED_DATA_FORMAT_AVCC): None,
(od.VDEF_FRAME_TYPE_CODED, od.VDEF_CODED_DATA_FORMAT_BYTE_STREAM):
None,
(od.VDEF_FRAME_TYPE_RAW, None): None,
}
self.start_callback = None
self.end_callback = None
self.flush_callbacks = {
od.VDEF_FRAME_TYPE_CODED: None,
od.VDEF_FRAME_TYPE_RAW: None,
}
self.url = None
if server_addr is None:
server_addr = "192.168.42.1"
self.server_addr = server_addr
self.resource_name = "live"
self.media_name = None
self.demuxer_cbs = od.struct_pdraw_demuxer_cbs.bind({
"open_resp":
self._open_resp,
"close_resp":
self._close_resp,
"unrecoverable_error":
self._unrecoverable_error,
"ready_to_play":
self._ready_to_play,
"play_resp":
self._play_resp,
"pause_resp":
self._pause_resp,
"seek_resp":
self._seek_resp,
"select_media":
self._select_media,
"end_of_range":
self._end_of_range,
})
self.pdraw_cbs = od.struct_pdraw_cbs.bind({
"socket_created": self._socket_created,
"media_added": self._media_added,
"media_removed": self._media_removed,
"stop_resp": self.stop_resp,
})
self.video_sink_vt = {
od.VDEF_FRAME_TYPE_CODED: _CodedVideoSink,
od.VDEF_FRAME_TYPE_RAW: _RawVideoSink,
}
self.pdraw_thread_loop.register_cleanup(self._dispose)
class Pdraw(LogMixin):
def __init__(
self,
name=None,
device_name=None,
server_addr=None,
buffer_queue_size=8,
pdraw_thread_loop=None,
):
"""
:param name: (optional) pdraw client name (used by Olympe logs)
:type name: str
:param device_name: (optional) the drone device name
(used by Olympe logs)
:type device_name: str
:param buffer_queue_size: (optional) video buffer queue size
(defaults to 8)
:type buffer_queue_size: int
"""
super().__init__(name, device_name, "pdraw")
if pdraw_thread_loop is None:
self.own_pdraw_thread_loop = True
self.pdraw_thread_loop = PompLoopThread(self.logger)
self.pdraw_thread_loop.start()
else:
self.own_pdraw_thread_loop = False
self.pdraw_thread_loop = pdraw_thread_loop
self.callbacks_thread_loop = PompLoopThread(
self.logger, parent=self.pdraw_thread_loop)
self.callbacks_thread_loop.start()
self.buffer_queue_size = buffer_queue_size
self.pomp_loop = self.pdraw_thread_loop.pomp_loop
self._open_resp_future = Future(self.pdraw_thread_loop)
self._close_resp_future = Future(self.pdraw_thread_loop)
self._close_resp_future.add_done_callback(self._on_close_resp_done)
self._play_resp_future = Future(self.pdraw_thread_loop)
self._pause_resp_future = Future(self.pdraw_thread_loop)
self._stop_resp_future = Future(self.pdraw_thread_loop)
self._state = PdrawState.Created
self._state_lock = threading.Lock()
self._state_wait_events = {k: list() for k in PdrawState}
self.pdraw = od.POINTER_T(od.struct_pdraw)()
self.pdraw_demuxer = od.POINTER_T(od.struct_pdraw_demuxer)()
self.streams = defaultdict(StreamFactory)
self.session_metadata = {}
self.outfiles = {
od.VDEF_FRAME_TYPE_CODED: {
'data': None,
'meta': None,
'info': None,
},
od.VDEF_FRAME_TYPE_RAW: {
'data': None,
'meta': None,
'info': None,
},
}
self.frame_callbacks = {
(od.VDEF_FRAME_TYPE_CODED, od.VDEF_CODED_DATA_FORMAT_AVCC): None,
(od.VDEF_FRAME_TYPE_CODED, od.VDEF_CODED_DATA_FORMAT_BYTE_STREAM):
None,
(od.VDEF_FRAME_TYPE_RAW, None): None,
}
self.start_callback = None
self.end_callback = None
self.flush_callbacks = {
od.VDEF_FRAME_TYPE_CODED: None,
od.VDEF_FRAME_TYPE_RAW: None,
}
self.url = None
if server_addr is None:
server_addr = "192.168.42.1"
self.server_addr = server_addr
self.resource_name = "live"
self.media_name = None
self.demuxer_cbs = od.struct_pdraw_demuxer_cbs.bind({
"open_resp":
self._open_resp,
"close_resp":
self._close_resp,
"unrecoverable_error":
self._unrecoverable_error,
"ready_to_play":
self._ready_to_play,
"play_resp":
self._play_resp,
"pause_resp":
self._pause_resp,
"seek_resp":
self._seek_resp,
"select_media":
self._select_media,
"end_of_range":
self._end_of_range,
})
self.pdraw_cbs = od.struct_pdraw_cbs.bind({
"socket_created": self._socket_created,
"media_added": self._media_added,
"media_removed": self._media_removed,
"stop_resp": self.stop_resp,
})
self.video_sink_vt = {
od.VDEF_FRAME_TYPE_CODED: _CodedVideoSink,
od.VDEF_FRAME_TYPE_RAW: _RawVideoSink,
}
self.pdraw_thread_loop.register_cleanup(self._dispose)
@property
def state(self):
"""
Return the current Pdraw state
:rtype: PdrawState
"""
return self._state
@state.setter
def state(self, value):
with self._state_lock:
self._state = value
for event in self._state_wait_events[self._state]:
event.set()
self._state_wait_events[self._state] = []
def wait(self, state, timeout=None):
"""
Wait for the provided Pdraw state
This function returns True when the requested state is reached or False
if the timeout duration is reached.
If the requested state is already reached, this function returns True
immediately.
This function may block indefinitely when called without a timeout
value.
:type state: PdrawState
:param timeout: the timeout duration in seconds or None (the default)
:type timeout: float
:rtype: bool
"""
with self._state_lock:
if self._state == state:
return True
event = threading.Event()
self._state_wait_events[state].append(event)
return event.wait(timeout=timeout)
def dispose(self):
return self.pdraw_thread_loop.run_later(self._dispose)
def destroy(self):
self.callbacks_thread_loop.stop()
try:
self.dispose().result_or_cancel(timeout=2.)
except FutureTimeoutError:
self.logger.error("Pdraw.destroy() timedout")
self.pdraw_thread_loop.stop()
@callback_decorator()
def _dispose(self):
self.pdraw_thread_loop.unregister_cleanup(self._dispose,
ignore_error=True)
return self.pdraw_thread_loop.run_async(self._dispose_impl)
@callback_decorator()
def _dispose_impl(self):
f = self.close().then(lambda _: self._stop(), )
return f
@callback_decorator()
def _stop(self):
if self._stop_resp_future.done():
self._stop_resp_future = Future(self.pdraw_thread_loop)
if not self.pdraw:
self._stop_resp_future.set_result(True)
return self._stop_resp_future
res = od.pdraw_stop(self.pdraw)
if res != 0:
self.logger.error(f"cannot stop pdraw session {res}")
self._stop_resp_future.set_result(False)
if self.callbacks_thread_loop.stop():
self.logger.info("pdraw callbacks thread loop stopped")
# cleanup some FDs from the callbacks thread loop that might
# have been lost
for stream in self.streams.values():
if stream['video_queue_event'] is not None:
self.logger.warning(
"cleanup leftover pdraw callbacks eventfds")
self.callbacks_thread_loop.remove_event_from_loop(
stream['video_queue_event'])
stream['video_queue_event'] = None
return self._stop_resp_future
@callback_decorator()
def stop_resp(self, pdraw, status, userdata):
if status != 0:
self.logger.error(f"_stop_resp called {status}")
self._stop_resp_future.set_result(False)
self.state = PdrawState.Error
else:
self.logger.info(f"_stop_resp called {status}")
self._stop_resp_future.set_result(True)
self.state = PdrawState.Stopped
return self._stop_resp_future
def _destroy_pdraw(self):
ret = True
if self.pdraw_demuxer:
if not self.pdraw:
self.logger.error(
"Cannot destroy pdraw demuxer: a NULL pdraw session")
return False
self.logger.info("destroying pdraw demuxer...")
res = od.pdraw_demuxer_destroy(self.pdraw, self.pdraw_demuxer)
if res != 0:
self.logger.error(f"cannot destroy pdraw demuxer {res}")
ret = False
else:
self.logger.info("pdraw demuxer destroyed")
self.pdraw_demuxer = od.POINTER_T(od.struct_pdraw_demuxer)()
if self.pdraw:
self.logger.info("destroying pdraw...")
res = od.pdraw_destroy(self.pdraw)
if res != 0:
self.logger.error(f"cannot destroy pdraw {res}")
ret = False
else:
self.logger.info("pdraw destroyed")
self.pdraw = od.POINTER_T(od.struct_pdraw)()
return ret
def _open_url(self):
"""
Opening rtsp streaming url
"""
if self.resource_name.startswith("replay/"):
if self.media_name is None:
self.logger.error(
"Error media_name should be provided in video stream "
"replay mode")
return False
res = od.pdraw_demuxer_new_from_url(
self.pdraw, self.url, self.demuxer_cbs,
ctypes.cast(ctypes.pointer(ctypes.py_object(self)),
ctypes.c_void_p), ctypes.byref(self.pdraw_demuxer))
if res != 0:
self.logger.error(
f"Error while opening pdraw url: {self.url} ({res})")
return False
else:
self.logger.info(f"Opening pdraw url OK: {self.url}")
return True
@callback_decorator()
def _open_stream(self):
"""
Opening pdraw stream using an rtsp url
"""
self._open_resp_future = Future(self.pdraw_thread_loop)
if self.state not in (PdrawState.Error, PdrawState.Stopped,
PdrawState.Closed, PdrawState.Created):
self.logger.warning(f"Cannot open stream from {self.state}")
self._open_resp_future.set_result(False)
return self._open_resp_future
self.state = PdrawState.Opening
if not self.pdraw and not self._pdraw_new():
self._open_resp_future.set_result(False)
return self._open_resp_future
ret = self._open_url()
if not ret:
self._open_resp_future.set_result(False)
return self._open_resp_future
def close(self):
"""
Close a playing or paused video stream session
"""
if self.state is PdrawState.Closed:
f = Future(self.pdraw_thread_loop)
f.set_result(True)
if self.state in (PdrawState.Opened, PdrawState.Paused,
PdrawState.Playing, PdrawState.Error):
self.logger.debug(f"pdraw closing from the {self.state} state")
if self._close_resp_future.done():
self._close_resp_future = Future(self.pdraw_thread_loop)
self._close_resp_future.add_done_callback(
self._on_close_resp_done)
f = self._close_resp_future
self.state = PdrawState.Closing
self.pdraw_thread_loop.run_async(self._close_stream)
elif self.state is not PdrawState.Closing:
f = Future(self.pdraw_thread_loop)
f.set_result(False)
else:
f = self._close_resp_future
return f
@callback_decorator()
def _close_stream(self):
"""
Close pdraw stream
"""
if self.state is PdrawState.Closed:
self.logger.info("pdraw is already closed")
self._close_resp_future.set_result(True)
return self._close_resp_future
if not self.pdraw:
self.logger.error("Error Pdraw interface seems to be destroyed")
self.state = PdrawState.Error
self._close_resp_future.set_result(False)
return self._close_resp_future
if not self._close_stream_impl():
self.state = PdrawState.Error
self._close_resp_future.set_result(False)
return self._close_resp_future
def _close_stream_impl(self):
res = od.pdraw_demuxer_close(self.pdraw, self.pdraw_demuxer)
if res != 0:
self.logger.error(f"Error while closing pdraw demuxer: {res}")
self.state = PdrawState.Error
return False
else:
self.logger.info("Closing pdraw demuxer OK")
return True
@callback_decorator()
def _on_close_resp_done(self, close_resp_future):
if not close_resp_future.cancelled():
return
close_resp_future.cancel()
self.logger.error("Closing pdraw demuxer timedout")
if self.state not in (PdrawState.Closed, PdrawState.Stopped,
PdrawState.Error):
return
# FIXME: workaround pdraw closing timeout
# This random issue is quiet hard to reproduce
self.logger.error("destroying pdraw demuxer...")
if self.pdraw and self.pdraw_demuxer:
self.pdraw_thread_loop.run_later(
od.pdraw_demuxer_destroy, self.pdraw,
self.pdraw_demuxer).then(lambda _: self._stop())
self.pdraw = od.POINTER_T(od.struct_pdraw)()
self.state = PdrawState.Closed
def _open_resp(self, pdraw, demuxer, status, userdata):
self.logger.debug("_open_resp called")
if status != 0:
self.state = PdrawState.Error
else:
self.state = PdrawState.Opened
self._open_resp_future.set_result(status == 0)
def _close_resp(self, pdraw, demuxer, status, userdata):
self._close_output_files()
if status != 0:
self.logger.error(f"_close_resp called {status}")
self._close_resp_future.set_result(False)
self.state = PdrawState.Error
else:
self.logger.debug(f"_close_resp called {status}")
self.state = PdrawState.Closed
self._close_resp_future.set_result(True)
if not self._open_resp_future.done():
self._open_resp_future.set_result(False)
if demuxer == self.pdraw_demuxer:
self.pdraw_demuxer = od.POINTER_T(od.struct_pdraw_demuxer)()
res = od.pdraw_demuxer_destroy(pdraw, demuxer)
if res != 0:
self.logger.error(f"pdraw_demuxer_destroy: {res}")
else:
self.logger.debug(f"pdraw_demuxer_destroy: {res}")
def _pdraw_new(self):
res = od.pdraw_new(
self.pomp_loop, self.pdraw_cbs,
ctypes.cast(ctypes.pointer(ctypes.py_object(self)),
ctypes.c_void_p), ctypes.byref(self.pdraw))
if res != 0:
self.logger.error(f"Error while creating pdraw interface: {res}")
self.pdraw = od.POINTER_T(od.struct_pdraw)()
return False
else:
self.logger.info("Pdraw interface has been created")
return True
def _unrecoverable_error(self, pdraw, demuxer, userdata):
self.logger.error("_unrecoverable_error() -> pdraw teardown...")
# remove every video sinks
for id_ in self.streams:
self._video_sink_flush_impl(id_)
self._media_removed_impl(id_)
# demuxer.close -> demuxer.destroy
if self.pdraw and self.pdraw_demuxer:
od.pdraw_demuxer_close(self.pdraw, self.pdraw_demuxer)
# the demuxer will be destroyed in close_resp
self.pdraw_demuxer = od.POINTER_T(od.struct_pdraw_demuxer)()
self.logger.error("_unrecoverable_error() -> pdraw teardown done")
# we should be good to go again with a Pdraw.play()
self._state = PdrawState.Created
def _ready_to_play(self, pdraw, demuxer, ready, userdata):
self.logger.info(f"_ready_to_play({ready}) called")
self._is_ready_to_play = bool(ready)
if self._is_ready_to_play:
if self._play_resp_future.done():
self._play_resp_future = Future(self.pdraw_thread_loop)
self._play_impl()
if self.start_callback is not None:
self.callbacks_thread_loop.run_async(self.start_callback)
else:
if self.end_callback is not None:
self.callbacks_thread_loop.run_async(self.end_callback)
def _play_resp(self, pdraw, demuxer, status, timestamp, speed, userdata):
if status == 0:
self.logger.debug(f"_play_resp called {status}")
self.state = PdrawState.Playing
if not self._play_resp_future.done():
self._play_resp_future.set_result(True)
else:
self.logger.error(f"_play_resp called {status}")
self.state = PdrawState.Error
if not self._play_resp_future.done():
self._play_resp_future.set_result(False)
def _pause_resp(self, pdraw, demuxer, status, timestamp, userdata):
if status == 0:
self.logger.debug(f"_pause_resp called {status}")
self.state = PdrawState.Paused
self._pause_resp_future.set_result(True)
else:
self.logger.error(f"_pause_resp called {status}")
self.state = PdrawState.Error
self._pause_resp_future.set_result(False)
def _seek_resp(self, pdraw, demuxer, status, timestamp, userdata):
if status == 0:
self.logger.debug(f"_seek_resp called {status}")
else:
self.logger.error(f"_seek_resp called {status}")
self.state = PdrawState.Error
def _socket_created(self, pdraw, fd, userdata):
self.logger.debug("_socket_created called")
def _select_media(self, pdraw, demuxer, medias, count, userdata):
# by default select the default media (media_id=0)
selected_media_id = 0
selected_media_idx = 0
default_media_id = 0
default_media_idx = 0
for idx in range(count):
self.logger.info(f"_select_media: "
f"idx={idx} media_id={medias[idx].media_id} "
f"name={od.string_cast(medias[idx].name)} "
f"default={str(bool(medias[idx].is_default))}")
if bool(medias[idx].is_default):
default_media_id = medias[idx].media_id
default_media_idx = idx
if (self.media_name is not None
and self.media_name == od.string_cast(medias[idx].name)):
selected_media_id = medias[idx].media_id
selected_media_idx = idx
if (self.media_name is not None and od.string_cast(
medias[selected_media_idx].name) != self.media_name):
default_media_name = od.string_cast(medias[default_media_idx].name)
self.logger.warning(
f"media_name {self.media_name} is unavailable. "
f"Selecting the default media instead: {default_media_name}")
self.session_metadata = od.struct_vmeta_session.as_dict(
medias[default_media_idx].session_meta)
else:
self.session_metadata = od.struct_vmeta_session.as_dict(
medias[selected_media_idx].session_meta)
if selected_media_id:
return 1 << selected_media_id
elif default_media_id:
return 1 << default_media_id
else:
return 0
def _media_added(self, pdraw, media_info, userdata):
id_ = int(media_info.contents.id)
self.logger.info(f"_media_added id : {id_}")
video_info = media_info.contents.pdraw_media_info_0.video
frame_type = video_info.format
if frame_type == od.VDEF_FRAME_TYPE_CODED:
video_info = video_info.pdraw_video_info_0.coded
vdef_format = video_info.format
data_format = vdef_format.data_format
else:
video_info = video_info.pdraw_video_info_0.raw
vdef_format = video_info.format
data_format = None
media_type = (frame_type, data_format)
# store the information if it is supported and requested media
# otherwise exit
if (frame_type != od.VDEF_FRAME_TYPE_RAW
and frame_type != od.VDEF_FRAME_TYPE_CODED):
self.logger.warning(f"Ignoring unsupported media id {id_} "
f"(type {video_info.format})")
return
requested_media = False
if self.frame_callbacks[media_type] is not None:
requested_media = True
elif any(map(lambda f: f is not None, self.outfiles[frame_type])):
requested_media = True
if not requested_media:
self.logger.info(f"Skipping non-requested media id {id_} "
f"(type {video_info.format})")
return
self.streams[id_]["media_type"] = media_type
self.streams[id_]["frame_type"] = frame_type
self.streams[id_]["vdef_format"] = vdef_format
if frame_type == od.VDEF_FRAME_TYPE_CODED and (
od.VDEF_CODED_DATA_FORMAT_BYTE_STREAM):
outfile = self.outfiles[frame_type]["data"]
if outfile:
header = video_info.pdraw_coded_video_info_0.h264
header = H264Header(
bytearray(header.sps),
int(header.spslen),
bytearray(header.pps),
int(header.ppslen),
)
self.streams[id_]['h264_header'] = header
self.streams[id_]['track_id'] = outfile.add_track(
type=od.MP4_TRACK_TYPE_VIDEO,
name="video",
enabled=1,
in_movie=1,
in_preview=1,
)
self.streams[id_]['metadata_track_id'] = outfile.add_track(
type=od.MP4_TRACK_TYPE_METADATA,
name="metadata",
enabled=0,
in_movie=0,
in_preview=0,
)
outfile.ref_to_track(self.streams[id_]['metadata_track_id'],
self.streams[id_]['track_id'])
# start a video sink attached to the new media
video_sink_params = od.struct_pdraw_video_sink_params.bind(
dict(
# drop buffers when the queue is full (buffer_queue_size > 0)
queue_max_count=self.buffer_queue_size, # buffer queue size
))
self.streams[id_]['id_userdata'] = ctypes.cast(
ctypes.pointer(ctypes.py_object(id_)), ctypes.c_void_p)
self.streams[id_]['id'] = id_
self.streams[id_]['video_sink_cbs'] = self.video_sink_vt[
frame_type].cbs.bind({"flush": self._video_sink_flush})
self.streams[id_]["frame_type"] = frame_type
self.streams[id_]['video_sink'] = od.POINTER_T(
self.video_sink_vt[frame_type].video_sink_type)()
res = self.video_sink_vt[frame_type].new(
pdraw, id_, video_sink_params, self.streams[id_]['video_sink_cbs'],
self.streams[id_]['id_userdata'],
ctypes.byref(self.streams[id_]['video_sink']))
if res != 0 or not self.streams[id_]['video_sink']:
self.logger.error("Unable to start video sink")
return
# Retrieve the queue belonging to the sink
queue = self.video_sink_vt[frame_type].get_queue(
pdraw,
self.streams[id_]['video_sink'],
)
self.streams[id_]['video_queue'] = queue
# Retrieve event object and related file descriptor
res = self.video_sink_vt[frame_type].queue_get_event(
queue, ctypes.byref(self.streams[id_]['video_queue_event']))
if res < 0 or not self.streams[id_]['video_queue_event']:
self.logger.error(f"Unable to get video sink queue event: {-res}")
return
# add the file description to our pomp loop
self.callbacks_thread_loop.add_event_to_loop(
self.streams[id_]['video_queue_event'],
lambda *args: self._video_sink_queue_event(*args), id_)
def _media_removed(self, pdraw, media_info, userdata):
id_ = media_info.contents.id
if id_ not in self.streams:
self.logger.error(f"Received removed event from unknown ID {id_}")
return
self.logger.info(f"_media_removed called id : {id_}")
# FIXME: Workaround media_removed called with destroyed media
if not self.pdraw:
self.logger.error(
f"_media_removed called with a destroyed pdraw id : {id_}")
return
self._media_removed_impl(id_)
def _media_removed_impl(self, id_):
frame_type = self.streams[id_]['frame_type']
with self.streams[id_]['video_sink_lock']:
if self.streams[id_]['video_queue_event']:
self.callbacks_thread_loop.remove_event_from_loop(
self.streams[id_]['video_queue_event']).result_or_cancel(
timeout=5.)
self.streams[id_]['video_queue_event'] = None
if not self.streams[id_]['video_sink']:
self.logger.error(f"pdraw_video_sink for media_id {id_} "
f"has already been stopped")
return
res = self.video_sink_vt[frame_type].destroy(
self.pdraw, self.streams[id_]['video_sink'])
if res < 0:
self.logger.error(f"pdraw_stop_video_sink() returned {res}")
else:
self.logger.debug(
f"_media_removed video sink destroyed id : {id_}")
self.streams[id_]['video_queue'] = None
self.streams[id_]['video_sink'] = None
self.streams[id_]['video_sink_cbs'] = None
def _end_of_range(self, pdraw, demuxer, timestamp, userdata):
self.logger.info("_end_of_range")
self.close()
def _video_sink_flush(self, pdraw, videosink, userdata):
id_ = py_object_cast(userdata)
if id_ not in self.streams:
self.logger.error(f"Received flush event from unknown ID {id_}")
return -errno.ENOENT
return self._video_sink_flush_impl(id_)
def _video_sink_flush_impl(self, id_):
# FIXME: Workaround video_sink_flush called with destroyed media
if not self.pdraw:
self.logger.error(
f"_video_sink_flush called with a destroyed pdraw id : {id_}")
return -errno.EINVAL
# FIXME: Workaround video_sink_flush called with destroyed video queue
if not self.streams[id_]['video_queue']:
self.logger.error(
f"_video_sink_flush called with a destroyed queue id : {id_}")
return -errno.EINVAL
with self.streams[id_]['video_sink_lock']:
self.logger.debug(f"flush_callback {id_}")
flush_callback = self.flush_callbacks[self.streams[id_]
['frame_type']]
if flush_callback is not None:
flushed = self.callbacks_thread_loop.run_async(
flush_callback, self.streams[id_])
try:
if not flushed.result_or_cancel(timeout=5.):
self.logger.error(f"video sink flush id {id_} error")
except FutureTimeoutError:
self.logger.error(f"video sink flush id {id_} timeout")
# NOTE: If the user failed to flush its buffer at this point,
# bad things WILL happen we're acknowledging the buffer flush
# in all cases...
frame_type = self.streams[id_]['frame_type']
res = self.video_sink_vt[frame_type].queue_flush(
self.streams[id_]['video_queue'])
if res < 0:
self.logger.error(
f"mbuf_coded/raw_video_frame_queue_flush() returned {res}")
else:
self.logger.info(
f"mbuf_coded/raw_video_frame_queue_flush() returned {res}")
res = self.video_sink_vt[frame_type].queue_flushed(
self.pdraw, self.streams[id_]['video_sink'])
if res < 0:
self.logger.error(
f"pdraw_coded/raw_video_sink_queue_flushed() "
f"returned {res}")
else:
self.logger.debug(
f"pdraw_coded/raw_video_sink_queue_flushed() "
f"returned {res}")
return 0
@callback_decorator()
def _video_sink_queue_event(self, pomp_evt, userdata):
id_ = py_object_cast(userdata)
self.logger.debug(f"media id = {id_}")
if id_ not in self.streams:
self.logger.error(f"Received queue event from unknown ID {id_}")
return
# acknowledge event
res = od.pomp_evt_clear(self.streams[id_]['video_queue_event'])
if res != 0:
self.logger.error(f"Unable to clear frame received event ({res})")
if not self._is_ready_to_play:
self.logger.debug("The stream is no longer ready: drop one frame")
return
# process all available buffers in the queue
with self.streams[id_]['video_sink_lock']:
while self._process_stream(id_):
pass
def _pop_stream_buffer(self, id_):
frame_type = self.streams[id_]['frame_type']
mbuf_video_frame = od.POINTER_T(
self.video_sink_vt[frame_type].mbuf_video_frame_type)()
res = self.video_sink_vt[frame_type].queue_pop(
self.streams[id_]['video_queue'], ctypes.byref(mbuf_video_frame))
if res < 0:
if res not in (-errno.EAGAIN, -errno.ENOENT):
self.logger.error(
f"mbuf_coded_video_frame_queue_pop returned error {res}")
mbuf_video_frame = od.POINTER_T(
self.video_sink_vt[frame_type].mbuf_video_frame_type)()
elif not mbuf_video_frame:
self.logger.error('mbuf_coded_video_frame_queue_pop returned NULL')
return mbuf_video_frame
def _process_stream(self, id_):
self.logger.debug(f"media id = {id_}")
mbuf_video_frame = self._pop_stream_buffer(id_)
if not mbuf_video_frame:
return False
video_frame = VideoFrame(self.logger, mbuf_video_frame,
id_, self.streams[id_],
self.get_session_metadata())
try:
self._process_stream_buffer(id_, video_frame)
return True
except Exception:
self.logger.exception("_process_stream_buffer exception")
return False
finally:
# Once we're done with this frame, dispose the
# associated frame buffer
video_frame.unref()
def _process_stream_buffer(self, id_, video_frame):
stream = self.streams[id_]
frame_type = stream['frame_type']
media_type = stream["media_type"]
# write and/or send data over the requested channels
# handle output files
files = self.outfiles[frame_type]
f = files['meta']
if f and not f.closed:
vmeta_type, vmeta = video_frame.vmeta()
files['meta'].write(json.dumps({str(vmeta_type): vmeta}) + '\n')
f = files['info']
if f and not f.closed:
info = video_frame.info()
files['info'].write(json.dumps(info) + '\n')
f = files['data']
if f and not f.closed:
if frame_type == od.VDEF_FRAME_TYPE_CODED:
if stream["track_id"] is not None:
track_id = stream["track_id"]
metadata_track_id = stream["metadata_track_id"]
h264_header = stream["h264_header"]
if f.tell(track_id) == 0:
now = time.time()
f.set_decoder_config(track_id, h264_header,
video_frame.width,
video_frame.height)
f.set_metadata_mime_type(
metadata_track_id,
od.VMETA_FRAME_PROTO_CONTENT_ENCODING,
od.VMETA_FRAME_PROTO_MIME_TYPE)
f.add_track_metadata(
track_id, "com.parrot.olympe.first_timestamp",
str(now * PDRAW_TIMESCALE))
f.add_track_metadata(
track_id,
"com.parrot.olympe.resolution",
f"{video_frame.width}x{video_frame.height}",
)
f.add_coded_frame(track_id, metadata_track_id, video_frame)
else:
frame_array = video_frame.as_ndarray()
if frame_array is not None:
f.write(
ctypes.string_at(
frame_array.ctypes.data_as(
ctypes.POINTER(ctypes.c_ubyte)),
frame_array.size,
))
# call callbacks when existing
cb = self.frame_callbacks[media_type]
if cb is not None:
cb(video_frame)
def set_output_files(self, video=None, metadata=None, info=None):
"""
Records the video stream session to the disk
- video: path to the video stream mp4 recording file
- metadata: path to the video stream metadata json output file
- info: path to video stream frames info json output file
This function MUST NOT be called when a video streaming session is
active.
Setting a file parameter to `None` disables the recording for the
related stream part.
"""
if self.state is PdrawState.Playing:
raise RuntimeError(
'Cannot set video streaming files while streaming is on.')
for frame_type, data_type, filepath, attrib in (
(od.VDEF_FRAME_TYPE_CODED, 'data', video,
'wb'), (od.VDEF_FRAME_TYPE_CODED, 'meta', metadata,
'w'), (od.VDEF_FRAME_TYPE_CODED, 'info', info, 'w')):
if self.outfiles[frame_type][data_type]:
self.outfiles[frame_type][data_type].close()
self.outfiles[frame_type][data_type] = None
if filepath is None:
continue
# open and close file to store its filename and attribute
self.outfiles[frame_type][data_type] = open(filepath, attrib)
self.outfiles[frame_type][data_type].close()
def set_callbacks(self,
h264_cb=None,
h264_avcc_cb=None,
h264_bytestream_cb=None,
raw_cb=None,
start_cb=None,
end_cb=None,
flush_h264_cb=None,
flush_raw_cb=None):
"""
Set the callback functions that will be called when a new video stream
frame is available, when the video stream starts/ends or when the video
buffer needs to get flushed.
**Video frame callbacks**
- `h264_cb` is associated to the H264 encoded video (AVCC) stream
- `h264_avcc_cb` is associated to the H264 encoded video (AVCC) stream
- `h264_bytestream_cb` is associated to the H264 encoded video
(ByteStream) stream
- `raw_cb` is associated to the decoded video stream
Each video frame callback function takes an
:py:func:`~olympe.VideoFrame` parameter whose lifetime ends after the
callback execution. If this video frame is passed to another thread,
its internal reference count need to be incremented first by calling
:py:func:`~olympe.VideoFrame.ref`. In this case, once the frame is no
longer needed, its reference count needs to be decremented so that this
video frame can be returned to memory pool.
**Video flush callbacks**
- `flush_h264_cb` is associated to the H264 encoded video stream
- `flush_raw_cb` is associated to the decoded video stream
Video flush callback functions are called when a video stream reclaim
all its associated video buffer. Every frame that has been referenced
**Start/End callbacks**
The `start_cb`/`end_cb` callback functions are called when the video
stream start/ends. They don't accept any parameter.
The return value of all these callback functions are ignored.
If a callback is not desired, leave the parameter to its default value
or set it to `None` explicitly.
"""
if h264_cb and (h264_avcc_cb or h264_bytestream_cb):
raise ValueError(
"Invalid parameters combination: "
"h264_cb and one of h264_avcc_cb or h264_bytestream_cb have "
"been set")
h264_avcc_cb = h264_avcc_cb or h264_cb
for media_type, cb in (((od.VDEF_FRAME_TYPE_CODED,
od.VDEF_CODED_DATA_FORMAT_AVCC),
h264_avcc_cb),
((od.VDEF_FRAME_TYPE_CODED,
od.VDEF_CODED_DATA_FORMAT_BYTE_STREAM),
h264_bytestream_cb), ((od.VDEF_FRAME_TYPE_RAW,
None), raw_cb)):
self.frame_callbacks[media_type] = callback_decorator(
logger=self.logger)(cb)
for frame_type, cb in ((od.VDEF_FRAME_TYPE_CODED, flush_h264_cb),
(od.VDEF_FRAME_TYPE_RAW, flush_raw_cb)):
self.flush_callbacks[frame_type] = callback_decorator(
logger=self.logger)(cb)
self.start_callback = callback_decorator(logger=self.logger)(start_cb)
self.end_callback = callback_decorator(logger=self.logger)(end_cb)
def _open_output_files(self):
self.logger.debug('opening video output files')
for frame_type, data in self.outfiles.items():
for data_type, f in data.items():
if f and f.closed:
if data_type == "data" and (frame_type
== od.VDEF_FRAME_TYPE_CODED):
self.outfiles[frame_type][data_type] = Mp4Mux(f.name)
else:
self.outfiles[frame_type][data_type] = open(
f.name, f.mode)
def _close_output_files(self):
self.logger.debug('closing video output files')
for files in self.outfiles.values():
for f in files.values():
if f:
f.close()
def start(self, *args, **kwds):
"""
See :py:func:`~olympe.video.Pdraw.play`
"""
return self.play(*args, **kwds)
def stop(self, timeout=5):
"""
Stops the video stream
"""
f = self.pause().then(lambda f: self.close() if f else None)
if timeout > 0:
return f.result_or_cancel(timeout=timeout)
else:
return f
def play(self,
url=None,
media_name="DefaultVideo",
resource_name="live",
timeout=5):
"""
Play a video
By default, open and play a live video streaming session available
from rtsp://192.168.42.1/live where "192.168.42.1" is the default IP
address of a physical (Anafi) drone. The default is equivalent to
`Pdraw.play(url="rtsp://192.168.42.1/live")`
For a the live video streaming from a **simulated drone**, you have to
specify the default simulated drone IP address (10.202.0.1) instead:
`Pdraw.play(url="rtsp://10.202.0.1/live")`.
The `url` parameter can also point to a local file example:
`Pdraw.play(url="file://~/Videos/100000010001.MP4")`.
:param url: rtsp or local file video URL
:type url: str
:param media_name: name of the media/track (defaults to "DefaultVideo").
If the provided media name is not available from the requested video
stream, the default media is selected instead.
:type media_name: str
"""
if self.pdraw is None:
self.logger.error("Error Pdraw interface seems to be destroyed")
self._play_resp_future.set_result(False)
return self._play_resp_future
if self.state in (PdrawState.Opening, PdrawState.Closing):
self.logger.error(
f"Cannot play stream from the {self.state} state")
f = Future(self.pdraw_thread_loop)
f.set_result(False)
return f
self.resource_name = resource_name
self.media_name = media_name
if url is None:
self.url = b"rtsp://%s/%s" % (self.server_addr.encode(),
self.resource_name.encode())
else:
if isinstance(url, bytes):
url = url.decode('utf-8')
if url.startswith('file://'):
url = url[7:]
if url.startswith('~/'):
url = os.path.expanduser(url)
url = os.path.expandvars(url)
url = url.encode('utf-8')
self.url = url
# reset session metadata from any previous session
self.session_metadata = {}
self.streams = defaultdict(StreamFactory)
self._open_output_files()
if self._play_resp_future.done():
self._play_resp_future = Future(self.pdraw_thread_loop)
if self.state in (PdrawState.Created, PdrawState.Closed):
open_resp_future = self.pdraw_thread_loop.run_async(
self._open_stream)
f = self.pdraw_thread_loop.complete_futures(
open_resp_future, self._play_resp_future)
else:
f = self._play_resp_future
self.pdraw_thread_loop.run_async(self._play_impl)
if timeout > 0:
return f.result_or_cancel(timeout=timeout)
else:
return f
@callback_decorator()
def _play_impl(self):
self.logger.debug("play_impl")
if self.state is PdrawState.Playing:
if not self._play_resp_future.done():
self._play_resp_future.set_result(True)
return self._play_resp_future
res = od.pdraw_demuxer_play(self.pdraw, self.pdraw_demuxer)
if res != 0:
self.logger.error(f"Unable to start streaming ({res})")
if not self._play_resp_future.done():
self._play_resp_future.set_result(False)
return self._play_resp_future
def pause(self):
"""
Pause the currently playing video
"""
if self.pdraw is None:
self.logger.error("Error Pdraw interface seems to be destroyed")
self._pause_resp_future.set_result(False)
return self._pause_resp_future
if self._pause_resp_future.done():
self._pause_resp_future = Future(self.pdraw_thread_loop)
if self.state is PdrawState.Playing:
self.pdraw_thread_loop.run_async(self._pause_impl)
elif self.state is PdrawState.Closed:
# Pause an closed stream is OK
self._pause_resp_future.set_result(True)
else:
self.logger.error(
f"Cannot pause stream from the {self.state} state")
self._pause_resp_future.set_result(False)
return self._pause_resp_future
@callback_decorator()
def _pause_impl(self):
res = od.pdraw_demuxer_pause(self.pdraw, self.pdraw_demuxer)
if res != 0:
self.logger.error(f"Unable to stop streaming ({res})")
self._pause_resp_future.set_result(False)
return self._pause_resp_future
def get_session_metadata(self):
"""
Returns a dictionary of video stream session metadata
"""
if self.pdraw is None:
self.logger.error("Error Pdraw interface seems to be destroyed")
return None
return self.session_metadata
vmeta_session = od.struct_vmeta_session()
res = od.pdraw_get_peer_session_metadata(self.pdraw,
ctypes.pointer(vmeta_session))
if res != 0:
self.logger.error("Unable to get sessions metata")
return None
self.session_metadata = od.struct_vmeta_session.as_dict(vmeta_session)
return self.session_metadata
def play(self,
url=None,
media_name="DefaultVideo",
resource_name="live",
timeout=5):
"""
Play a video
By default, open and play a live video streaming session available
from rtsp://192.168.42.1/live where "192.168.42.1" is the default IP
address of a physical (Anafi) drone. The default is equivalent to
`Pdraw.play(url="rtsp://192.168.42.1/live")`
For a the live video streaming from a **simulated drone**, you have to
specify the default simulated drone IP address (10.202.0.1) instead:
`Pdraw.play(url="rtsp://10.202.0.1/live")`.
The `url` parameter can also point to a local file example:
`Pdraw.play(url="file://~/Videos/100000010001.MP4")`.
:param url: rtsp or local file video URL
:type url: str
:param media_name: name of the media/track (defaults to "DefaultVideo").
If the provided media name is not available from the requested video
stream, the default media is selected instead.
:type media_name: str
"""
if self.pdraw is None:
self.logger.error("Error Pdraw interface seems to be destroyed")
self._play_resp_future.set_result(False)
return self._play_resp_future
if self.state in (PdrawState.Opening, PdrawState.Closing):
self.logger.error(
f"Cannot play stream from the {self.state} state")
f = Future(self.pdraw_thread_loop)
f.set_result(False)
return f
self.resource_name = resource_name
self.media_name = media_name
if url is None:
self.url = b"rtsp://%s/%s" % (self.server_addr.encode(),
self.resource_name.encode())
else:
if isinstance(url, bytes):
url = url.decode('utf-8')
if url.startswith('file://'):
url = url[7:]
if url.startswith('~/'):
url = os.path.expanduser(url)
url = os.path.expandvars(url)
url = url.encode('utf-8')
self.url = url
# reset session metadata from any previous session
self.session_metadata = {}
self.streams = defaultdict(StreamFactory)
self._open_output_files()
if self._play_resp_future.done():
self._play_resp_future = Future(self.pdraw_thread_loop)
if self.state in (PdrawState.Created, PdrawState.Closed):
open_resp_future = self.pdraw_thread_loop.run_async(
self._open_stream)
f = self.pdraw_thread_loop.complete_futures(
open_resp_future, self._play_resp_future)
else:
f = self._play_resp_future
self.pdraw_thread_loop.run_async(self._play_impl)
if timeout > 0:
return f.result_or_cancel(timeout=timeout)
else:
return f