bearing_from_mag.py 文件源码

def read_settings(self):
        # Declination
        self._declination = math.radians(rospy.get_param('~declination_deg', 0.0))

        # Calibration offset
        self._calibration_offset = np.array(rospy.get_param('~calibration_offset', [0.0, 0.0, 0.0]))

        # Calibration compensation
        self._calibration_compensation = np.array(rospy.get_param('~calibration_compensation',
                                                                  [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]))
        # create matrix from array
        self._calibration_compensation = self._calibration_compensation.reshape(3,3)

        # Constant bearing offset
        self._bearing_offset = math.radians(rospy.get_param('~bearing_constant_offset_deg', 0.0))

        # Number of samples used to compute average
        self._number_samples_average = rospy.get_param('~number_samples_average', 10)

        # Verbose
        self._verbose = rospy.get_param('~verbose', "True")

        # Print some useful information
        rospy.logwarn(rospy.get_name() +
                      " declination: " +
                      str(math.degrees(self._declination)) + " (deg)")

        rospy.logwarn(rospy.get_name() +
                      " constant bearing offset added to final bearing: " +
                      str(math.degrees(self._bearing_offset)) + " (deg)")

        if self._verbose:
            rospy.loginfo(rospy.get_name() +
                          " calibration offset: " + str(self._calibration_offset))
            rospy.loginfo(rospy.get_name() +
                          " calibration compensation: \n" +
                          str(self._calibration_compensation))
            rospy.loginfo(rospy.get_name() +
                          " number of samples to average: " +
                          str(self._number_samples_average))