bearing_from_mag.py 文件源码

def magnetic_field_callback(self, magnetometer_msg):

        # Correct magnetic filed
        raw_mag = np.array([magnetometer_msg.vector.x,
                            magnetometer_msg.vector.y,
                            magnetometer_msg.vector.z])

        # corrected_mag = compensation * (raw_mag - offset)
        corrected_mag = np.dot(self._calibration_compensation,
                               raw_mag - self._calibration_offset)

        # compute yaw angle using corrected magnetometer measurements
        # and ASSUMING ZERO pitch and roll of the magnetic sensor!
        # adapted from
        # https://github.com/KristofRobot/razor_imu_9dof/blob/indigo-devel/src/Razor_AHRS/Compass.ino
        corrected_mag = corrected_mag / np.linalg.norm(corrected_mag)
        mag_bearing = math.atan2(corrected_mag[1], -corrected_mag[0])

        # add declination and constant bearing offset
        mag_bearing = mag_bearing + self._declination + self._bearing_offset

        # publish unfiltered bearing, degrees only for debug purposes
        self._pub_bearing_raw.publish(Float64(math.degrees(mag_bearing)))

        # compute mean
        self._latest_bearings[self._num_magnetometer_reads] = mag_bearing
        self._num_magnetometer_reads += 1

        if self._num_magnetometer_reads >= self._number_samples_average:
            self._num_magnetometer_reads = 0 # delete oldest samples
            self._received_enough_samples = True

        if self._received_enough_samples:
            bearing_avg = self.angular_mean(self._latest_bearings)
        else:
            # not enough samples, use latest value
            bearing_avg = mag_bearing

        # WARNING: we assume zero roll and zero pitch!
        q_avg = tf.quaternion_from_euler(0.0, 0.0, bearing_avg);
        imu_msg = Imu()
        imu_msg.orientation.w = q_avg[3]
        imu_msg.orientation.x = q_avg[0]
        imu_msg.orientation.y = q_avg[1]
        imu_msg.orientation.z = q_avg[2]

        self._pub_bearing_avg.publish(Float64(math.degrees(bearing_avg)))
        self._pub_imu_bearing_avg.publish(imu_msg)

        # debug
        if self._verbose:
            rospy.loginfo("bearing_avg : " +
                          str(math.degrees(bearing_avg)) + " deg")

            mag_corrected_msg = magnetometer_msg
            mag_corrected_msg.vector.x = corrected_mag[0]
            mag_corrected_msg.vector.y = corrected_mag[1]
            mag_corrected_msg.vector.z = corrected_mag[2]
            self._pub_mag_corrected.publish(mag_corrected_msg)