ros2 tf2详解
tf2工具
ros2 run tf2_tools view_frames使用tf2_echo
ros2 run tf2_ros tf2_echo [source_frame] [target_frame]静态坐标变换
静态坐标变换用于定义机器人基座与传感器或固定部件之间的相对关系，这在机器人系统中非常实用。例如，激光雷达的扫描数据在其自身坐标系下处理会更直观，而静态坐标变换能明确该坐标系与机器人基座坐标系的关联。
Python
import math
import sysfrom geometry_msgs.msg import TransformStampedimport numpy as npimport rclpy
from rclpy.node import Nodefrom tf2_ros.static_transform_broadcaster import StaticTransformBroadcasterdef quaternion_from_euler(ai, aj, ak):ai /= 2.0aj /= 2.0ak /= 2.0ci = math.cos(ai)si = math.sin(ai)cj = math.cos(aj)sj = math.sin(aj)ck = math.cos(ak)sk = math.sin(ak)cc = ci*ckcs = ci*sksc = si*ckss = si*skq = np.empty((4, ))q[0] = cj*sc - sj*csq[1] = cj*ss + sj*ccq[2] = cj*cs - sj*scq[3] = cj*cc + sj*ssreturn qclass StaticFramePublisher(Node):"""Broadcast transforms that never change.This example publishes transforms from `world` to a static turtle frame.The transforms are only published once at startup, and are constant for alltime."""def __init__(self, transformation):super().__init__('static_turtle_tf2_broadcaster')self.tf_static_broadcaster = StaticTransformBroadcaster(self)# Publish static transforms once at startupself.make_transforms(transformation)def make_transforms(self, transformation):t = TransformStamped()t.header.stamp = self.get_clock().now().to_msg()t.header.frame_id = 'world't.child_frame_id = transformation[1]t.transform.translation.x = float(transformation[2])t.transform.translation.y = float(transformation[3])t.transform.translation.z = float(transformation[4])quat = quaternion_from_euler(float(transformation[5]), float(transformation[6]), float(transformation[7]))t.transform.rotation.x = quat[0]t.transform.rotation.y = quat[1]t.transform.rotation.z = quat[2]t.transform.rotation.w = quat[3]self.tf_static_broadcaster.sendTransform(t)def main():logger = rclpy.logging.get_logger('logger')# obtain parameters from command line argumentsif len(sys.argv) != 8:logger.info('Invalid number of parameters. Usage: \n''$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster''child_frame_name x y z roll pitch yaw')sys.exit(1)if sys.argv[1] == 'world':logger.info('Your static turtle name cannot be "world"')sys.exit(2)# pass parameters and initialize noderclpy.init()node = StaticFramePublisher(sys.argv)try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()这段代码是一个 ROS 2 节点，用于广播静态坐标变换（TF 变换）。它的主要功能是将一个子坐标系相对于父坐标系（这里是 "world" 坐标系）的变换关系发布出去，供系统中的其他节点使用。
解释：
主函数先检查命令行参数个数是不是8个，然后检查子坐标系名字是否与父坐标系重名
package.xml
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf2_ros_py</exec_depend>
<exec_depend>turtlesim</exec_depend>setup.py
'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',运行
ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0其他方式发布静态坐标系
1.
ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --yaw 0 --pitch 0 --roll 0 --frame-id world --child-frame-id mystaticturtle2.
ros2 run tf2_ros static_transform_publisher --x 0 --y 0 --z 1 --qx 0 --qy 0 --qz 0 --qw 1 --frame-id world --child-frame-id mystaticturtle3.
from launch import LaunchDescription
from launch_ros.actions import Nodedef generate_launch_description():return LaunchDescription([Node(package='tf2_ros',executable='static_transform_publisher',arguments=['--x', '0', '--y', '0', '--z', '1','--yaw', '0', '--pitch', '0', '--roll','0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']),])C++
推荐使用命令行和launch文件来发送（见目录 静态坐标变换/Python/其他方式发布静态坐标系）
动态坐标变换
Python
import mathfrom geometry_msgs.msg import TransformStampedimport numpy as npimport rclpy
from rclpy.node import Nodefrom tf2_ros import TransformBroadcasterfrom turtlesim.msg import Posedef quaternion_from_euler(ai, aj, ak):ai /= 2.0aj /= 2.0ak /= 2.0ci = math.cos(ai)si = math.sin(ai)cj = math.cos(aj)sj = math.sin(aj)ck = math.cos(ak)sk = math.sin(ak)cc = ci*ckcs = ci*sksc = si*ckss = si*skq = np.empty((4, ))q[0] = cj*sc - sj*csq[1] = cj*ss + sj*ccq[2] = cj*cs - sj*scq[3] = cj*cc + sj*ssreturn qclass FramePublisher(Node):def __init__(self):super().__init__('turtle_tf2_frame_publisher')# Declare and acquire `turtlename` parameterself.turtlename = self.declare_parameter('turtlename', 'turtle').get_parameter_value().string_value# Initialize the transform broadcasterself.tf_broadcaster = TransformBroadcaster(self)# Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose# callback function on each messageself.subscription = self.create_subscription(Pose,f'/{self.turtlename}/pose',self.handle_turtle_pose,1)self.subscription # prevent unused variable warningdef handle_turtle_pose(self, msg):t = TransformStamped()# Read message content and assign it to# corresponding tf variablest.header.stamp = self.get_clock().now().to_msg()t.header.frame_id = 'world't.child_frame_id = self.turtlename# Turtle only exists in 2D, thus we get x and y translation# coordinates from the message and set the z coordinate to 0t.transform.translation.x = msg.xt.transform.translation.y = msg.yt.transform.translation.z = 0.0# For the same reason, turtle can only rotate around one axis# and this why we set rotation in x and y to 0 and obtain# rotation in z axis from the messageq = quaternion_from_euler(0, 0, msg.theta)t.transform.rotation.x = q[0]t.transform.rotation.y = q[1]t.transform.rotation.z = q[2]t.transform.rotation.w = q[3]# Send the transformationself.tf_broadcaster.sendTransform(t)def main():rclpy.init()node = FramePublisher()try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()这段代码是一个 ROS 2 节点，用于发布动态坐标系变换，特别是针对 turtlesim 仿真中的海龟位置和姿态。与之前的静态坐标系发布器不同，这个节点会实时跟踪海龟的运动并更新其坐标系变换。
setup.py
data_files=[...(os.path.join('share', package_name, 'launch'), glob('launch/*')),
],
'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',launch
from launch import LaunchDescription
from launch_ros.actions import Nodedef generate_launch_description():return LaunchDescription([Node(package='turtlesim',executable='turtlesim_node',name='sim'),Node(package='learning_tf2_py',executable='turtle_tf2_broadcaster',name='broadcaster1',parameters=[{'turtlename': 'turtle1'}]),])package.xml
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>C++
#include <functional>
#include <memory>
#include <sstream>
#include <string>#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/msg/pose.hpp"class FramePublisher : public rclcpp::Node
{
public:FramePublisher(): Node("turtle_tf2_frame_publisher"){// Declare and acquire `turtlename` parameterturtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");// Initialize the transform broadcastertf_broadcaster_ =std::make_unique<tf2_ros::TransformBroadcaster>(*this);// Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose// callback function on each messagestd::ostringstream stream;stream << "/" << turtlename_.c_str() << "/pose";std::string topic_name = stream.str();subscription_ = this->create_subscription<turtlesim::msg::Pose>(topic_name, 10,std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1));}private:void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg){geometry_msgs::msg::TransformStamped t;// Read message content and assign it to// corresponding tf variablest.header.stamp = this->get_clock()->now();t.header.frame_id = "world";t.child_frame_id = turtlename_.c_str();// Turtle only exists in 2D, thus we get x and y translation// coordinates from the message and set the z coordinate to 0t.transform.translation.x = msg->x;t.transform.translation.y = msg->y;t.transform.translation.z = 0.0;// For the same reason, turtle can only rotate around one axis// and this why we set rotation in x and y to 0 and obtain// rotation in z axis from the messagetf2::Quaternion q;q.setRPY(0, 0, msg->theta);t.transform.rotation.x = q.x();t.transform.rotation.y = q.y();t.transform.rotation.z = q.z();t.transform.rotation.w = q.w();// Send the transformationtf_broadcaster_->sendTransform(t);}rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;std::string turtlename_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<FramePublisher>());rclcpp::shutdown();return 0;
}CMakeList
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(turtle_tf2_broadcastergeometry_msgsrclcpptf2tf2_rosturtlesim
)
install(TARGETSturtle_tf2_broadcasterDESTINATION lib/${PROJECT_NAME})launch
from launch import LaunchDescription
from launch_ros.actions import NodeNode(package='turtlesim',executable='turtlesim_node',name='sim'),Node(package='learning_tf2_cpp',executable='turtle_tf2_broadcaster',name='broadcaster1',parameters=[{'turtlename': 'turtle1'}]),package
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>CMakeList
install(DIRECTORY launchDESTINATION share/${PROJECT_NAME})
wrighting a listener
待定
using time
核心内容：将教授如何获取特定时间点的变换。这一功能在很多场景下非常有用，比如需要分析过去某个时刻的系统状态、同步不同传感器在同一时间点的数据等。
t = tf_buffer_->lookupTransform(toFrameRel,fromFrameRel,tf2::TimePointZero);具体作用是获取两个坐标系在 “最早可用时间点” 的变换关系。
rclcpp::Time now = this->get_clock()->now();
t = tf_buffer_->lookupTransform(toFrameRel,fromFrameRel,now);这段代码的功能是：获取 “当前时刻” 下，从 fromFrameRel 到 toFrameRel 的坐标变换关系。
Traveling in time
rclcpp::Time when = this->get_clock()->now() - rclcpp::Duration(5, 0);
t = tf_buffer_->lookupTransform(toFrameRel,fromFrameRel,when,50ms);这段代码是 ROS 2 中使用 tf2 库查询过去特定时间点的坐标系变换，并设置了超时等待机制，具体解释如下：
1. this->get_clock()->now() 获取当前时间 ,rclcpp::Duration(5, 0) 表示 5 秒的时间间隔。两者相减得到 “5 秒前” 的时间点，存储在 when 变量中。
- 这是 tf2 查询变换的扩展用法，四个参数的含义：
toFrameRel：目标坐标系（变换终点）fromFrameRel：源坐标系（变换起点）when：要查询的时间点（这里是 5 秒前）50ms：超时等待时间（50 毫秒）
rclcpp::Time now = this->get_clock()->now();
rclcpp::Time when = now - rclcpp::Duration(5, 0);
t = tf_buffer_->lookupTransform(toFrameRel,now,fromFrameRel,when,"world",50ms);这段代码是 ROS 2 中 tf2 库的一种更灵活的坐标变换查询方式，允许为源坐标系和目标坐标系指定不同的时间点，并指定一个参考坐标系。