python版tf2_ros做transform

之前曾想着使用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.Duration(5))
#res 便是p在world坐标系里的pose
print (res)

可以发现输入数据msg的定义从我们常用的geometry_msg变成了 tf2_geometry_msgs。观察其源码

/opt/ros/kinetic/lib/python2.7/dist-packages/tf2_geometry_msgs

我们可以看到tf2_geometry_msgs 实质上是对geometry_msgs的继承，不过它又对我们常用的数据类型在tf2上进行了注册，省去了我们自己注册的麻烦。如果涉及到其他的数据类型，我们可以照猫画虎，注册要使用的数据类型。

最后给出使用tf进行transform的一个例子。

import rospy
from geometry_msgs.msg import PoseStamped
import tf
if __name__ == "__main__":
rospy.init_node('tf_test')
listener=tf.TransformListener()
point_trans=listener.waitForTransform(ref_frame, init_frame, rospy.Time(0), rospy.Duration(5))
point_=PointStamped()
point_.header.frame_id=init_frame
point_.header.stamp=rospy.Time()
point_.point.x=position_[0]
point_.point.y=position_[1]
point_.point.z=position_[2]
result_=listener.transformPoint(ref_frame,point_)
print(result_)

最后

以上就是谦让大地为你收集整理的python版tf2_ros做transform的全部内容，希望文章能够帮你解决python版tf2_ros做transform所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。