ROS TF static coordinate transformation implementation

Method 1: Coding Implementation

① Publisher code implementation: create a function package and add dependencies

catkin_create_pkg tf_static roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

②Create a new python file, add executable permissions to the file, and modify the CMakelists file

③ Write the publisher code, compile and execute

#! /usr/bin/env python
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf.transformations #欧拉角获取四元数
"""
发布方实现：用于发布两个坐标系的新相对关系（小车底盘——baselink和雷达——laser）
流程：1、导包
    2、初始化ROS节点
    3、创建发布对象
    4、组织被发布的数据
    5、循环发布数据
    6、spin（）
"""
if __name__=="__main__":
    rospy.init_node("tf_static_pub")
    #创建发布对象
    pub = tf2_ros.StaticTransformBroadcaster()
    #组织被发布的数据
    ts = TransformStamped()
    #header
    ts.header.stamp=rospy.Time.now()#设置时间戳
    ts.header.frame_id = "baselink"#父级坐标系
    #child fram
    ts.child_frame_id = "laser"

    #相对关系（偏移和四元数）
    ts.transform.translation.x = 0.2
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5
    #四元数：一般先设置欧拉角，再转换成四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)#是一个列表对象，可以下标索引获取对应的值
    #四元数获取
    ts.transform.rotation.x =qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    #发布数据
    pub.sendTransform(ts)
    rospy.spin()

④ View the data released by the publisher

Rostopic list, you can see that there is a topic named tf_static

insert image description here

Rostopic echo tf_static can see the specific information released

insert image description here

You can also use the visualization tool rviz to visually see the two coordinate systems: enter rviz in the terminal to open the visualization window, select Baselink for Fixed Frame in the upper left corner, add in the lower left corner, and add TF to see)

insert image description here

④ Subscriber code writing

Create a new python file tf_static_sub.py, add executable permissions to the file, and modify the CMakelists file

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
"""
订阅方实现：
订阅坐标变换消息，传入被转换的坐标点，调用转换算法实现坐标转换
1、导包（用到什么包导入什么包）
2、初始化ros节点
3、创建订阅对象
4、组织被转换的坐标点
5、转换逻辑编写（调用tf封装算法）
6、输出转换结果B
7、spin（）/spinonce（）
"""
if __name__=="__main__":
    #初始化ros节点
    rospy.init_node("static_tf_pub")
    #创建订阅对象
    buffer = tf2_ros.Buffer() #创建缓存对象
    sub = tf2_ros.TransformListener(buffer)#将缓存对象传入
    #组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()#可以用rosmsg info geometry_msgs.PointStamped 查看消息结构
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser"
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0
    #转换逻辑实现，由于雷达是不间断获取信息的，这里用循环发布数据来模拟
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #转换：buffer提供了对应的转换函数
            ps_out = buffer.transform(ps,"baselink")
            rospy.loginfo("转换后的坐标结果:(%.2f,%.2f,%.2f)当前参考的坐标系：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id) 
        except:
            rospy.logwarn("错误提示：%s",Exception)
        rate.sleep()

Method 2: ros encapsulation command to realize static coordinate transformation

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 x轴偏转角 y轴偏转角 z轴偏转角 父坐标系名 子坐标系名

It can achieve the same effect as encoding, and you can also view the topic name and message format specific information through rostopic. You can also see the relative position of the two coordinate systems through rviz visualization