get_robot_position.py 文件源码

def __init__(self):
        self.base_frame = rospy.get_param("base_frame_name","base_link")
        self.odom_frame = rospy.get_param("odom_frame_name","odom")
        self.tf_listener = tf.TransformListener()

        #??tf???????????odom?bask_link?TF
        self.tf_listener = tf.TransformListener()

        rospy.loginfo("[robot_state_pkg]->robot_position_state module is waiting for the tf between"
                      " %s and %s "%(self.base_frame , self.odom_frame))

        warn_time = 0
        wait_tf_start_time = rospy.Time.now()
        while not rospy.is_shutdown():
            is_tf_ok = self.tf_listener.canTransform(self.odom_frame,self.base_frame,rospy.Time())
            current_time = rospy.Time.now()
            if is_tf_ok:
                rospy.loginfo('[robot_state_pkg]->robot_position_state module the transform between '
                              '%s and %s is ok!!'%(self.odom_frame , self.base_frame))
                break
            if current_time.to_sec()-wait_tf_start_time.to_sec() >= 10.0 and warn_time == 0:
                warn_time += 1
                rospy.logwarn('[robot_state_pkg]->robot_position_state module the transform between '
                              '%s and %s is time out!!'%(self.odom_frame , self.base_frame))
                rospy.logwarn('[robot_state_pkg]->robot_position_state module this information only '
                              'warn once ,please check the odom !!!')

########################????????????tf??####################################

    #????????X?Y????