bearing_from_mag.py 文件源码

def __init__(self):
        # Read Settings
        self.read_settings()

        # Init other variables
        self._num_magnetometer_reads = 0
        self._latest_bearings = np.zeros(shape = (self._number_samples_average, 1))
        self._received_enough_samples = False

        # Subscribe to magnetometer topic
        rospy.Subscriber("magnetic_field", Vector3Stamped, self.magnetic_field_callback)

        # Publishers
        self._pub_bearing_raw = rospy.Publisher(rospy.get_name() + '/bearing_raw_deg',
                                                Float64, queue_size = 10)
        self._pub_bearing_avg = rospy.Publisher(rospy.get_name() + '/bearing_avg_deg',
                                                Float64, queue_size = 10)
        self._pub_imu_bearing_avg = rospy.Publisher(rospy.get_name() + '/imu_bearing_avg',
                                                Imu, queue_size = 10)

        if self._verbose:
            self._pub_mag_corrected = rospy.Publisher(rospy.get_name() + '/mag_corrected',
                                                      Vector3Stamped, queue_size = 10)

        rospy.spin()