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.do_transform_pose(pose_in_source_frame, transform)
     

---

5. 异常处理

• 在使用tf库时，可能会遇到以下异常：

  • LookupException：查找的坐标系不存在。
  • ConnectivityException：坐标系之间没有连接路径。
  • ExtrapolationException：请求的时间戳超出了变换的时间范围。

• 需要通过try-except块捕获并处理这些异常。

---

6. 可视化工具

• 使用rviz可以可视化坐标系变换树，帮助调试和验证变换是否正确。
• 在rviz中，可以通过TF插件查看所有坐标系的相对位置和方向。

---

总结

tf库的工作原理可以概括为：

1. 维护一个坐标系变换树。
2. 允许节点发布和订阅坐标系之间的变换关系。
3. 支持时间同步，查询过去某一时刻的变换。
4. 提供工具将位姿从一个坐标系转换到另一个坐标系。

通过tf库，开发者可以轻松处理复杂的坐标系变换问题，从而专注于机器人应用的开发。
以下是一个目标抓取的完整示例，展示如何使用ROS和tf库实现机械臂的目标抓取任务。这个例子包括以下步骤：

1. 获取目标物体在传感器坐标系中的位姿。
2. 将目标物体位姿转换到机械臂基座坐标系。
3. 使用逆运动学计算机械臂的关节角度。
4. 控制机械臂移动到目标位置并执行抓取动作。

---

1. 场景描述

• 机器人配备了一个摄像头和一个机械臂。
• 摄像头检测到目标物体的位姿（在摄像头坐标系中）。
• 机械臂需要抓取目标物体。

---

2. 代码实现

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/PoseStamped.h>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char** argv) {
    // 1. 初始化ROS节点
    ros::init(argc, argv, "target_grasping_example");
    ros::NodeHandle nh;

    // 2. 创建tf相关的对象
    tf2_ros::Buffer tf_buffer;
    tf2_ros::TransformListener tf_listener(tf_buffer);

    // 3. 创建MoveIt!运动规划接口
    moveit::planning_interface::MoveGroupInterface move_group("arm_group");

    // 4. 设置目标物体的位姿（假设在摄像头坐标系中）
    geometry_msgs::PoseStamped target_pose_camera_frame;
    target_pose_camera_frame.header.frame_id = "camera_frame";
    target_pose_camera_frame.pose.position.x = 0.5;  // 目标物体在摄像头坐标系中的x坐标
    target_pose_camera_frame.pose.position.y = 0.2;  // 目标物体在摄像头坐标系中的y坐标
    target_pose_camera_frame.pose.position.z = 0.3;  // 目标物体在摄像头坐标系中的z坐标
    target_pose_camera_frame.pose.orientation.w = 1.0;  // 无旋转

    // 5. 将目标物体位姿转换到机械臂基座坐标系
    geometry_msgs::PoseStamped target_pose_base_frame;
    try {
        // 查找从base_link到camera_frame的变换
        geometry_msgs::TransformStamped transform = tf_buffer.lookupTransform(
            "base_link", "camera_frame", ros::Time(0), ros::Duration(1.0));

        // 应用变换，将目标物体位姿从camera_frame转换到base_link
        tf2::doTransform(target_pose_camera_frame, target_pose_base_frame, transform);
        ROS_INFO("Target pose in base_link frame: [%f, %f, %f]",
                 target_pose_base_frame.pose.position.x,
                 target_pose_base_frame.pose.position.y,
                 target_pose_base_frame.pose.position.z);
    } catch (tf2::TransformException &ex) {
        ROS_ERROR("Failed to transform target pose: %s", ex.what());
        return -1;
    }

    // 6. 设置机械臂的目标位姿
    move_group.setPoseTarget(target_pose_base_frame);

    // 7. 进行运动规划
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (success) {
        ROS_INFO("Planning succeeded, moving the arm to the target pose.");
        move_group.move();  // 执行运动
    } else {
        ROS_ERROR("Failed to plan the motion.");
        return -1;
    }

    // 8. 执行抓取动作（假设抓取动作通过服务调用实现）
    ros::ServiceClient grasp_client = nh.serviceClient<std_srvs::Trigger>("grasp");
    std_srvs::Trigger grasp_srv;
    if (grasp_client.call(grasp_srv)) {
        ROS_INFO("Grasping action succeeded: %s", grasp_srv.response.message.c_str());
    } else {
        ROS_ERROR("Failed to call grasping service.");
        return -1;
    }

    return 0;
}

---

3. 代码说明

(1) 初始化ROS节点

• 使用ros::init初始化ROS节点，节点名为target_grasping_example。

(2) 创建tf相关的对象

• 使用tf2_ros::Buffer和tf2_ros::TransformListener监听坐标系变换。

(3) 创建MoveIt!运动规划接口

• 使用moveit::planning_interface::MoveGroupInterface创建机械臂的运动规划接口，组名为arm_group。

(4) 设置目标物体的位姿

• 假设目标物体在摄像头坐标系（camera_frame）中的位姿为(0.5, 0.2, 0.3)。

(5) 将目标物体位姿转换到机械臂基座坐标系

• 使用tf_buffer.lookupTransform查找从base_link到camera_frame的变换。
• 使用tf2::doTransform将目标物体位姿从camera_frame转换到base_link。

(6) 设置机械臂的目标位姿

• 使用move_group.setPoseTarget设置机械臂的目标位姿。

(7) 进行运动规划

• 使用move_group.plan进行运动规划。
• 如果规划成功，使用move_group.move执行运动。

(8) 执行抓取动作

• 假设抓取动作通过服务调用实现，使用ros::ServiceClient调用grasp服务。

---

4. 运行代码

1. 安装依赖：

   • 确保安装了MoveIt!和tf2_ros。
   • 如果未安装，可以使用以下命令安装：

     sudo apt-get install ros-noetic-moveit ros-noetic-tf2-ros
     

2. 创建ROS包：

   • 如果还没有ROS包，可以使用以下命令创建一个：

     catkin_create_pkg target_grasping roscpp tf2_ros moveit_core geometry_msgs
     

3. 编译并运行：

   • 将代码保存为src/target_grasping_example.cpp。
   • 修改CMakeLists.txt，添加以下内容：

     add_executable(target_grasping_example src/target_grasping_example.cpp)
     target_link_libraries(target_grasping_example ${catkin_LIBRARIES})
     

   • 编译并运行：

     catkin_make
     rosrun target_grasping target_grasping_example
     

---

5. 总结

这个示例展示了如何使用ROS和tf库实现目标抓取任务，包括：

1. 获取目标物体在传感器坐标系中的位姿。
2. 将目标物体位姿转换到机械臂基座坐标系。
3. 使用MoveIt!进行运动规划和控制。
4. 执行抓取动作。

通过这个例子，你可以掌握目标抓取的基本流程，并将其应用到实际的机器人项目中。