ROS TF坐标变换 - 动态坐标变换

一、动态坐标变换（C++实现）

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。比如机械臂末端执行器与 base_link 之间，移动机器人base_link与world之间。可以理解动态坐标关系是随时间变化的静态坐标关系（即静态是动态对时间的微分）。

我们使用ROS的 turtlesim 模拟一个移动机器人，通过TF发布它相对世界坐标系的坐标。

在创建的 tf2_learning 包路径下的 src 目录中创建 dynamic_frame_broadcast.cpp 和 dynamic_frame_listen.cpp ，修改 CMakeLists.txt ，添加如下内容：

add_executable(${PROJECT_NAME}_dynamic_broadcast src/dynamic_frame_broadcast.cpp)
add_executable(${PROJECT_NAME}_dynamic_listen src/dynamic_frame_listen.cpp)

target_link_libraries(${PROJECT_NAME}_dynamic_broadcast
  ${catkin_LIBRARIES}
)

target_link_libraries(${PROJECT_NAME}_dynamic_listen
  ${catkin_LIBRARIES}
)

dynamic_frame_broadcast.cpp 实现广播子坐标系相对于父坐标系的动态坐标关系，内容如下：

/**
 * @file: dynamic_frame_broadcast.cpp
 * @brief: 动态的坐标系相对姿态发布
 * @author: 万俟淋曦([email protected])
 * @date: 2023-12-30 22:47:33
 * @modifier:
 * @date: 2023-12-30 22:47:33
 */

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void turtle1PoseCallback(const turtlesim::Pose::ConstPtr &pose)
{
    // 创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;

    // 创建 广播的数据
    geometry_msgs::TransformStamped tfs;
    // --头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    // --坐标系id
    tfs.child_frame_id = "turtle1";
    // --坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维, z为0
    //  --欧拉角转四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, pose->theta); // 二维, 只有偏航角
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    // 广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char **argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "dynamic_frame_broadcast");

    // 创建 ROS 句柄
    ros::NodeHandle nh;

    // 创建订阅对象，订阅乌龟的世界位姿
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 1000, turtle1PoseCallback);

    ros::spin();

    return 0;
}

dynamic_frame_listen.cpp 订阅动态坐标转换关系，并利用该关系将小乌龟坐标系下的坐标转换到 world 坐标系，编辑内容如下：

/**
 * @file: dynamic_frame_listen.cpp
 * @brief: 订阅动态坐标系并转换相应坐标
 * @author: 万俟淋曦([email protected])
 * @date: 2023-12-31 11:55:40
 * @modifier:
 * @date: 2023-12-31 11:55:40
 */
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" // 包含TF坐标转换方法

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

    // 创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
        // 生成一个坐标点, 模拟末端执行器坐标系下的点坐标(小乌龟坐标系下的坐标)
        geometry_msgs::PointStamped point_turtle1;
        point_turtle1.header.frame_id = "turtle1";
        point_turtle1.header.stamp = ros::Time();
        point_turtle1.point.x = 1;
        point_turtle1.point.y = 1;
        point_turtle1.point.z = 0;

        // 转换坐标点, 计算小乌龟坐标系下的坐标点在 world 下的坐标
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_turtle1, "world");
            ROS_INFO("point_base: (%.2f, %.2f, %.2f), frame: %s", 
                point_base.point.x, 
                point_base.point.y, 
                point_base.point.z,
                point_base.header.frame_id.c_str());
        }
        catch (const std::exception &e)
        {
            ROS_ERROR("%s", e.what());
        }

        r.sleep();
        ros::spinOnce();
    }

    return 0;
}

编译后，

• 首先开启小乌龟 rosrun turtlesim turtlesim_node

• 执行 rosrun tf2_learning tf2_learning_dynamic_broadcast 开始广播坐标，此时打开rviz订阅TF看到TF树模型

• 输入命令：rviz

• 在启动的 rviz 中设置 Fixed Frame 为 world

• 点击左下的 Add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系，如下：

继续执行命令rosrun tf2_learning tf2_learning_listen可以看到转换后的坐标，以及所属父坐标系

执行命令 rosrun turtlesim turtle_teleop_key 使用键盘控制小乌龟移动，可以看到 rviz以及转换后的坐标都在同步动态变化。

二、动态坐标变换（Python实现）

在创建的 tf2_learning 包路径下 src 目录的同级，创建一个 scripts 目录，在这里存储脚本（如python脚本），我们创建 dynamic_frame_broadcast.py 以实现坐标广播，编辑内容如下：

#! /usr/bin/env python

import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped

# 回调函数处理
def turtle1PoseCallback(pose):
    # 创建 TF 广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    # 创建 广播的数据(通过 pose 设置)
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.child_frame_id = "turtle1"
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]
    # 广播器发布数据
    broadcaster.sendTransform(tfs)

if __name__ == "__main__":
    # 初始化 ROS 节点
    rospy.init_node("dynamic_frame_broadcast_py")
    # 订阅 /turtle1/pose 话题消息
    sub = rospy.Subscriber("/turtle1/pose", Pose, turtle1PoseCallback)
    rospy.spin()

创建 dynamic_frame_listen.py 以订阅静态坐标转换关系，并利用该关系将雷达坐标系的点转换到 world 坐标系，编辑内容如下：

#! /usr/bin/env python

import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 初始化 ROS 节点
    rospy.init_node("dynamic_frame_listen_py")
    # 创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 创建一个 radar 坐标系中的坐标点
        point_source = PointStamped()
        point_source.header.frame_id = "turtle1"
        point_source.header.stamp = rospy.Time.now()
        point_source.point.x = 10
        point_source.point.y = 2
        point_source.point.z = 3

        try:
            # 转换坐标点, 计算小乌龟坐标系下的坐标点在 world 下的坐标
            point_target = buffer.transform(point_source,"world",rospy.Duration(1))
            rospy.loginfo("point_target: (%.2f, %.2f, %.2f), frame: %s",
                            point_target.point.x,
                            point_target.point.y,
                            point_target.point.z,
                            point_target.header.frame_id)
        except Exception as e:
            rospy.logerr("%s", e)

        rate.sleep()