utils.py 文件源码

def broadcast_position(pose, to_frame, from_frame):
    broadcaster = tf.TransformBroadcaster()
    pose = pose
    broadcaster.sendTransform(
        (pose.position.x, pose.position.y, pose.position.z),
        (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w),
        rospy.Time.now(),
        to_frame,
        from_frame
    )