ros编程位姿变换，原理以及代码实例

tf库是ROS中用于处理坐标系变换的核心工具，它的主要功能是维护一个坐标系变换树（Transform Tree），并允许用户在不同的坐标系之间进行位姿变换。以下是tf库的工作原理的简单说明：

1. 坐标系变换树（Transform Tree）

• tf库的核心是一个树状结构，称为坐标系变换树。每个节点代表一个坐标系，边代表两个坐标系之间的变换关系（包括平移和旋转）。

• 例如，机器人系统中常见的坐标系包括：

  • map：地图坐标系（全局固定）。
  • odom：里程计坐标系（局部固定）。
  • base_link：机器人基座坐标系。
  • sensor_frame：传感器坐标系（如激光雷达、摄像头）。

• 这些坐标系之间的变换关系构成了一个树状结构。例如：

  map -> odom -> base_link -> sensor_frame
  

2. 变换的发布与订阅

• 发布变换：

  • 通过tf库，节点可以发布两个坐标系之间的变换关系。例如，里程计节点可以发布odom到base_link的变换，传感器驱动节点可以发布base_link到sensor_frame的变换。
  • 发布的变换会被存储在tf库的缓冲区中。

• 订阅变换：

  • 其他节点可以通过tf库查询任意两个坐标系之间的变换关系。例如，导航节点可以查询map到base_link的变换，以确定机器人在全局地图中的位置。

3. 时间同步

• tf库支持时间同步，允许用户查询过去某一时刻的坐标系变换关系。
• 每个变换都带有时间戳，tf库会根据时间戳找到最接近的变换数据。

4. 工作原理的核心步骤

1. 发布变换：

   • 节点使用tf2_ros.TransformBroadcaster发布两个坐标系之间的变换关系。
   • 例如：

     transform_stamped = geometry_msgs.msg.TransformStamped()
     transform_stamped.header.stamp = rospy.Time.now()
     transform_stamped.header.frame_id = "parent_frame"  # 父坐标系
     transform_stamped.child_frame_id = "child_frame"   # 子坐标系
     transform_stamped.transform.translation.x = 1.0    # 平移
     transform_stamped.transform.rotation.w = 1.0       # 旋转（四元数）
     broadcaster.sendTransform(transform_stamped)       # 发布变换
     

2. 监听变换：

   • 节点使用tf2_ros.TransformListener监听坐标系变换，并通过tf2_ros.Buffer查询变换关系。
   • 例如：

     transform = tf_buffer.lookup_transform("target_frame", "source_frame", rospy.Time(0))
     

3. 应用变换：

   • 使用查询到的变换关系，将位姿从一个坐标系转换到另一个坐标系。
   • 例如：

     transformed_pose = tf2_geometry_msgs