TF_多坐标系下的转换

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

学习心得

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

阅读量581

点赞数 11

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

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

版权

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

11 篇文章 0 订阅

订阅专栏

需求

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

学习案例

静态坐标系的相对坐标系的发布
静态坐标系下坐标点的转换

实现思路：

利用小乌龟节点，产生两只小乌龟作为son1 和 son2 ，左下角原点为world，编写tf广播，发布两个坐标系的相对关系
随便设置一个在son1 的坐标，订阅广播，处理坐标转换。

代码实现

小乌龟产生，结合前面内容：

参考：动态坐标系相对关系的发布:

顶层launch文件

<launch>

<!-- 小乌龟启动节点 -->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"/>
<!-- 添加第二个小乌龟 -->
<node pkg="comunication_learn" type="turtlesim_create.py" name="turtle2" args= "turtle2 2 3 4 1.57"      output="screen"/>

<!-- 发布两个坐标系与世界坐标系的广播信息  -->
<node pkg="tf_learn" type="pub_tf_dynamic_m.py" name="son1" args= "/turtle1/pose world son1"      output="screen"/>
<node pkg="tf_learn" type="pub_tf_dynamic_m.py" name="son2" args= "/turtle2/pose world son2"      output="screen"/>

<node pkg="tf_learn" type="sub_tf_dynamic_m.py" name="listener_tf" args= "son2 son1"      output="screen"/>

</launch>

sub_tf_dynamic_m.py是接受相对坐标位置关系，并处理数据的文件。

#! usr/bin/env python

import rospy
import tf2_ros
import sys
from tf2_geometry_msgs import PointStamped
'''
需求：通过缓存获取坐标系之间的相对关系,然后进行数据处理
步骤：
    1.导包
    2.初始化ros节点
    3.创建缓存
    4.创建监听对象
    5.获取坐标系之间的相对关系  

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

if __name__ == "__main__":

    rospy.init_node("dynamic_tf_listener")
    rospy.loginfo("开始获取坐标系之间的相对关系")

    #创建缓存
    buffer = tf2_ros.Buffer()
    #等待缓存来临
    rospy.sleep(1)
    #创造监听对象
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        try:
            #获取坐标系之间的相对关系
            trans = buffer.lookup_transform(parent_link, child_link, rospy.Time())
            rospy.loginfo("获取成功")
            rospy.loginfo("位置：%s"%trans.transform.translation)
            rospy.loginfo("四元数：%s"%trans.transform.rotation)

             # 5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标
            point_source = PointStamped()
            point_source.header.frame_id = "son1"
            point_source.header.stamp = rospy.Time.now()
            point_source.point.x = 1
            point_source.point.y = 1
            point_source.point.z = 1

            point_target = buffer.transform(point_source, "son2",rospy.Duration(0.5))
            rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id)
            rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",
                        point_target.point.x,
                        point_target.point.y,
                        point_target.point.z
            )



        except Exception as e:
            rospy.logerr("错误提示:%s",e)
    rate.sleep()

在这里插入图片描述

总结

关键API

订阅者话题：rospy.Subscrible()
参数：订阅的话题名称、话题数据类型、回调函数、队列长度

程序挂起：rospy.spin()
保持主进程，等待调用回调函数

TF广播实例化：tf2_ros.TransformBroadcaster()
广播两个坐标系的相对位置关系
发送广播：tf2_ros.TransformBroadcaster.sendTransform(transform_msg)

TF接收
创建缓存区对象：buffer = tf2_ros.Buffer()
将TF消息放入缓存区内：tf2_ros.TransformLister(buffer)
延迟等待数据进入缓冲区：rospy.sleep(1)

TF相对坐标系信息的处理：
获取坐标的相对关系（从缓冲区中，任意取坐标系即可）：trans = buffer.lookup_transform(parent_link,child_link,rospy.Time())
相对坐标系位置关系查看：trans.transform.translation
相对坐标系的四元数位置查看：trans.transform.roration