【5】ros2的工具介绍----TF坐标变换，RQT，Riz，bag，Git等相关工具介绍
旋转矩阵、欧拉角和四元数。
·
1.TF坐标变换
在ROS的TF坐标变换中，三维旋转可以用三种“语言”来描述：旋转矩阵、欧拉角和四元数。它们本质上是描述同一个事物的三种不同方式。
1.1 旋转矩阵
旋转矩阵是一个 3×3 的正交矩阵，由旋转后的坐标系三个轴分别与原坐标系三个轴的夹角余弦值共九个数字组成。它的核心形式为：
1.2 欧拉角
欧拉角用三个角度来刻画刚体的旋转，通常称为：横滚角（Roll，绕X轴）、俯仰角（Pitch，绕Y轴）和偏航角（Yaw，绕Z轴）。其核心表达为：
Euler(ϕ,θ,ψ)=(Roll,Pitch,Yaw)
用日常语言理解：Roll是飞机翅膀上下倾斜，Pitch是机头上仰或下俯，Yaw是机头左右转动。欧拉角依赖旋转顺序，不同的顺序（如Z-Y-X、X-Y-Z）会产生不同的结果，航空航天领域常用Z-Y-X（yaw-pitch-roll）顺序。
1.3 四元数
四元数由一个实部和三个虚部组成，表示为(x, y, z, w)或(qx, qy, qz, qw)。在ROS的geometry_msgs中，x、y、z是虚部，w是实部且放在最后。
表达公式如下：
1.4优缺点总结
1.5理解旋转矩阵、欧拉角和四元数可视化工具
安装可视化工具
sudo apt install mrpt-apps
打开可视化工具
3d-rotation-converter
可以看到改变欧拉角之后旋转矩阵和四元数也跟着变化
1.6项目实战理解TF—python版本
安装TF环境
sudo apt install ros-$ROS_DISTRO-tf-transformations
项目背景
1.已知base_link（机械臂底座）跟camera_link(相机)位置坐标，需发布静态TF关系。
（注意，由于两者位置静态固定不变的，所以应该用静态坐标来求，对应python/c++中的StaticTransformBroadcaster）
2.已知camera_link(相机)跟bottle_link(瓶顶)位置坐标，需发布动态TF关系。
（注意，由于两者位置动态改变的，所以应该用动态坐标来求，对应python/c++中的TransformBroadcaster）
静态跟动态的区别就是，动态多了一个定时器，定时发布
3.已知上面1,2的转换关系，求base_link跟bottle_link的位置关系。
这里就是查询TF关系，主要步骤：将上面静态TF关系/动态TF关系全都放在一个buffer类型的容器，在调用buffer里函数计算，同时，还需要调用TransformListener（坐标监听器类）持续向 buffer填充数据，也需要定时器，因为输入中存在一个动态TF
1.6.1发布静态TF(base_link与camera_link的TF)
ros2/chapt5/chapt5_ws/src文件下创建功能包
ros2 pkg create demo_python_tf --build-type ament_python --dependencies rclpy geometry_msgs tf_ros tf_transformations --license Apache-2.0
在ros2/chapt5/chapt5_ws/src/demo_python_tf/demo_python_tf下面新建文件
static_tf_broadcaster.py
代码如下：
import math
import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler
class StaticTFBroadcaster(Node):
def __init__(self):
super().__init__('static_tf2_broadcaster')
self.static_broadcaster_ = StaticTransformBroadcaster(self)
self.publish_static_tf()
def publish_static_tf(self):
transform = TransformStamped()
transform.header.stamp = self.get_clock().now().to_msg()
transform.header.frame_id = "base_link"
transform.child_frame_id = "camera_link"
#------------------------第一部分给定的xyz位置信息-------------------------------------
transform.transform.translation.x = 0.5
transform.transform.translation.y = 0.3
transform.transform.translation.z = 0.6
#------------------------第二部分给定的旋转信息-------------------------------------
# 欧拉角转四元数
#人工指定一个旋转姿态（这里指定了绕 X 轴转 180 度），并将它从欧拉角形式转换成 ROS 需要的四元数形式。
rotation_quat = quaternion_from_euler(math.radians(180), 0, 0)
#提取四元数里面的信息
transform.transform.rotation.x = rotation_quat[0]
transform.transform.rotation.y = rotation_quat[1]
transform.transform.rotation.z = rotation_quat[2]
transform.transform.rotation.w = rotation_quat[3]
#------------------------第三部分发布信息-------------------------------------
# 发布静态坐标变换
self.static_broadcaster_.sendTransform(transform)
self.get_logger().info(f"发布 TF:{transform}")
def main():
rclpy.init()
static_tf_broadcaster = StaticTFBroadcaster()
rclpy.spin(static_tf_broadcaster)
rclpy.shutdown()
上面文件关键代码：
#------------------------第一部分给定的xyz位置信息-------------------------------------
transform.transform.translation.x = 0.5
transform.transform.translation.y = 0.3
transform.transform.translation.z = 0.6
#------------------------第二部分给定的旋转信息-------------------------------------
# 欧拉角转四元数
#人工指定一个旋转姿态（这里指定了绕 X 轴转 180 度），并将它从欧拉角形式转换成 ROS 需要的四元数形式。
rotation_quat = quaternion_from_euler(math.radians(180), 0, 0)
#提取四元数里面的信息
transform.transform.rotation.x = rotation_quat[0]
transform.transform.rotation.y = rotation_quat[1]
transform.transform.rotation.z = rotation_quat[2]
transform.transform.rotation.w = rotation_quat[3]
数据来自于
修改setup.py文件
新增内容：
entry_points={
'console_scripts': [
'static_tf_broadcaster=demo_python_tf.static_tf_broadcaster:main',
],
},
终端路径：ros2/chapt5/chapt5_ws/src$
开始构建
colcon build
编译环境
source install/setup.bash
运行
ros2 run demo_python_tf static_tf_broadcaster
输出
[INFO] [1778738316.277443574] [static_tf2_broadcaster]: 发布 TF:geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1778738316, nanosec=271075646), frame_id='base_link'), child_frame_id='camera_link',
transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.5, y=0.3, z=0.6), rotation=geometry_msgs.msg.Quaternion(x=1.0, y=0.0, z=0.0, w=6.123233995736766e-17)))
1.6.2发布动态TF(bottle_link与camera_link的TF)
在ros2/chapt5/chapt5_ws/src/demo_python_tf/demo_python_tf下面新建文件
dynamic_tf_broadcaster.py
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler
class DynamicTFBroadcaster(Node):
def __init__(self):
super().__init__("tf_node")
self.tf_broadcaster_ = TransformBroadcaster(self)
# 动态TF需要持续发布，这里发布频率设置为 100HZ
self.timer_ = self.create_timer(0.01, self.publish_transform)
def publish_transform(self):
transform = TransformStamped()
transform.header.stamp = self.get_clock().now().to_msg()
transform.header.frame_id = "camera_link"
transform.child_frame_id = "bottle_link"
transform.transform.translation.x = 0.2
transform.transform.translation.y = 0.0
transform.transform.translation.z = 0.5
rotation_quat = quaternion_from_euler(0, 0, 0)
transform.transform.rotation.x = rotation_quat[0]
transform.transform.rotation.y = rotation_quat[1]
transform.transform.rotation.z = rotation_quat[2]
transform.transform.rotation.w = rotation_quat[3]
self.tf_broadcaster_.sendTransform(transform)
# 发布坐标变换
self.tf_broadcaster_.sendTransform(transform)
self.get_logger().info(f"发布 TF:{transform}")
def main():
rclpy.init()
tf_node = DynamicTFBroadcaster()
rclpy.spin(tf_node)
rclpy.shutdown()
上面文件关键代码
# 动态TF需要持续发布，这里发布频率设置为 100HZ
self.timer_ = self.create_timer(0.01, self.publish_transform)
跟静态的区别就是把publish_transform发布函数放到计时器里面
还有注意！！！发布时候也有区别
动态广播器：std::make_shared<tf2_ros::TransformBroadcaster>(this)
静态广播器：std::make_shared<tf2_ros::StaticTransformBroadcaster>(this)
数据来源：
修改setup.py文件
新增内容：
entry_points={
'console_scripts': [
'static_tf_broadcaster=demo_python_tf.static_tf_broadcaster:main',
'dynamic_tf_broadcaster=demo_python_tf.dynamic_tf_broadcaster:main',
],
},
终端路径：ros2/chapt5/chapt5_ws/src$
开始构建
colcon build
编译环境
source install/setup.bash
运行
ros2 run demo_python_tf dynamic_tf_broadcaster
输出
[INFO] [1778740077.946561263] [tf_node]: 发布 TF:geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1778740077, nanosec=938207861), frame_id='camera_link'), child_frame_id='bottle_link', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.2, y=0.0, z=0.5), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)))
[INFO] [1778740077.948494572] [tf_node]: 发布 TF:geometry_msgs.msg.TransformStamped(header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=1778740077, nanosec=948166857), frame_id='camera_link'), child_frame_id='bottle_link', transform=geometry_msgs.msg.Transform(translation=geometry_msgs.msg.Vector3(x=0.2, y=0.0, z=0.5), rotation=geometry_msgs.msg.Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)))
1.6.3查询TF(根据上面两个TF节点，计算base_link与camera_link的TF)
在ros2/chapt5/chapt5_ws/src/demo_python_tf/demo_python_tf下面新建文件
tf_listener.py
import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer
from tf_transformations import euler_from_quaternion
class TFListener(Node):
def __init__(self):
super().__init__("tf2_listener")
self.buffer_ = Buffer()
self.listener_ = TransformListener(self.buffer_, self)
self.timer_ = self.create_timer(1, self.get_transform)
def get_transform(self):
try:
result = self.buffer_.lookup_transform("base_link", "bottle_link", rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
transform = result.transform
rotation_euler = euler_from_quaternion([
transform.rotation.x,
transform.rotation.y,
transform.rotation.z,
transform.rotation.w
])
self.get_logger().info(f"平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}")
except Exception as e:
self.get_logger().warn(f"不能够获取坐标变换，原因: {str(e)}")
def main():
rclpy.init()
node = TFListener()
rclpy.spin(node)
rclpy.shutdown()
上面文件关键代码
self.buffer_ = Buffer()
self.listener_ = TransformListener(self.buffer_, self)
在 tf_listener 代码中的确没有直接出现 static_tf_broadcaster 或dynamic_tf_broadcaster 的“输入”。这是因为 ROS 的 TF系统是通过话题（topic）通信的，广播器和监听器是两个独立的节点，它们之间不需要在代码中显式传递对象或数据。TransformListener会自动订阅 /tf 和 /tf_static 话题。每当广播器发布新的变换消息时，listener_ 就会把它存入 buffer_中缓存起来。因此，您不需要在代码中显式“输入”广播器——话题是它们的桥梁。
在这里输入你需要计算/打印的TF，这里result为base_link与bottle_link的TF关系
result = self.buffer_.lookup_transform("base_link", "bottle_link", rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
修改setup.py文件
新增内容：
entry_points={
'console_scripts': [
'static_tf_broadcaster=demo_python_tf.static_tf_broadcaster:main',
'dynamic_tf_broadcaster=demo_python_tf.dynamic_tf_broadcaster:main',
'tf_listener=demo_python_tf.tf_listener:main',
],
},
终端路径：ros2/chapt5/chapt5_ws/src$
开始构建
colcon build
编译环境
source install/setup.bash
运行打开三个终端分别运行
base_link与camera_link的TF
ros2 run demo_python_tf static_tf_broadcaster
bottle_link与camera_link的TF
ros2 run demo_python_tf dynamic_tf_broadcaster
第三个运行查询TF
计算bottle_link和base_link的TF
ros2 run demo_python_tf tf_listener
输出
[INFO] [1778740925.304551338] [tf2_listener]: 平移:geometry_msgs.msg.Vector3(x=0.7, y=0.29999999999999993, z=0.09999999999999998),旋转四元数:geometry_msgs.msg.Quaternion(x=1.0, y=0.0, z=0.0, w=6.123233995736766e-17):旋转欧拉角:(3.141592653589793, -0.0, 0.0)
[INFO] [1778740926.296722657] [tf2_listener]: 平移:geometry_msgs.msg.Vector3(x=0.7, y=0.29999999999999993, z=0.09999999999999998),旋转四元数:geometry_msgs.msg.Quaternion(x=1.0, y=0.0, z=0.0, w=6.123233995736766e-17):旋转欧拉角:(3.141592653589793, -0.0, 0.0)
1.7项目实战理解TF—c++版本
安装TF环境
sudo apt install ros-$ROS_DISTRO-tf-transformations
项目背景
1.已知map跟target_point位置坐标，需发布静态TF关系。
（注意，由于两者位置静态固定不变的，所以应该用静态坐标来求，对应python/c++中的StaticTransformBroadcaster）
2.已知map跟base_link位置坐标，需发布动态TF关系。
（注意，由于两者位置动态改变的，所以应该用动态坐标来求，对应python/c++中的TransformBroadcaster）
静态跟动态的区别就是，动态多了一个定时器，定时发布
3.已知上面1,2的转换关系，求target_point跟base_link的位置关系。
这里就是查询TF关系，主要步骤：将上面静态TF关系/动态TF关系全都放在一个buffer类型的容器，在调用buffer里函数计算，同时，还需要调用TransformListener（坐标监听器类）持续向 buffer填充数据，也需要定时器，因为输入中存在一个动态TF
1.7.1发布静态TF(map与target_point的TF)
ros2/chapt5/chapt5_ws/src文件下创建功能包
ros2 pkg create demo_cpp_tf --build-type ament_cmake --dependencies rclpy tf2_ros geometry_msgs tf2_geometry_msgs --license Apache-2.0
在ros2/chapt5/chapt5_ws/src/demo_cpp_tf/src下面新建文件
static_tf_broadcaster.cpp
代码如下：
#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/static_transform_broadcaster.h"
class StaticTFBroadcaster : public rclcpp::Node {
public:
StaticTFBroadcaster() : Node("tf_broadcaster_node") {
RCLCPP_INFO(this->get_logger(), "静态TF广播器节点已启动");
broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
this->publish_tf();
}
void publish_tf() {
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = this->get_clock()->now();
//--------------------第一步，指定头跟尾--------------------
transform.header.frame_id = "map";
transform.child_frame_id = "target_point";
//--------------------第二步，传xyz的值---------------------
transform.transform.translation.x = 5.0;
transform.transform.translation.y = 3.0;
transform.transform.translation.z = 0.0;
//--------------------第三步，传角度值---------------------
tf2::Quaternion quat;
quat.setRPY(0, 0, 60 * M_PI / 180); // 绕Z轴旋转60度
transform.transform.rotation = tf2::toMsg(quat);
// 打印平移信息
RCLCPP_INFO(this->get_logger(),
"发布静态TF: %s -> %s",
transform.header.frame_id.c_str(),
transform.child_frame_id.c_str());
RCLCPP_INFO(this->get_logger(),
"平移: (%.2f, %.2f, %.2f)",
transform.transform.translation.x,
transform.transform.translation.y,
transform.transform.translation.z);
// 打印四元数
RCLCPP_INFO(this->get_logger(),
"旋转四元数: (%.3f, %.3f, %.3f, %.3f)",
transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z,
transform.transform.rotation.w);
// 计算并打印欧拉角（度）
double roll, pitch, yaw;
tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
RCLCPP_INFO(this->get_logger(),
"旋转欧拉角(度): (%.1f°, %.1f°, %.1f°)",
roll * 180.0 / M_PI,
pitch * 180.0 / M_PI,
yaw * 180.0 / M_PI);
broadcaster_->sendTransform(transform);
RCLCPP_INFO(this->get_logger(), "静态TF已发送至 /tf_static 话题");
}
private:
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> broadcaster_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<StaticTFBroadcaster>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
如果引入头文件报错
步骤1，按F1键
步骤2，输入C/C++: Edit Configurations (UI)
步骤3，找到包含路径，添加
${workspaceFolder}/**
/opt/ros/humble/include/**
/usr/include/x86_64-linux-gnu/qt5/**
关键代码：
//--------------------第一步，指定头跟尾--------------------
transform.header.frame_id = "map";
transform.child_frame_id = "target_point";
//--------------------第二步，传xyz的值---------------------
transform.transform.translation.x = 5.0;
transform.transform.translation.y = 3.0;
transform.transform.translation.z = 0.0;
//--------------------第三步，传角度值---------------------
tf2::Quaternion quat;
quat.setRPY(0, 0, 60 * M_PI / 180); // 绕Z轴旋转60度
transform.transform.rotation = tf2::toMsg(quat);
数据来自于
修改CMakeLists.txt文件
新增内容：
add_executable(static_tf_broadcaster src/static_tf_broadcaster.cpp)
ament_target_dependencies(static_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)
install(TARGETS
static_tf_broadcaster
DESTINATION lib/${PROJECT_NAME}
)
终端路径：ros2/chapt5/chapt5_ws/src$
开始构建
colcon build
编译环境
source install/setup.bash
运行
ros2 run demo_cpp_tf static_tf_broadcaster
输出
[INFO] [1778746067.083097811] [tf_broadcaster_node]: 静态TF广播器节点已启动
[INFO] [1778746067.084223794] [tf_broadcaster_node]: 发布静态TF: map -> target_point
[INFO] [1778746067.084246417] [tf_broadcaster_node]: 平移: (5.00, 3.00, 0.00)
[INFO] [1778746067.084255223] [tf_broadcaster_node]: 旋转四元数: (0.000, 0.000, 0.500, 0.866)
[INFO] [1778746067.084264574] [tf_broadcaster_node]: 旋转欧拉角(度): (0.0°, -0.0°, 60.0°)
[INFO] [1778746067.084300489] [tf_broadcaster_node]: 静态TF已发送至 /tf_static 话题
1.7.2发布动态TF(map与base_link的TF)
在ros2/chapt5/chapt5_ws/src/demo_cpp_tf/src下面新建文件
dynamic_tf_broadcaster.cpp
#include <memory>
#include <cmath> // 提供 M_PI
#include <chrono>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h" // 提供 Matrix3x3 用于欧拉角转换
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/transform_broadcaster.h"
using namespace std::chrono_literals;
class DynamicTFBroadcaster : public rclcpp::Node
{
public:
DynamicTFBroadcaster() : Node("dynamic_tf_broadcaster")
{
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
//---------------------------跟静态比就多了个计时器-----------------------
timer_ = create_wall_timer(10ms, std::bind(&DynamicTFBroadcaster::publishTransform, this));
RCLCPP_INFO(this->get_logger(), "动态TF广播器已启动，周期 10ms");
}
void publishTransform()
{
geometry_msgs::msg::TransformStamped transform;
transform.header.stamp = this->get_clock()->now();
transform.header.frame_id = "map";
transform.child_frame_id = "base_link";
transform.transform.translation.x = 2.0;
transform.transform.translation.y = 3.0;
transform.transform.translation.z = 0.0;
tf2::Quaternion quat;
quat.setRPY(0, 0, 30 * M_PI / 180); // 绕 Z 轴旋转 30°
transform.transform.rotation = tf2::toMsg(quat);
// 打印变换信息
RCLCPP_INFO(this->get_logger(),
"[动态TF] %s -> %s, 时间戳: %.3f",
transform.header.frame_id.c_str(),
transform.child_frame_id.c_str(),
transform.header.stamp.sec + transform.header.stamp.nanosec * 1e-9);
RCLCPP_INFO(this->get_logger(),
" 平移: (%.2f, %.2f, %.2f)",
transform.transform.translation.x,
transform.transform.translation.y,
transform.transform.translation.z);
RCLCPP_INFO(this->get_logger(),
" 四元数: (%.3f, %.3f, %.3f, %.3f)",
transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z,
transform.transform.rotation.w);
// 转换欧拉角（弧度 -> 度）并打印
double roll, pitch, yaw;
tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
RCLCPP_INFO(this->get_logger(),
" 欧拉角(度): (%.1f°, %.1f°, %.1f°)",
roll * 180.0 / M_PI,
pitch * 180.0 / M_PI,
yaw * 180.0 / M_PI);
tf_broadcaster_->sendTransform(transform);
}
private:
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<DynamicTFBroadcaster>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
上面文件关键代码
//---------------------------跟静态比就多了个计时器-----------------------
timer_ = create_wall_timer(10ms, std::bind(&DynamicTFBroadcaster::publishTransform, this));
跟静态的区别就是把publish_transform发布函数放到计时器里面
还有注意！！！发布时候也有区别
tf_broadcaster_->sendTransform(transform);#动态发布
broadcaster_->sendTransform(transform);#静态发布
数据来源：
修改CMakeLists.txt文件
新增内容：
add_executable(static_tf_broadcaster src/static_tf_broadcaster.cpp)
ament_target_dependencies(static_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)
add_executable(dynamic_tf_broadcaster src/dynamic_tf_broadcaster.cpp)
ament_target_dependencies(dynamic_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)
install(TARGETS
static_tf_broadcaster
dynamic_tf_broadcaster
DESTINATION lib/${PROJECT_NAME}
)
终端路径：ros2/chapt5/chapt5_ws/src$
开始构建
colcon build
编译环境
source install/setup.bash
运行
ros2 run demo_python_tf dynamic_tf_broadcaster
输出
[INFO] [1778746625.645521279] [dynamic_tf_broadcaster]: 平移: (2.00, 3.00, 0.00)
[INFO] [1778746625.645525878] [dynamic_tf_broadcaster]: 四元数: (0.000, 0.000, 0.259, 0.966)
[INFO] [1778746625.645530925] [dynamic_tf_broadcaster]: 欧拉角(度): (0.0°, -0.0°, 30.0°)
[INFO] [1778746625.655414946] [dynamic_tf_broadcaster]: [动态TF] map -> base_link, 时间戳: 1778746625.655
1.7.3查询TF(根据上面两个TF节点，计算target_point与base_link的TF)
在ros2/chapt5/chapt5_ws/src/demo_cpp_tf/src下面新建文件
tf_listener.cpp
#include <memory>
#include "geometry_msgs/msg/transform_stamped.hpp" // 提供消息接口
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h" // 提供 tf2::Quaternion 类
#include "tf2/utils.h" // 提供 tf2::getEulerYPR 函数
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" // 提供消息类型转换函数
#include "tf2_ros/buffer.h" // 提供 TF 缓冲类 Buffer
#include "tf2_ros/transform_listener.h" // 提供坐标监听器类
#include <chrono> // 引入时间相关头文件
using namespace std::chrono_literals;
class TFListener : public rclcpp::Node {
public:
TFListener() : Node("tf_listener") {
buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);
timer_ = this->create_wall_timer(5s, std::bind(&TFListener::getTransform, this));
}
void getTransform() {
try {
// 等待变换可用
const auto transform = buffer_->lookupTransform(
"base_link", "target_point", this->get_clock()->now(),
rclcpp::Duration::from_seconds(1.0f));
// 转换结果及打印
const auto &translation = transform.transform.translation;
const auto &rotation = transform.transform.rotation;
double yaw, pitch, roll;
tf2::getEulerYPR(rotation, yaw, pitch, roll); // 四元数转欧拉角
RCLCPP_INFO(get_logger(), "平移分量: (%f, %f, %f)", translation.x,
translation.y, translation.z);
RCLCPP_INFO(get_logger(), "旋转分量: (%f, %f, %f)", roll, pitch, yaw);
} catch (tf2::TransformException &ex) {
RCLCPP_WARN(get_logger(), "异常: %s", ex.what()); // 处理异常
}
}
private:
std::unique_ptr<tf2_ros::Buffer> buffer_;
std::shared_ptr<tf2_ros::TransformListener> listener_;
rclcpp::TimerBase::SharedPtr timer_;
};
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<TFListener>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
关于 TF 监听器如何接收坐标变换的说明（C++ 版）
在 ROS 的 TF 系统中，广播器（Broadcaster） 和 监听器（Listener） 是两个独立的节点，它们之间通过 话题（topic） 通信，不需要在代码中显式传递对象或数据。
广播器节点将坐标变换发布到 /tf（动态变换）或 /tf_static（静态变换）话题。
监听器节点中的 tf2_ros::TransformListener 会自动订阅这两个话题。
每当广播器发布新的变换消息时，TransformListener 就会将其存入 tf2_ros::Buffer 中缓存起来。
您的代码只需要调用 buffer_->lookupTransform() 查询所需的坐标系变换即可，无需关心广播器的存在。
如何获取 base_link 到 bottle_link 的变换？
只需要修改 lookupTransform() 的参数即可：
const auto transform = buffer_->lookupTransform(
"base_link", // 源坐标系
"bottle_link", // 目标坐标系（您要查询的）
this->get_clock()->now(),
rclcpp::Duration::from_seconds(1.0f));
修改CMakeLists.txt文件
新增内容：
add_executable(static_tf_broadcaster src/static_tf_broadcaster.cpp)
ament_target_dependencies(static_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)
add_executable(dynamic_tf_broadcaster src/dynamic_tf_broadcaster.cpp)
ament_target_dependencies(dynamic_tf_broadcaster rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)
add_executable(tf_listener src/tf_listener.cpp)
ament_target_dependencies(tf_listener rclcpp tf2 tf2_ros geometry_msgs tf2_geometry_msgs)
install(TARGETS
tf_listener
static_tf_broadcaster
dynamic_tf_broadcaster
DESTINATION lib/${PROJECT_NAME}
)
终端路径：ros2/chapt5/chapt5_ws/src$
开始构建
colcon build
编译环境
source install/setup.bash
运行打开三个终端分别运行
ros2/chapt5/chapt5_ws/src/下
source install/setup.bash
ros2 run demo_cpp_tf static_tf_broadcaster
source install/setup.bash
ros2 run demo_cpp_tf dynamic_tf_broadcaster
第三个运行查询TF
ros2 run demo_cpp_tf tf_listener
输出
[INFO] [1778747654.889526758] [tf_listener]: 平移分量: (2.598076, -1.500000, 0.000000)
[INFO] [1778747654.889608788] [tf_listener]: 旋转分量: (0.000000, -0.000000, 0.523599)
openEuler 是由开放原子开源基金会孵化的全场景开源操作系统项目，面向数字基础设施四大核心场景（服务器、云计算、边缘计算、嵌入式），全面支持 ARM、x86、RISC-V、loongArch、PowerPC、SW-64 等多样性计算架构
更多推荐
7
0- 0
已为社区贡献1条内容
所有评论(0)