ROS静态坐标转化

#! /usr/bin/env python

"""
    发布方: 发布两个坐标系的相对关系(车辆底盘 --- base_link 和 雷达 --- laser)
"""

import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
     
    # 2. 初始化节点
    rospy.init_node("start_pub_p")
    # 3. 创建发布对象 头文件2
    pub = tf2_ros.StaticTransformBroadcaster()
    # 4. 组织被发布的数据 头文件4
    tfs = TransformStamped()
    # 4.1 header
    tfs.header.stamp = rospy.Time.now()
    tfs.header.frame_id = "base_link"
    # 4.2 child frame
    tfs.child_frame_id = "laser"
    # 4.3 相对关系(偏移 与 四元数)
    tfs.transform.translation.x = 0.2
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.5
    # 4.4 先从欧拉角转换成四元数 头文件3
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    # 4.5 再设置四元数
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    # 5. 发布数据
    pub.sendTransform(tfs)
    # 6. spin()
    rospy.spin()
 

#! /usr/bin/env python
# encoding:utf-8
 
from ast import Try
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
# """
# 订阅方实现：订阅坐标变换信息，传入被转换的坐标点，调用转换算法
# """
 
if __name__ == "__main__":
 
    rospy.init_node("static_sub_p")
 
    buffer = tf2_ros.Buffer()
    sub = tf2_ros.transform_listener(buffer)
 
    ps = tf2_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:
            ps_out  = buffer.transform(ps,"base_link")
            rospy.loginfo("转换后的坐标：（%.2f,%.2f,%.2f),当前参考的坐标系：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id)
        except Exception as e:
            rospy.logwarn("%s",e)
        rate.sleep()