# -*- coding: utf-8 -*-

#!/usr/bin/env python

import rospy
import tf2_ros
import geometry_msgs.msg
from nav_msgs.msg import Odometry
import tf

def odometry_callback(msg):
    # 创建 TransformStamped 消息
    transformStamped = geometry_msgs.msg.TransformStamped()

    # 获取当前时间戳
    transformStamped.header.stamp = rospy.Time.now()
    transformStamped.header.frame_id = "odom"  # 原坐标系
    transformStamped.child_frame_id = "base_link"  # 目标坐标系

    # 提取平移信息
    transformStamped.transform.translation.x = msg.pose.pose.position.x
    transformStamped.transform.translation.y = msg.pose.pose.position.y
    transformStamped.transform.translation.z = msg.pose.pose.position.z

    # 提取旋转信息（四元数）
    q = msg.pose.pose.orientation
    transformStamped.transform.rotation.x = q.x
    transformStamped.transform.rotation.y = q.y
    transformStamped.transform.rotation.z = q.z
    transformStamped.transform.rotation.w = q.w

    # 发布TF变换
    tf_broadcaster.sendTransform(transformStamped)

    rospy.loginfo("Published transform from odom to base_link")

def listener():
    # 初始化 ROS 节点
    rospy.init_node('odom_to_base_link_tf_broadcaster')

    # 创建 TF 广播器
    global tf_broadcaster
    tf_broadcaster = tf2_ros.TransformBroadcaster()

    # 订阅 /laser_odom 话题
    rospy.Subscriber('/laser_odom', Odometry, odometry_callback)

    # 保持节点运行
    rospy.spin()

if __name__ == '__main__':
    try:
        listener()
    except rospy.ROSInterruptException:
        pass