ROS动态坐标转化

来自b站up：Autolabor官方

#dynamic_pub and dynamic_sub

#! /usr/bin/env python

"""
发布方订阅乌龟位置信息，转换成坐标系的相对关系，再发布
话题：
/turtle1/pose
类型：/turtlesim/Ppse
"""

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

def doPose(Pose):
    pub=tf2_ros.StaticTransformBroadcaster()
    # 4. 组织被发布的数据 头文件4
    tfs = TransformStamped()
    # 4.1 header
    tfs.header.stamp = rospy.Time.now()
    tfs.header.frame_id = "world"
    # 4.2 child frame
    tfs.child_frame_id = "turtle1"
    # 4.3 相对关系(偏移 与 四元数)  Pose的坐标系的坐标信息转化为父子两坐标系的相对信息
    tfs.transform.translation.x = Pose.x
    tfs.transform.translation.y = Pose.y
    tfs.transform.translation.z = 0
    # 4.4 先从欧拉角转换成四元数 头文件3
    #乌龟只有z轴上的偏航，没有x上的翻滚，没有y轴上的俯仰
    qtn = tf.transformations.quaternion_from_euler(0,0,Pose.theta)
    # 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)

if __name__ =='__main__':
    rospy.init_node('dynamic_pub')
    sub=rospy.Subscriber('/turtle1/pose',Pose,doPose,queue_size=100)
    rospy.spin()

#订阅
#! /usr/bin/env python
#encoding:utf-8
 
# """
# 发布方实现：订阅乌龟的位置信息，转换成坐标系的相对关系，再发布
# 1.导包
# 2.初始化ros结点
# 3.创建订阅对象 
# 4.回调函数处理订阅到的消息
# 5.spin()
# """
 
import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
def doPose(pose):
    #1.创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    #2.将pose位置信息转换成坐标系相对关系信息
    ts = TransformStamped()
    #被参考的坐标系
    ts.header.frame_id = "world"
    ts.child_frame_id = "turtle1"
    ts.header.stamp = rospy.Time.now()
    #子集坐标系相对于父集坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0
    #四元数的设置
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]
    #3.发布
    pub.sendTransform(ts)
    pass
 
if __name__ == "__main__":
    rospy.init_node("dynamic_pub_p")
    #创建订阅对象 : 话题名称 数据类型 回调函数
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)
 
    rospy.spin()
    pass