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_)