def __init__(self, parent=None): super(FlieControl, self).__init__(parent) # Temporary #cache_dir = os.path.dirname(os.path.realpath(__file__)) #cache_dir = cache_dir[0:cache_dir.find("src/crazyflieROS")]+"cache/" # Parameters # Members self.console_cache = "" # Used to buffer crazyflie console messages that go over newlines self.crazyflie = Crazyflie() #self.crazyflie = Crazyflie(cache_dir+"/ro", cache_dir+"/rw") self.linkQuality = LinkQuality(window=50) self.status = STATE.DISCONNECTED self.killswitch = False self.hovering = False self.hoverAllowed = True # Timers self.inKBPS = KBSecMonitor() self.outKBPS = KBSecMonitor() # Callbacks self.crazyflie.connected.add_callback( self.connectedCB ) # Called when the link is established and the TOCs (that are not cached) have been downloaded self.crazyflie.disconnected.add_callback( self.disconnectedCB) # Called on disconnect, no matter the reason self.crazyflie.connection_lost.add_callback( self.connectionLostCB) # Called on unintentional disconnect only self.crazyflie.connection_failed.add_callback( self.connectionFailedCB ) # Called if establishing of the link fails (i.e times out) self.crazyflie.connection_requested.add_callback( self.connectionRequestedCB ) # Called when the user requests a connection self.crazyflie.link_established.add_callback( self.linkEstablishedCB ) # Called when the first packet in a new link is received self.crazyflie.link_quality_updated.add_callback( self.linkQualityCB ) # Called when the link driver updates the link quality measurement self.crazyflie.packet_received.add_callback( self.packetReceivedCB) # Called for every packet received self.crazyflie.packet_sent.add_callback( self.packetSentCB) # Called for every packet sent self.crazyflie.console.receivedChar.add_callback( self.consoleCB) # Called with console text
def __init__(self, parent=None): super(FlieControl, self).__init__(parent) # Temporary # cache_dir = os.path.dirname(os.path.realpath(__file__)) # cache_dir = cache_dir[0:cache_dir.find("src/crazyflieROS")]+"cache/" # Parameters # Members self.console_cache = "" # Used to buffer crazyflie console messages that go over newlines self.crazyflie = Crazyflie() # self.crazyflie = Crazyflie(cache_dir+"/ro", cache_dir+"/rw") self.linkQuality = LinkQuality(window=50) self.status = STATE.DISCONNECTED self.killswitch = False self.hovering = False self.hoverAllowed = True # Timers self.inKBPS = KBSecMonitor() self.outKBPS = KBSecMonitor() # Callbacks self.crazyflie.connected.add_callback( self.connectedCB ) # Called when the link is established and the TOCs (that are not cached) have been downloaded self.crazyflie.disconnected.add_callback(self.disconnectedCB) # Called on disconnect, no matter the reason self.crazyflie.connection_lost.add_callback(self.connectionLostCB) # Called on unintentional disconnect only self.crazyflie.connection_failed.add_callback( self.connectionFailedCB ) # Called if establishing of the link fails (i.e times out) self.crazyflie.connection_requested.add_callback( self.connectionRequestedCB ) # Called when the user requests a connection self.crazyflie.link_established.add_callback( self.linkEstablishedCB ) # Called when the first packet in a new link is received self.crazyflie.link_quality_updated.add_callback( self.linkQualityCB ) # Called when the link driver updates the link quality measurement self.crazyflie.packet_received.add_callback(self.packetReceivedCB) # Called for every packet received self.crazyflie.packet_sent.add_callback(self.packetSentCB) # Called for every packet sent self.crazyflie.console.receivedChar.add_callback(self.consoleCB) # Called with console text
class FlieControl(QObject): """ Class that andles the flie library """ sig_console = pyqtSignal( str ) # Console messages from the flie - emiited for every console message sig_packetSpeed = pyqtSignal( int, int ) # Packets in/out per second - emitted every self.updatePacketSpeed ms sig_stateUpdate = pyqtSignal( int, str, str) # Send state update and optional messages (stateNr, uri, errmsg) sig_flieLink = pyqtSignal(int) #sig_flieBattery = pyqtSignal() def __init__(self): super(FlieControl, self).__init__() # Temporary #cache_dir = os.path.dirname(os.path.realpath(__file__)) #cache_dir = cache_dir[0:cache_dir.find("src/crazyflieROS")]+"cache/" # Parameters # Members self.console_cache = "" # Used to buffer crazyflie console messages that go over newlines self.crazyflie = Crazyflie() #self.crazyflie = Crazyflie(cache_dir+"/ro", cache_dir+"/rw") self.linkQuality = LinkQuality(window=50) self.status = STATE.DISCONNECTED self.killswitch = False self.hovering = False # Timers self.inKBPS = KBSecMonitor() self.outKBPS = KBSecMonitor() # Callbacks self.crazyflie.connected.add_callback( self.connectedCB ) # Called when the link is established and the TOCs (that are not cached) have been downloaded self.crazyflie.disconnected.add_callback( self.disconnectedCB) # Called on disconnect, no matter the reason self.crazyflie.connection_lost.add_callback( self.connectionLostCB) # Called on unintentional disconnect only self.crazyflie.connection_failed.add_callback( self.connectionFailedCB ) # Called if establishing of the link fails (i.e times out) self.crazyflie.connection_requested.add_callback( self.connectionRequestedCB ) # Called when the user requests a connection self.crazyflie.link_established.add_callback( self.linkEstablishedCB ) # Called when the first packet in a new link is received self.crazyflie.link_quality_updated.add_callback( self.linkQualityCB ) # Called when the link driver updates the link quality measurement self.crazyflie.packet_received.add_callback( self.packetReceivedCB) # Called for every packet received self.crazyflie.packet_sent.add_callback( self.packetSentCB) # Called for every packet sent self.crazyflie.console.receivedChar.add_callback( self.consoleCB) # Called with console text ### CRAZYFLIE CALLBACKS def connectedCB(self, uri, msg=""): """ Called when the link is established and the TOCs (that are not cached) have been downloaded """ self.sig_stateUpdate.emit(STATE.CONNECTED, uri, msg) def disconnectedCB(self, uri, msg=""): """ Called on disconnect, no matter the reason """ self.console_cache = "" self.inKBPS.stop() self.outKBPS.stop() self.crazyflie.packet_received.remove_callback(self.packetReceivedCB) self.crazyflie.packet_sent.remove_callback(self.packetSentCB) self.sig_stateUpdate.emit(STATE.DISCONNECTED, uri, msg) def connectionLostCB(self, uri, msg=""): """ Called on unintentional disconnect only """ self.sig_stateUpdate.emit(STATE.CONNECTION_LOST, uri, msg) def connectionFailedCB(self, uri, msg=""): """ Called if establishing of the link fails (i.e times out) """ # stop counting packets self.inKBPS.stop() self.outKBPS.stop() self.crazyflie.packet_received.remove_callback(self.packetReceivedCB) self.crazyflie.packet_sent.remove_callback(self.packetSentCB) self.sig_stateUpdate.emit(STATE.CONNECTION_FAILED, uri, msg) def connectionRequestedCB(self, uri, msg=""): """ Called when the user requests a connection """ # Start counting packets self.inKBPS.start() self.outKBPS.start() self.crazyflie.packet_received.add_callback(self.packetReceivedCB) self.crazyflie.packet_sent.add_callback(self.packetSentCB) self.sig_stateUpdate.emit(STATE.CONNECTION_REQUESTED, uri, msg) def linkEstablishedCB(self, uri, msg=""): """ Called when the first packet in a new link is received """ self.sig_stateUpdate.emit(STATE.LINK_ESTABLISHED, uri, msg) def linkQualityCB(self, percentage): """ Called when the link driver updates the link quality measurement """ #q = self.linkQuality.addMeasurementMin(percentage) #q = self.linkQuality.addMeasurementAvg(percentage) q = self.linkQuality.addMeasurementCount(percentage) if q is not None: self.sig_flieLink.emit(percentage) def consoleCB(self, msg): """ Crazyflie console messages are routed to this function. Newlines are detected and the strings joined. """ # Join messages if they are max length if len(msg) == 30: self.console_cache += msg else: #msg = cyan+(self.console_cache+msg).strip("\n")+reset msg = (self.console_cache + msg).strip("\n") self.sig_console.emit(msg + "\n") CSI = "\x1b[" cyan = CSI + "36m" reset = CSI + "m" msg = cyan + (self.console_cache + msg).strip("\n") + reset rospy.loginfo(msg) self.console_cache = "" def packetReceivedCB(self, pk=None): """ Called for every packet received """ self.inKBPS.count(1 + len(pk.datal)) def packetSentCB(self, pk=None): """ Called for every packet sent """ self.outKBPS.count( 1 + len(pk.datal)) #TODO: write function that gets the size directly ### LOG CALLBACKS ### OUTGOING @pyqtSlot(float, float, float, int, bool) def sendCmd(self, roll, pitch, yawrate, thrust, hover): """ Send the flie a control command """ if not self.killswitch: if self.hovering != hover: self.requestHover(hover) self.hovering = hover self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) else: if self.hovering: self.requestHover(self, on=False) self.hovering = False def requestHover(self, on=True): self.crazyflie.param.set_value( "%s.%s" % (self.param.group, self.param.name), on) def getLogToC(self): return self.crazyflie.log._toc.toc if self.crazyflie.log._toc else None ### USER INITIATED @pyqtSlot(str) def requestConnect(self, uri): """ Request connection to the flie """ rospy.loginfo("Requesting connection to [%s]", uri) self.crazyflie.open_link(uri) @pyqtSlot() def requestDisconnect(self): """ Request shutdown to flie """ rospy.loginfo("Requesting disconnect") self.crazyflie.close_link() @pyqtSlot(bool) def setKillswitch(self, on): rospy.loginfo("GUI KillSwitch " + "on" if on else "off") self.killswitch = on
class FlieControl(QObject): """ Class that andles the flie library """ sig_console = pyqtSignal(str) # Console messages from the flie - emiited for every console message sig_packetSpeed = pyqtSignal(int, int) # Packets in/out per second - emitted every self.updatePacketSpeed ms sig_stateUpdate = pyqtSignal(int, str, str) # Send state update and optional messages (stateNr, uri, errmsg) sig_flieLink = pyqtSignal(int) # sig_flieBattery = pyqtSignal() def __init__(self, parent=None): super(FlieControl, self).__init__(parent) # Temporary # cache_dir = os.path.dirname(os.path.realpath(__file__)) # cache_dir = cache_dir[0:cache_dir.find("src/crazyflieROS")]+"cache/" # Parameters # Members self.console_cache = "" # Used to buffer crazyflie console messages that go over newlines self.crazyflie = Crazyflie() # self.crazyflie = Crazyflie(cache_dir+"/ro", cache_dir+"/rw") self.linkQuality = LinkQuality(window=50) self.status = STATE.DISCONNECTED self.killswitch = False self.hovering = False self.hoverAllowed = True # Timers self.inKBPS = KBSecMonitor() self.outKBPS = KBSecMonitor() # Callbacks self.crazyflie.connected.add_callback( self.connectedCB ) # Called when the link is established and the TOCs (that are not cached) have been downloaded self.crazyflie.disconnected.add_callback(self.disconnectedCB) # Called on disconnect, no matter the reason self.crazyflie.connection_lost.add_callback(self.connectionLostCB) # Called on unintentional disconnect only self.crazyflie.connection_failed.add_callback( self.connectionFailedCB ) # Called if establishing of the link fails (i.e times out) self.crazyflie.connection_requested.add_callback( self.connectionRequestedCB ) # Called when the user requests a connection self.crazyflie.link_established.add_callback( self.linkEstablishedCB ) # Called when the first packet in a new link is received self.crazyflie.link_quality_updated.add_callback( self.linkQualityCB ) # Called when the link driver updates the link quality measurement self.crazyflie.packet_received.add_callback(self.packetReceivedCB) # Called for every packet received self.crazyflie.packet_sent.add_callback(self.packetSentCB) # Called for every packet sent self.crazyflie.console.receivedChar.add_callback(self.consoleCB) # Called with console text ### CRAZYFLIE CALLBACKS def connectedCB(self, uri, msg=""): """ Called when the link is established and the TOCs (that are not cached) have been downloaded """ self.sig_stateUpdate.emit(STATE.CONNECTED, uri, msg) def disconnectedCB(self, uri, msg=""): """ Called on disconnect, no matter the reason """ self.console_cache = "" self.inKBPS.stop() self.outKBPS.stop() self.crazyflie.packet_received.remove_callback(self.packetReceivedCB) self.crazyflie.packet_sent.remove_callback(self.packetSentCB) self.sig_stateUpdate.emit(STATE.DISCONNECTED, uri, msg) def connectionLostCB(self, uri, msg=""): """ Called on unintentional disconnect only """ self.sig_stateUpdate.emit(STATE.CONNECTION_LOST, uri, msg) def connectionFailedCB(self, uri, msg=""): """ Called if establishing of the link fails (i.e times out) """ # stop counting packets self.inKBPS.stop() self.outKBPS.stop() self.crazyflie.packet_received.remove_callback(self.packetReceivedCB) self.crazyflie.packet_sent.remove_callback(self.packetSentCB) self.sig_stateUpdate.emit(STATE.CONNECTION_FAILED, uri, msg) def connectionRequestedCB(self, uri, msg=""): """ Called when the user requests a connection """ # Start counting packets self.inKBPS.start() self.outKBPS.start() self.crazyflie.packet_received.add_callback(self.packetReceivedCB) self.crazyflie.packet_sent.add_callback(self.packetSentCB) self.sig_stateUpdate.emit(STATE.CONNECTION_REQUESTED, uri, msg) def linkEstablishedCB(self, uri, msg=""): """ Called when the first packet in a new link is received """ self.sig_stateUpdate.emit(STATE.LINK_ESTABLISHED, uri, msg) def linkQualityCB(self, percentage): """ Called when the link driver updates the link quality measurement """ # q = self.linkQuality.addMeasurementMin(percentage) # q = self.linkQuality.addMeasurementAvg(percentage) q = self.linkQuality.addMeasurementCount(percentage) if q is not None: self.sig_flieLink.emit(percentage) def consoleCB(self, msg): """ Crazyflie console messages are routed to this function. Newlines are detected and the strings joined. """ # Join messages if they are max length if len(msg) == 30: self.console_cache += msg else: # msg = cyan+(self.console_cache+msg).strip("\n")+reset msg = (self.console_cache + msg).strip("\n") self.sig_console.emit(msg + "\n") CSI = "\x1b[" cyan = CSI + "36m" reset = CSI + "m" msg = cyan + (self.console_cache + msg).strip("\n") + reset rospy.loginfo(msg) self.console_cache = "" def packetReceivedCB(self, pk=None): """ Called for every packet received """ self.inKBPS.count(1 + len(pk.datal)) def packetSentCB(self, pk=None): """ Called for every packet sent """ self.outKBPS.count(1 + len(pk.datal)) # TODO: write function that gets the size directly def setPacketUpdateSpeed(self, hz): self.inKBPS.setHZ(hz) self.outKBPS.setHZ(hz) ### LOG CALLBACKS ### OUTGOING @pyqtSlot(bool) def setHoverDisabled(self, on=True): self.hoverAllowed = not on @pyqtSlot(float, float, float, int, bool) def sendCmd(self, roll, pitch, yawrate, thrust, hover): """ Send the flie a control command """ if not self.killswitch: if self.hovering != hover: self.requestHover(hover) self.hovering = hover self.crazyflie.commander.send_setpoint(roll, pitch, yawrate, thrust) else: if self.hovering: self.requestHover(self, on=False) self.hovering = False def requestHover(self, on=True): self.crazyflie.param.set_value("flightmode.althold", "1" if on and self.hoverAllowed else "0") def getLogToC(self): return self.crazyflie.log._toc.toc if self.crazyflie.log._toc else None ### USER INITIATED @pyqtSlot(str) def requestConnect(self, uri): """ Request connection to the flie """ rospy.loginfo("Requesting connection to [%s]", uri) self.crazyflie.open_link(uri) @pyqtSlot() def requestDisconnect(self): """ Request shutdown to flie """ rospy.loginfo("Requesting disconnect") self.crazyflie.close_link() @pyqtSlot(bool) def setKillswitch(self, on): rospy.loginfo("GUI KillSwitch " + "on" if on else "off") self.killswitch = on