def receiveCrazyflieLog(self, log, tsCF, tsROS):
""" Handle sending messages to ROS """
## SPECIALLY HANDLED MESSAGES
if isGroup(log, "gyro"):
self.pub(getGroup(log), self.genMsg(log, tsROS, f=radians))
elif isGroup(
log,
"motor",
):
self.pub(getGroup(log),
self.genMsg(log, tsROS, f=thrustToPercentage))
## AUTOMATICALLY GENERATED HERE
else:
self.pub(getGroup(log), self.genMsg(log, tsROS))
##ADITIONALLY HANDLED ROS STUFF
# spam TF #TODO test this!!
if isGroup(log, "stabilizer"):
if hasAllKeys(log, ["roll", "pitch", "yaw"], "stabilizer"):
self.pub_tf.sendTransform(
(0, 0, 0),
tf.transformations.quaternion_from_euler(
radians(log["stabilizer.roll"]),
-radians(log["stabilizer.pitch"]),
-radians(log["stabilizer.yaw"]), 'sxyz'), tsROS,
"/cf%d" % self.options.radio, "/cf_xyz")
def genMsg(self, data, ts,f=lambda x: x):
""" generates a message using sexy python magic. Supply an optional function to apply to each member"""
group = getGroup(data)
msgt = eval("msgCF."+group+"CF")
msg = msgt()
msg.header.stamp = ts
for name in getNames(data):
setattr(msg, name, f(data[group+"."+name])) # sexy python magic
return msg
def genMsg(self, data, ts,f=lambda x: x):
""" generates a message using sexy python magic. Supply an optional function to apply to each member"""
group = getGroup(data)
msgt = eval("msgCF."+group)
msg = msgt()
msg.header.stamp = ts
for name in getNames(data):
setattr(msg, name, f(data[group+"."+name])) # sexy python magic
return msg
def receiveCrazyflieLog(self, log, tsCF, tsROS):
""" Handle sending messages to ROS """
## SPECIALLY HANDLED MESSAGES
if isGroup(log, "gyro"):
self.pub(getGroup(log), self.genMsg(log, tsROS, f=radians))
elif isGroup(log, "motor", ):
self.pub(getGroup(log), self.genMsg(log, tsROS, f=thrustToPercentage))
## AUTOMATICALLY GENERATED HERE
else:
self.pub(getGroup(log), self.genMsg(log, tsROS))
##ADITIONALLY HANDLED ROS STUFF
# spam TF #TODO test this!!
if isGroup(log, "stabilizer"):
if hasAllKeys(log,["roll","pitch","yaw"], "stabilizer"):
self.pub_tf.sendTransform((0, 0, 0),tf.transformations.quaternion_from_euler(
radians(log["stabilizer.roll"]),
-radians(log["stabilizer.pitch"]),
-radians(log["stabilizer.yaw"]),'sxyz'), tsROS, "/cf%d" % self.options.radio, "/cf_xyz")
