多传感器静态TF发布

最新推荐文章于 2024-01-01 22:46:54 发布

被苹果砸坏的牛顿

最新推荐文章于 2024-01-01 22:46:54 发布

阅读量146

点赞数 1

分类专栏：标定文章标签： python 数学建模开发语言

本文链接：https://blog.csdn.net/weixin_45482063/article/details/131137750

版权

标定专栏收录该内容

1 篇文章 0 订阅

订阅专栏

该代码示例展示了如何在ROS中使用TF2库发布和订阅静态TransformStamped消息。发布部分创建了一个类Tf_Fusion，从yaml文件读取传感器转换数据，然后通过StaticTransformBroadcaster广播TF。订阅部分创建了一个TransformListener来监听TF转换。

摘要由CSDN通过智能技术生成

1.发布静态TF

#! /usr/bin/env python
#-- coding:UTF-8 --
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import numpy as np
import yaml
import io

file="./Data.yaml"

def read_Normal_Yaml(file):
    f = io.open(file, 'r', encoding='utf-8')
    # cont
    cont = f.read()
    # config
    config = yaml.safe_load(cont)
    return config

class Tf_Fusion():
    def __init__(self,tfs,son_frame_id,sensor_name):
        # tfs=TransformStamped()
        self.tfs=tfs
        #---父坐标系
        self.tfs.header.frame_id = "rslidar"
        self.tfs.header.stamp = rospy.Time.now()
        self.tfs.header.seq = 101
        # --- 子坐标系
        self.tfs.child_frame_id = son_frame_id
        self.sensor_name = sensor_name

    def publish_tf(self,config):
        self.tfs.transform.translation.x = config[self.sensor_name]['transform']['translation']['x']
        self.tfs.transform.translation.y = config[self.sensor_name]['transform']['translation']['y']
        self.tfs.transform.translation.z = config[self.sensor_name]['transform']['translation']['z']

        # ------ 四元数
        self.tfs.transform.rotation.x = config[self.sensor_name]['transform']['rotation']['x']
        self.tfs.transform.rotation.y = config[self.sensor_name]['transform']['rotation']['y']
        self.tfs.transform.rotation.z = config[self.sensor_name]['transform']['rotation']['z']
        self.tfs.transform.rotation.w = config[self.sensor_name]['transform']['rotation']['w']

        qtn = [self.tfs.transform.rotation.x, self.tfs.transform.rotation.y,
               self.tfs.transform.rotation.z, self.tfs.transform.rotation.w]

        quat_norm = qtn / np.linalg.norm(qtn)  # 四元素归一化

        self.tfs.transform.rotation.x = quat_norm[0]
        self.tfs.transform.rotation.y = quat_norm[1]
        self.tfs.transform.rotation.z = quat_norm[2]
        self.tfs.transform.rotation.w = quat_norm[3]
        print(self.sensor_name+":发布TF成功！！！")
        return self.tfs


def talker():
    rospy.init_node('tf_fusion_listener', anonymous=True)
    rate = rospy.Rate(10)  # 10hz
    broadcaster = tf2_ros.StaticTransformBroadcaster()

    tfs_lidar=TransformStamped()  //添加传感器实现更多的TF发布

    tfs_all=[tfs_lidar]

    son_frame_id_all=["rslidar"]

    sensor_name_all = ["lidar"]

    while not rospy.is_shutdown():
        for i in range(len(tfs_all)):
            config = read_Normal_Yaml(file)
            lidar16_sensor = Tf_Fusion(tfs_all[i],son_frame_id_all[i],sensor_name_all[i])

            publish_tfs=lidar16_sensor.publish_tf(config)
            broadcaster.sendTransform(publish_tfs)

            rate.sleep()

if __name__ == '__main__':
    talker()

2.接收静态TF

#! /usr/bin/env python
#-- coding:UTF-8 --

import rospy
import tf2_ros

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("tf_fusion_listener")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
          print("订阅TF成功！！！")
          rate.sleep()