TF_动态坐标系之间的转换

最新推荐文章于 2024-09-11 18:01:31 发布

学习心得

最新推荐文章于 2024-09-11 18:01:31 发布

阅读量271

点赞数 3

分类专栏： ROS_入门项目文章标签： python

本文链接：https://blog.csdn.net/ccy1522929997/article/details/141089491

版权

ROS_入门项目专栏收录该内容

11 篇文章 0 订阅

订阅专栏

需求

启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

示例

学习案例

小乌龟订阅位置通信
TF_坐标变换案例

实现思路

实现目标：发布两个坐标系的相对位置关系
将小乌龟当成一个坐标系，已知话题 /turtle1/pose中会动态发布小乌龟的位置信息
将发布的小乌龟的位置信息进行相应的转换即可

具体：

编写一个可执行文件，去订阅话题里面的内容，在回调函数中，去发布相对位置信息即可。

代码实现

#！ 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
'''
    需求：订阅小乌龟位置信息的话题，在回调函数中，发布小乌龟的位置信息
    步骤：
        1.导包
        2.初始化ros节点
        3.创建订阅者对象
        4.回调函数
        5.spin()

'''

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 = "world"
    transform_msg.header.stamp = rospy.Time.now()
    #---------child_frame_id-----------------
    transform_msg.child_frame_id = "turtle1"
    #---------位置坐标系之间转化的坐标点，设置偏移量-----------------
    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('/turtle1/pose', Pose, pub_callback,queue_size=10)

    #编写回调函数

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