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())