【将录包的tf数据实时发送出来】

由于在使用播放包的时候，十分依赖定位数据，但是过期的tf数据又不能再使用了，因此写了一个小脚本将过期的tf数据再发出来

import geometry_msgs.msg
import rclpy
import tf2_ros
from rclpy.node import Node


class MyNode(Node):

    def __init__(self):
        super().__init__('tf2_python_example_node')
        self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
        self.timer = self.create_timer(1, self.push)

    def push(self):
        dict_list = read_file()
        for item in dict_list:
            try:
                self.publish_tf(item)
            except:
                pass

    def publish_tf(self, dict_tf):
        # Create transform data
        print('send test tf')
        tf_msg = geometry_msgs.msg.TransformStamped()
        tf_msg.header.stamp = self.get_clock().now().to_msg()
        tf_msg.header.frame_id = dict_tf['frame_id']
        tf_msg.child_frame_id = dict_tf['child_frame_id']
        tf_msg.transform.translation.x = float(dict_tf['x'])
        tf_msg.transform.translation.y = float(dict_tf['y'])
        tf_msg.transform.translation.z = float(dict_tf['z'])
        tf_msg.transform.rotation.x = float(dict_tf['x1'])
        tf_msg.transform.rotation.y = float(dict_tf['y1'])
        tf_msg.transform.rotation.z = float(dict_tf['z1'])
        tf_msg.transform.rotation.w = float(dict_tf['w'])

        # Publish transform data
        self.tf_broadcaster.sendTransform(tf_msg)


def main(args=None):
    rclpy.init(args=args)
    node = MyNode()
    rclpy.spin(node)
    rclpy.shutdown()


def parse_header(header_str):
    header_dict = {}
    lines = header_str.replace(' ','').split('\n')
    for line in lines:
        try:
            key, value = line.split(':')
        except Exception as e:
            # print(e)
            continue

        if key in header_dict:
            key += '1'
        header_dict[key] = value
    return header_dict


def read_file(file_name='/home/hello/桌面/tf_static.txt'):
    with open(file_name, 'r') as f:
        words = f.read()
        word_list = words.split('- header:')
        dict_list = []
        for word in word_list:
            print(parse_header(word))
            dict_list.append(parse_header(word))

    return dict_list


if __name__ == '__main__':
    main()
    # read_file()

修改 def read_file(file_name=‘/home/hello/桌面/tf_static.txt’): 这一行中的文件名就可以；
文件是通过

ros2 topic echo /tf >>1,txt

这种方式记录的