封装动态坐标系的相对位置发布

需求

将动态坐标系的发布，进行封装，传入参数：话题、parent_link、child_link

代码实现

#！ usr/bin/env python

import rospy
import tf.transformations
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf
import tf2_ros
import sys
'''

    封装：传入话题、两个坐标系的名称
    需求：订阅小乌龟位置信息的话题，在回调函数中，发布小乌龟的位置信息
    步骤：
        1.导包
        2.初始化ros节点
        3.创建订阅者对象
        4.回调函数
        5.spin()

'''
pose_topic_name = sys.argv[1]
parent_link = sys.argv[2]
child_link = sys.argv[3]

def pub_callback(msg):
    #获取小乌龟的位置信息
    '''
    float32 x
    float32 y
    float32 theta
    float32 linear_velocity
    float32 angular_velocity
    '''

    rospy.loginfo("乌龟的位置信息：(%.2f,%.2f)",msg.x,msg.y)

    #有了两个坐标系之间的相对关系，转化成标准格式
    transform_msg = TransformStamped()
    '''
    std_msgs/Header header
        uint32 seq
        time stamp
        string frame_id
    string child_frame_id
    geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
        float64 x
        float64 y
        float64 z
    geometry_msgs/Quaternion rotation
        float64 x
        float64 y
        float64 z
        float64 w

    '''
    #---------header-----------------
    transform_msg.header.frame_id = parent_link
    transform_msg.header.stamp = rospy.Time.now()
    #---------child_frame_id-----------------
    transform_msg.child_frame_id = child_link
    #---------位置坐标系之间转化的坐标点，设置偏移量-----------------
    transform_msg.transform.translation.x = msg.x
    transform_msg.transform.translation.y = msg.y
    transform_msg.transform.translation.z = 0.0
    #---------四元数的转化----------------
    qtn = tf.transformations.quaternion_from_euler(0,0,msg.theta)
    transform_msg.transform.rotation.x = qtn[0]
    transform_msg.transform.rotation.y = qtn[1]
    transform_msg.transform.rotation.z = qtn[2]
    transform_msg.transform.rotation.w = qtn[3]

    #采用广播发布
    broadcaster = tf2_ros.TransformBroadcaster()
    broadcaster.sendTransform(transform_msg)






if __name__ == "__main__":
    
    #节点初始化
    rospy.init_node('tf_optimistic')

    #创建订阅者对象
    sub = rospy.Subscriber(pose_topic_name, Pose, pub_callback,queue_size=10)

    #编写回调函数

    #挂起，等待消息来临
    rospy.spin()
    

结果：

总结

关键API

import sys
sys.args:是获取命令中的数据，以列表形式进行的保存