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 incomingData(self, data, ts):
""" All incoming data is routed to this function. Ros data is spammed from the tree groups """
#self.sigHandler.handleData(data, ts)
""" BATTERY """
#TODO: cap at 5hz
if isGroup(data, "pm"):
if hasAllKeys(data, ["vbat"], "pm"):
self.sig_batteryUpdated.emit(int(1000 * data["pm.vbat"]))
def incomingData(self, data, ts):
""" All incoming data is routed to this function. Ros data is spammed from the tree groups """
#self.sigHandler.handleData(data, ts)
""" BATTERY """
#TODO: cap at 5hz
if isGroup(data, "pm"):
if hasAllKeys(data, ["vbat"], "pm"):
self.sig_batteryUpdated.emit(int(1000*data["pm.vbat"]))
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 incomingData(self, data, ts):
""" All incoming data is routed to this function. Ros data is spammed from the tree groups """
rostime = rospy.Time.now()
# Battery
if isGroup(data, "pm"):
if hasAllKeys(data, ["vbat"], "pm"):
self.sig_batteryUpdated.emit(int(1000*data["pm.vbat"]))
if hasAllKeys(data, ["state"], "pm"):
self.sig_batteryState.emit(data["pm.state"])
# CPU Usage
elif isGroup(data, "utilization"):
if hasAllKeys(data, ["cpuUsage"], "utilization"):
self.sig_cpuUpdated.emit(data["utilization.cpuUsage"])
# RPY
elif isGroup(data, "stabilizer"):
if data.has_key("stabilizer.yaw"):
# Yaw offset, also negate so its CW #TODO: gyro velocities negated???
self.preYaw = -data["stabilizer.yaw"]
data["stabilizer.yaw"] = normaliseAngle(self.yawOffset + self.preYaw)
if hasAllKeys(data, ["roll","pitch","yaw"], "stabilizer"):
self.sig_rpy.emit(data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"])
if hasAllKeys(data, ["thrust"], "stabilizer"):
self.sig_thrust.emit(data["stabilizer.thrust"])
# Accelerometer
elif isGroup(data, "acc"):
if hasAllKeys(data, ["x","y","z"], "acc"):
self.sig_acc.emit(data["acc.x"],data["acc.y"],data["acc.z"])
self.sig_g.emit(math.sqrt(data["acc.x"]*data["acc.x"]+data["acc.y"]*data["acc.y"]+data["acc.z"]*data["acc.z"]))
elif hasAllKeys(data, ["mag2"], "acc"):
self.sig_g.emit(math.sqrt(data["mag2.x"]))
if hasAllKeys(data, ["zw"], "acc"):
self.sig_accZ.emit(data["acc.zw"])
# Hover Target
# Motor
elif isGroup(data, "motor"):
if hasAllKeys(data, ["m1","m2","m3","m4"], "motor"):
self.sig_motors.emit(thrustToPercentage(data["motor.m1"]), thrustToPercentage(data["motor.m2"]), thrustToPercentage(data["motor.m3"]), thrustToPercentage(data["motor.m4"]))
# Barometer
elif isGroup(data, "baro"):
if 'baro.asl' in data:
self.preAsl = self.preAsl*0.9 + data['baro.asl']*0.1 #little bit of smoothing
for k in data.keys():
if k.startswith('baro.asl'):
data[k] = data[k] - self.groundLevel
# show aslLong to gui
if hasAllKeys(data, ["aslLong"], "baro"):
self.sig_aslLong.emit(data["baro.aslLong"])
# aslLong = uncompensated asl for ROS
#TODO: cheap hack, use aslLong to store raw
data["baro.aslLong"] = data["baro.asl"]
# asl is compensated, for ros and gui
if hasAllKeys(data, ["asl"], "baro"):
data["baro.asl"] = data["baro.asl"] - self.aslOffset
self.sig_baroASL.emit(data["baro.asl"])
if hasAllKeys(data, ["temp"], "baro"):
self.sig_temp.emit(data["baro.temp"])
if hasAllKeys(data, ["pressure"], "baro"):
self.sig_pressure.emit(data["baro.pressure"])
# Hover Target
elif isGroup(data, "altHold"):
if hasAllKeys(data, ["target"], "altHold"):
# baro offset
data["altHold.target"] = data["altHold.target"] - self.groundLevel - self.aslOffset
self.sig_hoverTarget.emit(data["altHold.target"])
## Sys can fly TODO: doesnt seem to work
#elif isGroup(data, "sys"):
# if hasAllKeys(data, ["canfly"], "sys"):
# self.sig_sysCanFly.emit(data["sys.canfly"]>0)
# ROS
if self.pubRos:
self.sig_rosData.emit(data, ts, rostime) # Wall time ??
def incomingData(self, data, ts):
""" All incoming data is routed to this function. Ros data is spammed from the tree groups """
rostime = rospy.Time.now()
# Battery
if isGroup(data, "pm"):
if hasAllKeys(data, ["vbat"], "pm"):
self.sig_batteryUpdated.emit(int(1000 * data["pm.vbat"]))
if hasAllKeys(data, ["state"], "pm"):
self.sig_batteryState.emit(data["pm.state"])
# CPU Usage
elif isGroup(data, "utilization"):
if hasAllKeys(data, ["cpuUsage"], "utilization"):
self.sig_cpuUpdated.emit(data["utilization.cpuUsage"])
# RPY
elif isGroup(data, "stabilizer"):
if data.has_key("stabilizer.yaw"):
# Yaw offset, also negate so its CW #TODO: gyro velocities negated???
self.preYaw = -data["stabilizer.yaw"]
data["stabilizer.yaw"] = normaliseAngle(self.yawOffset +
self.preYaw)
if hasAllKeys(data, ["roll", "pitch", "yaw"], "stabilizer"):
self.sig_rpy.emit(data["stabilizer.roll"],
data["stabilizer.pitch"],
data["stabilizer.yaw"])
if hasAllKeys(data, ["thrust"], "stabilizer"):
self.sig_thrust.emit(data["stabilizer.thrust"])
# Accelerometer
elif isGroup(data, "acc"):
if hasAllKeys(data, ["x", "y", "z"], "acc"):
self.sig_acc.emit(data["acc.x"], data["acc.y"], data["acc.z"])
self.sig_g.emit(
math.sqrt(data["acc.x"] * data["acc.x"] +
data["acc.y"] * data["acc.y"] +
data["acc.z"] * data["acc.z"]))
elif hasAllKeys(data, ["mag2"], "acc"):
self.sig_g.emit(math.sqrt(data["mag2.x"]))
if hasAllKeys(data, ["zw"], "acc"):
self.sig_accZ.emit(data["acc.zw"])
# Hover Target
# Motor
elif isGroup(data, "motor"):
if hasAllKeys(data, ["m1", "m2", "m3", "m4"], "motor"):
self.sig_motors.emit(thrustToPercentage(data["motor.m1"]),
thrustToPercentage(data["motor.m2"]),
thrustToPercentage(data["motor.m3"]),
thrustToPercentage(data["motor.m4"]))
# Barometer
elif isGroup(data, "baro"):
if 'baro.asl' in data:
self.preAsl = self.preAsl * 0.9 + data[
'baro.asl'] * 0.1 #little bit of smoothing
for k in data.keys():
if k.startswith('baro.asl'):
data[k] = data[k] - self.groundLevel
# show aslLong to gui
if hasAllKeys(data, ["aslLong"], "baro"):
self.sig_aslLong.emit(data["baro.aslLong"])
# aslLong = uncompensated asl for ROS
#TODO: cheap hack, use aslLong to store raw
data["baro.aslLong"] = data["baro.asl"]
# asl is compensated, for ros and gui
if hasAllKeys(data, ["asl"], "baro"):
data["baro.asl"] = data["baro.asl"] - self.aslOffset
self.sig_baroASL.emit(data["baro.asl"])
if hasAllKeys(data, ["temp"], "baro"):
self.sig_temp.emit(data["baro.temp"])
if hasAllKeys(data, ["pressure"], "baro"):
self.sig_pressure.emit(data["baro.pressure"])
# Hover Target
elif isGroup(data, "altHold"):
if hasAllKeys(data, ["target"], "altHold"):
# baro offset
data["altHold.target"] = data[
"altHold.target"] - self.groundLevel - self.aslOffset
self.sig_hoverTarget.emit(data["altHold.target"])
## Sys can fly TODO: doesnt seem to work
#elif isGroup(data, "sys"):
# if hasAllKeys(data, ["canfly"], "sys"):
# self.sig_sysCanFly.emit(data["sys.canfly"]>0)
# ROS
if self.pubRos:
self.sig_rosData.emit(data, ts, rostime) # Wall time ??
