[ROS2基础] TF2使用细节

运行一个示例

安装依赖

sudo apt-get install ros-galactic-turtle-tf2-py ros-galactic-tf2-tools ros-galactic-tf-transformations

pip3 install transforms3d

运行示例

在不同的命令窗口中运行下面的命令

启动小乌龟窗口

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

启动键盘控制节点

ros2 run turtlesim turtle_teleop_key

观察坐标转换的结果

ros2 run tf2_ros tf2_echo turtle2 turtle1

示例分析

本示例中启动了两只小乌龟Turtle1和Turtle2。TF发布器会将Turtle1相对于world坐标系的位置关系和Turtle2相对于world坐标系的位置关系发布出来。为了实现Turtle2跟随Turtle1的效果，程序中获取了Turtle1相对于Turtle2的位置关系并且将其折算成速度控制量。

        // Look up for the transformation between target_frame and turtle2 frames// and send velocity commands for turtle2 to reach target_frametry {transformStamped = tf_buffer_->lookupTransform(toFrameRel, fromFrameRel,tf2::TimePointZero);//这里得到的是from_frame_rel在to_frame_rel坐标系下的相对位置} catch (tf2::TransformException & ex) {RCLCPP_INFO(this->get_logger(), "Could not transform %s to %s: %s",toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());return;}

代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener.cpp。

上面的示例代码可通过下面的方式获取：

git clone https://gitee.com/shoufei/ros2_galactic_turorials.git

或者

git clone git@github.com:shoufei403/ros2_galactic_tutorials.git

TF2树

tf2树表征了各个link的位置关系。一个link有且只有一个parent link。所以tf2树是不会形成闭环的。

ros2 run tf2_tools view_frames

用这个命令可以保存当前系统中tf2树的关系图（以pdf文件的形式保存在运行命令的目录下）。

静态TF发布器和动态TF发布器

静态TF发布器

编写代码发布TF（C++）

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

#include <geometry_msgs/msg/transform_stamped.hpp>#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/static_transform_broadcaster.h>#include <memory>using std::placeholders::_1;class StaticFramePublisher : public rclcpp::Node
{
public:explicit StaticFramePublisher(char * transformation[]): Node("static_turtle_tf2_broadcaster"){tf_publisher_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);// Publish static transforms once at startupthis->make_transforms(transformation);}private:void make_transforms(char * transformation[]){rclcpp::Time now = this->get_clock()->now();geometry_msgs::msg::TransformStamped t;t.header.stamp = now;t.header.frame_id = "world";t.child_frame_id = transformation[1];t.transform.translation.x = atof(transformation[2]);t.transform.translation.y = atof(transformation[3]);t.transform.translation.z = atof(transformation[4]);tf2::Quaternion q;q.setRPY(atof(transformation[5]),atof(transformation[6]),atof(transformation[7]));t.transform.rotation.x = q.x();t.transform.rotation.y = q.y();t.transform.rotation.z = q.z();t.transform.rotation.w = q.w();tf_publisher_->sendTransform(t);}std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_publisher_;
};int main(int argc, char * argv[])
{auto logger = rclcpp::get_logger("logger");// Obtain parameters from command line argumentsif (argc != 8) {RCLCPP_INFO(logger, "Invalid number of parameters\nusage: ""ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster ""child_frame_name x y z roll pitch yaw");return 1;}// As the parent frame of the transform is `world`, it is// necessary to check that the frame name passed is differentif (strcmp(argv[1], "world") == 0) {RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");return 1;}// Pass parameters and initialize noderclcpp::init(argc, argv);rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));rclcpp::shutdown();return 0;
}

编写代码代码发布TF（Python）

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

from geometry_msgs.msg import TransformStampedimport rclpy
from rclpy.node import Nodefrom tf2_ros import TransformBroadcasterimport tf_transformationsfrom turtlesim.msg import Poseclass FramePublisher(Node):def __init__(self):super().__init__('turtle_tf2_frame_publisher')# Declare and acquire `turtlename` parameterself.declare_parameter('turtlename', 'turtle')self.turtlename = self.get_parameter('turtlename').get_parameter_value().string_value# Initialize the transform broadcasterself.br = 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)def 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 = tf_transformations.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.br.sendTransform(t)def main():rclpy.init()node = FramePublisher()try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()

手动以命令行形式发布TF

角度用欧拉角表示

x/y/z 偏移的单位是米，roll/pitch/yaw偏移的单位是rad。

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and roll/pitch/yaw in radians. In our case, roll/pitch/yaw refers to rotation about the x/y/z-axis, respectively.

ros2 run tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

角度用四元数表示

Publish a static coordinate transform to tf2 using an x/y/z offset in meters and quaternion.

ros2 run tf2_ros static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

在launch文件中发布TF

from launch import LaunchDescription
from launch_ros.actions import Nodedef generate_launch_description():return LaunchDescription([Node(package='tf2_ros',executable='static_transform_publisher',arguments = ['0', '0', '1', '0', '0', '0', 'world', 'mystaticturtle']),])

动态TF发布器

动态的tf发布一般是编写代码来实现的。

C++实现动态TF发布

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_broadcaster.cpp

#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>#include <memory>
#include <string>using std::placeholders::_1;class FramePublisher : public rclcpp::Node
{
public:FramePublisher(): Node("turtle_tf2_frame_publisher"){// Declare and acquire `turtlename` parameterthis->declare_parameter<std::string>("turtlename", "turtle");this->get_parameter("turtlename", turtlename_);// 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, _1));}private:void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg){rclcpp::Time now = this->get_clock()->now();geometry_msgs::msg::TransformStamped t;// Read message content and assign it to// corresponding tf variablest.header.stamp = 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;
}

Python实现动态TF发布

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

from geometry_msgs.msg import TransformStampedimport rclpy
from rclpy.node import Nodefrom tf2_ros import TransformBroadcasterimport tf_transformationsfrom turtlesim.msg import Poseclass FramePublisher(Node):def __init__(self):super().__init__('turtle_tf2_frame_publisher')# Declare and acquire `turtlename` parameterself.declare_parameter('turtlename', 'turtle')self.turtlename = self.get_parameter('turtlename').get_parameter_value().string_value# Initialize the transform broadcasterself.br = 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)def 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 = tf_transformations.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.br.sendTransform(t)def main():rclpy.init()node = FramePublisher()try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()

监听TF数据

实际上，就是是获取两个坐标系之间的相对位置。

C++版本

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener.cpp

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>#include <rclcpp/rclcpp.hpp>
#include <tf2/exceptions.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <turtlesim/srv/spawn.hpp>#include <chrono>
#include <memory>
#include <string>using std::placeholders::_1;
using namespace std::chrono_literals;class FrameListener : public rclcpp::Node
{
public:FrameListener(): Node("turtle_tf2_frame_listener"),turtle_spawning_service_ready_(false),turtle_spawned_(false){// Declare and acquire `target_frame` parameterthis->declare_parameter<std::string>("target_frame", "turtle1");this->get_parameter("target_frame", target_frame_);tf_buffer_ =std::make_unique<tf2_ros::Buffer>(this->get_clock());transform_listener_ =std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);// Create a client to spawn a turtlespawner_ =this->create_client<turtlesim::srv::Spawn>("spawn");// Create turtle2 velocity publisherpublisher_ =this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);// Call on_timer function every secondtimer_ = this->create_wall_timer(1s, std::bind(&FrameListener::on_timer, this));}private:void on_timer(){// Store frame names in variables that will be used to// compute transformationsstd::string fromFrameRel = target_frame_.c_str();std::string toFrameRel = "turtle2";if (turtle_spawning_service_ready_) {if (turtle_spawned_) {geometry_msgs::msg::TransformStamped transformStamped;// Look up for the transformation between target_frame and turtle2 frames// and send velocity commands for turtle2 to reach target_frametry {transformStamped = tf_buffer_->lookupTransform(toFrameRel, fromFrameRel,tf2::TimePointZero);//这里得到的是from_frame_rel在to_frame_rel坐标系下的位置} catch (tf2::TransformException & ex) {RCLCPP_INFO(this->get_logger(), "Could not transform %s to %s: %s",toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());return;}geometry_msgs::msg::Twist msg;static const double scaleRotationRate = 1.0;msg.angular.z = scaleRotationRate * atan2(transformStamped.transform.translation.y,transformStamped.transform.translation.x);static const double scaleForwardSpeed = 0.5;msg.linear.x = scaleForwardSpeed * sqrt(pow(transformStamped.transform.translation.x, 2) +pow(transformStamped.transform.translation.y, 2));publisher_->publish(msg);} else {RCLCPP_INFO(this->get_logger(), "Successfully spawned");turtle_spawned_ = true;}} else {// Check if the service is readyif (spawner_->service_is_ready()) {// Initialize request with turtle name and coordinates// Note that x, y and theta are defined as floats in turtlesim/srv/Spawnauto request = std::make_shared<turtlesim::srv::Spawn::Request>();request->x = 4.0;request->y = 2.0;request->theta = 0.0;request->name = "turtle2";// Call requestusing ServiceResponseFuture =rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;auto response_received_callback = [this](ServiceResponseFuture future) {auto result = future.get();if (strcmp(result->name.c_str(), "turtle2") == 0) {turtle_spawning_service_ready_ = true;} else {RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");}};auto result = spawner_->async_send_request(request, response_received_callback);} else {RCLCPP_INFO(this->get_logger(), "Service is not ready");}}}// Boolean values to store the information// if the service for spawning turtle is availablebool turtle_spawning_service_ready_;// if the turtle was successfully spawnedbool turtle_spawned_;rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};rclcpp::TimerBase::SharedPtr timer_{nullptr};rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};std::shared_ptr<tf2_ros::TransformListener> transform_listener_{nullptr};std::unique_ptr<tf2_ros::Buffer> tf_buffer_;std::string target_frame_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<FrameListener>());rclcpp::shutdown();return 0;
}

增加超时时间的写法

        try {rclcpp::Time now = this->get_clock()->now();transformStamped = tf_buffer_->lookupTransform(toFrameRel,fromFrameRel,now,50ms);//获取当前时间的tf关系并且允许50ms的超时时间} catch (tf2::TransformException & ex) {RCLCPP_INFO(this->get_logger(), "Could not transform %s to %s: %s",toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());return;}

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener_timeout.cpp

可运行下面的命令查看效果

ros2 launch turtle_tf2_cpp turtle_tf2_demo_timeout.launch.py

ros2 run turtlesim turtle_teleop_key

获取历史时间点的tf关系

        try {rclcpp::Time now = this->get_clock()->now();rclcpp::Time when = now - rclcpp::Duration(5, 0);transformStamped = tf_buffer_->lookupTransform(toFrameRel,now,fromFrameRel,when,"world",50ms);} catch (tf2::TransformException & ex) {RCLCPP_INFO(this->get_logger(), "Could not transform %s to %s: %s",toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());return;}

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_cpp/src/turtle_tf2_listener_time_travel.cpp

运行命令

ros2 launch turtle_tf2_cpp turtle_tf2_demo_time_travel.launch.py

ros2 run turtlesim turtle_teleop_key

python版本

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py

import mathfrom geometry_msgs.msg import Twistimport rclpy
from rclpy.node import Nodefrom tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListenerfrom turtlesim.srv import Spawnclass FrameListener(Node):def __init__(self):super().__init__('turtle_tf2_frame_listener')# Declare and acquire `target_frame` parameterself.declare_parameter('target_frame', 'turtle1')self.target_frame = self.get_parameter('target_frame').get_parameter_value().string_valueself.tf_buffer = Buffer()self.tf_listener = TransformListener(self.tf_buffer, self)# Create a client to spawn a turtleself.spawner = self.create_client(Spawn, 'spawn')# Boolean values to store the information# if the service for spawning turtle is availableself.turtle_spawning_service_ready = False# if the turtle was successfully spawnedself.turtle_spawned = False# Create turtle2 velocity publisherself.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)# Call on_timer function every secondself.timer = self.create_timer(1.0, self.on_timer)def on_timer(self):# Store frame names in variables that will be used to# compute transformationsfrom_frame_rel = self.target_frameto_frame_rel = 'turtle2'if self.turtle_spawning_service_ready:if self.turtle_spawned:# Look up for the transformation between target_frame and turtle2 frames# and send velocity commands for turtle2 to reach target_frametry:now = rclpy.time.Time()trans = self.tf_buffer.lookup_transform(to_frame_rel,from_frame_rel,now)  #这里得到的是from_frame_rel在to_frame_rel坐标系下的位置except TransformException as ex:self.get_logger().info(f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')returnmsg = Twist()scale_rotation_rate = 1.0msg.angular.z = scale_rotation_rate * math.atan2(trans.transform.translation.y,trans.transform.translation.x)scale_forward_speed = 0.5msg.linear.x = scale_forward_speed * math.sqrt(trans.transform.translation.x ** 2 +trans.transform.translation.y ** 2)self.publisher.publish(msg)else:if self.result.done():self.get_logger().info(f'Successfully spawned {self.result.result().name}')self.turtle_spawned = Trueelse:self.get_logger().info('Spawn is not finished')else:if self.spawner.service_is_ready():# Initialize request with turtle name and coordinates# Note that x, y and theta are defined as floats in turtlesim/srv/Spawnrequest = Spawn.Request()request.name = 'turtle2'request.x = float(4)request.y = float(2)request.theta = float(0)# Call requestself.result = self.spawner.call_async(request)self.turtle_spawning_service_ready = Trueelse:# Check if the service is readyself.get_logger().info('Service is not ready')def main():rclpy.init()node = FrameListener()try:rclpy.spin(node)except KeyboardInterrupt:passrclpy.shutdown()

增加超时时间的写法

trans = self._tf_buffer.lookup_transform(to_frame_rel,from_frame_rel,now,timeout=rclpy.time.Duration(seconds=1.0))

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener_timeout.py

运行命令

ros2 launch turtle_tf2_py turtle_tf2_demo_timeout.launch.py

ros2 run turtlesim turtle_teleop_key

获取历史时间点的tf关系

                try:when = self.get_clock().now() - rclpy.time.Duration(seconds=5.0)trans = self.tf_buffer.lookup_transform_full(target_frame=to_frame_rel,target_time=rclpy.time.Time(),source_frame=from_frame_rel,source_time=when,fixed_frame='world',timeout=rclpy.time.Duration(seconds=0.05))except (LookupException, ConnectivityException, ExtrapolationException):self.get_logger().info('transform not ready')return

示例代码来自ros2_galactic_turorials/geometry_tutorials/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener_time_travel.py

运行命令

ros2 launch turtle_tf2_py turtle_tf2_demo_time_travel.launch.py

ros2 run turtlesim turtle_teleop_key

TF 调试工具

打印两个link的相对位置关系

ros2 run tf2_ros tf2_echo turtle2 turtle1

保存tf关系框图

ros2 run tf2_tools view_frames

监控两个link的转换延时

ros2 run tf2_ros tf2_monitor turtle2 turtle1

四元数与欧拉角转换

因为使用TF的过程中常常涉及到坐标转换。这会涉及到角度的表示方法。

通常我们会有两种表示角度的方法，一种是欧拉角，一种是四元数。欧拉角是更为直观的表示方法，但因为死锁的问题，在进行坐标变换计算时常常是使用四元数的。

在ROS2中，有两个数据类型表示四元数。它们是来自tf2功能包的tf2::Quaternion类型和来自geometry_msgs功能包的geometry_msgs::msg::Quaternion类型。我们可以通过tf2_geometry_msgs包来进行两个类型的互相转换。

C++的写法

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
...tf2::Quaternion tf2_quat, tf2_quat_from_msg;
tf2_quat.setRPY(roll, pitch, yaw);
// Convert tf2::Quaternion to geometry_msgs::msg::Quaternion
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(tf2_quat);// Convert geometry_msgs::msg::Quaternion to tf2::Quaternion
tf2::convert(msg_quat, tf2_quat_from_msg);
// or
tf2::fromMsg(msg_quat, tf2_quat_from_msg);

Python的写法

from geometry_msgs.msg import Quaternion
...# Create a list of floats, which is compatible with tf2
# Quaternion methods
quat_tf = [0.0, 1.0, 0.0, 0.0]# Convert a list to geometry_msgs.msg.Quaternion
msg_quat = Quaternion(x=quat_tf[0], y=quat_tf[1], z=quat_tf[2], w=quat_tf[3])

欧拉角和四元数之间的转换可以用下面的方式

欧拉角转四元数

C++版本

#include <tf2/LinearMath/Quaternion.h>tf2::Quaternion myQuaternion;
myQuaternion.setRPY(1.5707, 0, -1.5707);  // Create this quaternion from roll/pitch/yaw (in radians)
ROS_INFO("%f  %f  %f  %f" ,myQuaternion.x(),myQuaternion.y(),myQuaternion.z(),myQuaternion.w());  // Print the quaternion components (0,0,0,1)

Python版本

import tf_transformations
...q = tf_transformations.quaternion_from_euler(1.5707, 0, -1.5707)
print(f'The quaternion representation is x: {q[0]} y: {q[1]} z: {q[2]} w: {q[3]}.')

四元数转欧拉角

C++版本

在tf2/utils.h中定义了如下方法。

/** Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion* The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h* \param a the object to get data from (it represents a rotation/quaternion)* \param yaw yaw* \param pitch pitch* \param roll roll*/
template<class A>
void getEulerYPR(const A & a, double & yaw, double & pitch, double & roll)
{tf2::Quaternion q = impl::toQuaternion(a);impl::getEulerYPR(q, yaw, pitch, roll);
}/** Return the yaw of anything that can be converted to a tf2::Quaternion* The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h* This function is a specialization of getEulerYPR and is useful for its* wide-spread use in navigation* \param a the object to get data from (it represents a rotation/quaternion)* \param yaw yaw*/
template<class A>
double getYaw(const A & a)
{tf2::Quaternion q = impl::toQuaternion(a);return impl::getYaw(q);
}

Python版本

import tf_transformationsrpy = tf_transformations.euler_from_quaternion([0.06146124, 0, 0, 0.99810947])

更多细节请查看下面的文章：
https://docs.ros.org/en/galactic/Tutorials/Tf2/Quaternion-Fundamentals.html

tf2_ros::MessageFilter

https://docs.ros.org/en/galactic/Tutorials/Tf2/Using-Stamped-Datatypes-With-Tf2-Ros-MessageFilter.html

To do this turtle1 must listen to the topic where turtle3’s pose is being published, wait until transforms into the desired frame are ready, and then do its operations. To make this easier the tf2_ros::MessageFilter is very useful. The tf2_ros::MessageFilter will take a subscription to any ROS 2 message with a header and cache it until it is possible to transform it into the target frame.

tf2_ros::MessageFilter 的主要作用是，等待目标frame已经缓冲在tf树中。这样的话转换到该frame将是可行的。AMCL模块中处理激光数据就是一个好的例子。

void
AmclNode::initMessageFilters()
{laser_scan_sub_ = std::make_unique<message_filters::Subscriber<sensor_msgs::msg::LaserScan>>(rclcpp_node_.get(), scan_topic_, rmw_qos_profile_sensor_data);laser_scan_filter_ = std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>>(*laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_);laser_scan_connection_ = laser_scan_filter_->registerCallback(std::bind(&AmclNode::laserReceived,this, std::placeholders::_1));
}

当odom_frame_id_在tf树中可转换时，接受到激光数据后开始执行回调函数laserReceived 。在laserReceived 中通过odom_frame_id_ 获取到当前的odom 数据。也就是说，使用tf2_ros::MessageFilter 达到了，等能获取odom 数据时再处理激光数据的效果。

下面是官方的一个示例。可以参考其写法。

#include <geometry_msgs/msg/point_stamped.hpp>
#include <message_filters/subscriber.h>#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/create_timer_ros.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>
#ifdef TF2_CPP_HEADERS#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif#include <chrono>
#include <memory>
#include <string>using namespace std::chrono_literals;class PoseDrawer : public rclcpp::Node
{
public:PoseDrawer(): Node("turtle_tf2_pose_drawer"){// Declare and acquire `target_frame` parameterthis->declare_parameter<std::string>("target_frame", "turtle1");this->get_parameter("target_frame", target_frame_);typedef std::chrono::duration<int> seconds_type;seconds_type buffer_timeout(1);tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());// Create the timer interface before call to waitForTransform,// to avoid a tf2_ros::CreateTimerInterfaceException exceptionauto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(this->get_node_base_interface(),this->get_node_timers_interface());tf2_buffer_->setCreateTimerInterface(timer_interface);tf2_listener_ =std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),this->get_node_clock_interface(), buffer_timeout);// Register a callback with tf2_ros::MessageFilter to be called when transforms are availabletf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);//等待需要的tf关系就绪，一就绪有数据就进入到回调函数中。}private:void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr){geometry_msgs::msg::PointStamped point_out;try {tf2_buffer_->transform(*point_ptr, point_out, target_frame_);//point_out的值是turtle3想对于target_frame_的坐标RCLCPP_INFO(this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",point_out.point.x,point_out.point.y,point_out.point.z);} catch (tf2::TransformException & ex) {RCLCPP_WARN(// Print exception which was caughtthis->get_logger(), "Failure %s\n", ex.what());}}std::string target_frame_;std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
};int main(int argc, char * argv[])
{rclcpp::init(argc, argv);rclcpp::spin(std::make_shared<PoseDrawer>());rclcpp::shutdown();return 0;
}

参考：

https://docs.ros.org/en/galactic/Tutorials/Tf2/Tf2-Main.html

觉得有用就点赞吧！

我是首飞，一个帮大家填坑的机器人开发攻城狮。

另外在公众号《首飞》内回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2等机器人行业常用技术资料。