目录
- 一、动态坐标变换（C++实现）
- 二、动态坐标变换（Python实现）
一、动态坐标变换（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();
// --坐标系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: 万俟淋曦(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 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()
