python版tf2_ros做transform

最新推荐文章于 2025-04-11 09:23:40 发布

原创

最新推荐文章于 2025-04-11 09:23:40 发布 · 3.9k 阅读

7 ·

CC 4.0 BY-SA版权

文章标签：

#ros #tf2 #python

本文介绍了如何使用Python的tf2_ros库进行坐标变换，重点解决了数据类型匹配的问题。通过使用tf2_geometry_msgs模块，避免了手动注册数据类型的困扰，简化了坐标变换的过程。文章提供了一个具体的tf2坐标变换实例。

之前曾想着使用tf2做机器人的'wrist' 到 'world'的tf变换，然而一直没有成功，提示数据的输入形式一直不对，因为在tf2中的输入数据类型必须是一个已经注册的数据，一直卡在这里，最终被迫使用tf进行坐标变换。

最近重新探索了一下tf2做transform的过程，发现只需要改一下数据类型就可以，而我们常用的数据类型其实tf2库已经帮我们注册好啦，使用起来非常方便。下面给出实现过程。

import rospy
import tf2_ros
from tf2_geometry_msgs import PoseStamped,PointStamped

if __name__ == "__main__":
    
    rospy.init_node('tf2_test')
    
    #设置tf2_listener
    tf_buffer=tf2_ros.Buffer()
    tf_listener=tf2_ros.TransformListener(tf_buffer)
    
    #定义要转换的输入
    p=PoseStamped()
    p.header.stamp=rospy.Time()
    p.header.frame_id='rh_wrist'
    p.pose.position.x=1
    p.pose.position.y=1
    p.pose.position.z=2
    p.pose.orientation.x=0
    p.pose.orientation.y=0
    p.pose.orientation.z=0
    p.pose.orientation.w=0

    #trans 是两个帧之间的转换关系，此处不用，直接屏蔽就行  
    #trans=tf_buffer.lookup_transform('world','rh_wrist',time=rospy.Time(0),timeout=rospy.Duration(5))
   

    res=tf_buffer.transform(p,'world',timeout=rospy.