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: 万俟淋曦(1055311345@qq.com)* @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();// --坐标系idtfs.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: 万俟淋曦(1055311345@qq.com)* @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 pythonimport 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.xtfs.transform.translation.y = pose.ytfs.transform.translation.z = 0.0qtn = 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 pythonimport rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStampedif __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 = 10point_source.point.y = 2point_source.point.z = 3try:# 转换坐标点, 计算小乌龟坐标系下的坐标点在 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()