def connect_drone(self, drone_serial, drone_security_key=""):
self.update_drones()
if self.active_drone == drone_serial:
print("SkyController is already connected to {}".format(
drone_serial))
return True
print("SkyController is not currently connected to {}".format(
drone_serial))
if drone_serial in self.visible_drones:
print("Connecting to {}...".format(drone_serial))
connection = self.skyctrl(
connect(serial=drone_serial, key=drone_security_key) >>
connection_state(state="connected", serial=drone_serial)).wait(
_timeout=10)
elif drone_serial in self.known_drones:
print("{} is a known drone but is not currently visible".format(
drone_serial))
return
elif drone_serial is not None:
print("{} is an unknown drone and not currently visible".format(
drone_serial))
return
if connection.success():
print("Connected to {}".format(drone_serial))
return True
else:
print("Failed to connect to {}".format(drone_serial))
def forget_drone(self, drone_serial):
if drone_serial == self.active_drone:
print("Forgetting {} ...".format(drone_serial))
self.skyctrl(
forget(serial=drone_serial) >> connection_state(
state="disconnecting", serial=drone_serial)).wait(
_timeout=10)
elif drone_serial in self.known_drones:
print("Forgetting {} ...".format(drone_serial))
self.skyctrl(forget(serial=drone_serial)).wait(_timeout=10)
else:
print("{} is an unknown drone".format(drone_serial))
def connect(self):
self.every_event_listener.subscribe()
rate = rospy.Rate(1) # 1hz
while True:
self.pub_state.publish("CONNECTING")
connection = self.drone.connect()
if getattr(connection, 'OK') == True:
break
if rospy.is_shutdown():
exit()
rate.sleep()
# Connect to the SkyController
if rospy.get_param("/skycontroller"):
self.pub_state.publish("CONNECTED_SKYCONTROLLER")
rospy.loginfo("Connection to SkyController: " +
getattr(connection, 'message'))
self.switch_manual()
# Connect to the drone
while True:
if self.drone(
connection_state(state="connected", _policy="check")):
break
if rospy.is_shutdown():
exit()
else:
self.pub_state.publish("SERCHING_DRONE")
rospy.loginfo_once(
"Connection to Anafi: " +
str(self.drone.get_state(connection_state)["state"]))
rate.sleep()
self.pub_state.publish("CONNECTED_DRONE")
rospy.loginfo("Connection to Anafi: " +
str(self.drone.get_state(connection_state)["state"]))
# Connect to the Anafi
else:
self.pub_state.publish("CONNECTED_DRONE")
rospy.loginfo("Connection to Anafi: " +
getattr(connection, 'message'))
self.switch_offboard()
self.frame_queue = queue.Queue()
self.flush_queue_lock = threading.Lock()
# Setup the callback functions to do some live video processing
self.drone.set_streaming_callbacks(raw_cb=self.yuv_frame_cb,
flush_raw_cb=self.flush_cb)
self.drone.start_video_streaming()
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 __init__(self,
*,
name=None,
drone_type=0,
proto_v_min=1,
proto_v_max=3):
super().__init__(name, None, "drone")
self._create_backend(name, proto_v_min, proto_v_max)
self._thread_loop = self._backend._thread_loop
self._scheduler = Scheduler(self._thread_loop, name=self._name)
self._scheduler.add_context("olympe.controller", self)
# Extract arsdk-xml infos
self.enums = enums.ArsdkEnums.get("olympe")
self.messages = OrderedDict()
# Instantiate arsdk messages
for id_, message_type in messages.ArsdkMessages.get(
"olympe").by_id.items():
message = message_type.new()
self.messages[message.id] = message
# Instantiate protobufs messages
self.protobuf_messages = OrderedDict()
for (service_id,
message_id), message_type in messages.ArsdkMessages.get(
"olympe").service_messages.items():
message = message_type.new()
self.protobuf_messages[(service_id, message_id)] = message
self._external_messages = OrderedDict()
self._decoding_errors = []
self._cmd_itf = None
self._connected = False
self._reset_instance()
self._connect_future = None
self._disconnect_future = None
self._declare_callbacks()
self._thread_loop.register_cleanup(self.destroy)
self.subscribe(self._on_connection_state_changed,
drone_manager.connection_state())