Exemple #1
0
    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")
Exemple #2
0
 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"]))
Exemple #3
0
 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"]))
Exemple #4
0
    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")
Exemple #5
0
    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 ??