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
