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