科研向(仿真验证) ROS 学习笔记五 (A)：ROS常用组件之坐标变换

TF坐标变换

1. msg 结构说明：

   rosmsg info geometry_msgs/TransformStamped
   std_msgs/Header header                   #头信息
     uint32 seq                                #|-- 序列号
     time stamp                                #|-- 时间戳
     string frame_id                           #|-- 坐标 ID
   string child_frame_id                    #子坐标系的 id
   geometry_msgs/Transform transform        #坐标信息
     geometry_msgs/Vector3 translation        #偏移量
       float64 x                                #|-- X 方向(前)的偏移量
       float64 y                                #|-- Y 方向(左)的偏移量
       float64 z                                #|-- Z 方向(上)上的偏移量
     geometry_msgs/Quaternion rotation        #四元数
       float64 x                                
       float64 y                                
       float64 z                                
       float64 w
   
   rosmsg info geometry_msgs/PointStamped
   std_msgs/Header header                    #头
     uint32 seq                                #|-- 序号
     time stamp                                #|-- 时间戳
     string frame_id                           #|-- 所属坐标系的 id
   geometry_msgs/Point point                #点坐标
     float64 x                                 #|-- x y z 坐标
     float64 y
     float64 z
   

2. 相关功能包依赖如下：

    tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
   

3. python 代码实现：

   #! /usr/bin/env python
   """  
       静态坐标变换发布方:
           发布 laser 相对于base坐标系的位置信息 
   """
   import rospy
   import tf2_ros 
   import tf  
   from geometry_msgs.msg import TransformStamped
   
   if __name__ == "__main__":
       rospy.init_node("static_tf_pub_p")
       pub = tf2_ros.StaticTransformBroadcaster()  # 创建 静态坐标广播器
       tfs = TransformStamped()  # 消息
       tfs.header.frame_id = "world"
       tfs.header.stamp = rospy.Time.now()
       tfs.header.seq = 101
       tfs.child_frame_id = "radar"
       # 坐标系相对信息
       tfs.transform.translation.x = 0.2
       tfs.transform.translation.y = 0.0
       tfs.transform.translation.z = 0.5
       qtn = tf.transformations.quaternion_from_euler(0,0,0)
       tfs.transform.rotation.x = qtn[0]
       tfs.transform.rotation.y = qtn[1]
       tfs.transform.rotation.z = qtn[2]
       tfs.transform.rotation.w = qtn[3]
   
       pub.sendTransform(tfs)
       rospy.spin()
   

   #! /usr/bin/env python
   """  
       订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，
       转换成父级坐标系中的坐标点
   """
   import rospy
   import tf2_ros  # 为tf2提供了roscpp和rospy绑定
   #  tf2_geometry_msgs:可以将ROS消息转换成tf2消息,  tf2 封装了坐标变换的常用消息
   from tf2_geometry_msgs import PointStamped  # 非 geometry_msgs
   # from geometry_msgs.msg import PointStamped
   
   if __name__ == "__main__":
       rospy.init_node("static_sub_tf_p")
       buffer = tf2_ros.Buffer()  # 缓存保存订阅到的东西
       listener = tf2_ros.TransformListener(buffer)
   
       rate = rospy.Rate(1)
       while not rospy.is_shutdown():    
           point_source = PointStamped()
           point_source.header.frame_id = "radar"
           point_source.header.stamp = rospy.Time.now()
           point_source.point.x = 2.0
           point_source.point.y = 3.0
           point_source.point.z = 5.0
   
           try:
               point_target = buffer.transform(point_source,"world")
               rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                               point_target.point.x,
                               point_target.point.y,
                               point_target.point.z)
           except Exception as e:
               rospy.logerr("异常:%s",e)
               
           rate.sleep()
   

---

参考资料如下： ROS理论与实践