uwb_tracker_node.py 文件源码

def _read_configuration(self):
        """Initialize configuration from ROS parameters.
        """
        self.uwb_multi_range_topic = rospy.get_param('~multi_range_raw_topic', '/uwb/multi_range_with_offsets')
        self.uwb_tracker_topic = rospy.get_param('~tracker_topic', '/uwb/tracker')
        self.tracker_frame = rospy.get_param('~tracker_frame', 'uwb')
        self.target_frame = rospy.get_param('~target_frame', 'target')

        # Get parameters for covariance matrices
        self.initial_position_covariance = rospy.get_param('~initial_position_covariance', 10)
        self.process_covariance_position = rospy.get_param('~process_covariance_position', 0)
        self.process_covariance_velocity = rospy.get_param('~process_covariance_velocity', 1)
        self.measurement_covariance = rospy.get_param('~measurement_covariance', 0.1 ** 2)

        # Get parameters for filter update and initial gauss-newton estimation
        self.ignore_z_position = rospy.get_param('~ignore_z_position', True)
        # The default value of 7.779 represents the 0.9 quantile of a Chi-Square distribution
        # with 4 degrees of freedom (4 UWB measurements).
        self.outlier_threshold_quantile = rospy.get_param('~outlier_threshold_quantile', 0.1)
        self.ikf_iterations = rospy.get_param('~ikf_iterations', 2)
        self.initial_guess_position = np.empty((3, 1), dtype=np.float)
        self.initial_guess_position[0] = rospy.get_param('~initial_guess_position_x', 0)
        self.initial_guess_position[1] = rospy.get_param('~initial_guess_position_y', 0)
        self.initial_guess_position[2] = rospy.get_param('~initial_guess_position_z', 0)
        self.initial_guess_iterations = rospy.get_param('~initial_guess_iterations', 200)
        self.initial_guess_tolerance = rospy.get_param('~initial_guess_tolerance', 1e-5)
        self.initial_guess_residuals_threshold = rospy.get_param('~initial_guess_residuals_threshold', 0.1)
        self.ikf_max_outlier_count = rospy.get_param('~ikf_max_outlier_count', 200)