ROS2学习笔记——坐标变换

坐标变换

在ROS 2中，坐标变换（Coordinate Transformation）是通过tf2库实现的核心功能，用于描述机器人不同部件或传感器之间的相对位置和方向关系。

静态广播

对于静态广播器，tf2库已经实现了很好用的工具了，我感觉是没必要再自己去写代码了。

发布静态广播，它的指令是这样的

ros2 run tf2_ros static_transform_publisher [--x X] [--y Y] [--z Z] [--qx QX] [--qy QY] [--qz QZ] [--qw QW] [--roll ROLL] [--pitch PITCH] [--yaw YAW] --frame-id FRAME_ID --child-frame-id CHILD_FRAME_ID

其中，frame-id是父级坐标系，child-frame-id是子级坐标系。x、y、z、分别是坐标的偏移，qx、qy、qz、qw是四元数，用来描述旋转的，roll、pitch、yaw也是用来描述旋转的，不过这个更直观一点，它们分别是绕x轴、y轴、z轴旋转。偏移以米为单位，旋转以弧度为单位。

下面来举个例子，例如我们要发布雷达相对于底盘的坐标关系，雷达在x轴方向偏移1米，在z轴也偏移一米，我们就这样写

ros2 run tf2_ros static_transform_publisher --frame-id base_link --child-frame-id laser --x 1 --z 1

然后，再打开rviz2，把Fixed Frame选择为base_link，然后再添加tf组件，就可以看到坐标变换了，如图

动态广播

传感器的位置关系并非一成不变，例如机械臂就会经常移动，此时我们就需要发布动态广播。

为了方便演示，我会打开一个turtlesim节点，然后用键盘控制小乌龟移动，同时在rviz2中也能看到坐标变换。先来看C++的实现。

先创建功能包

ros2 pkg create tf_cpp --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim

这里的依赖较多，坐标变换是基于话题实现的，发布坐标变换，其实是发布了话题，因此，这里导入geometry_msgs是为了用对应消息接口发布话题，导入turtlesim是为了订阅海龟的位置信息。

我们可以先看一下对应的消息接口,输入以下指令，就能看到。

ros2 interface show geometry_msgs/msg/TransformStamped

# This expresses a transform from coordinate frame header.frame_id
# to the coordinate frame child_frame_id at the time of header.stamp
#
# This message is mostly used by the
# <a href="https://index.ros.org/p/tf2/">tf2</a> package.
# See its documentation for more information.
#
# The child_frame_id is necessary in addition to the frame_id
# in the Header to communicate the full reference for the transform
# in a self contained message.

# The frame id in the header is used as the reference frame of this transform.
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

# The frame id of the child frame to which this transform points.
string child_frame_id

# Translation and rotation in 3-dimensions of child_frame_id from header.frame_id.
Transform transform
        Vector3 translation
                float64 x
                float64 y
                float64 z
        Quaternion rotation
                float64 x 0
                float64 y 0
                float64 z 0
                float64 w 1

在包的src目录下，创建文件pub.cpp，然后输入代码

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "turtlesim/msg/pose.hpp"
using namespace std;

//思路：
//订阅获取乌龟的pose信息
//每订阅一次就发布一次
//发布要用tf2_ros中的广播
class PubTF:public rclcpp::Node{
private:
    //用于发布广播
    shared_ptr<tf2_ros::TransformBroadcaster> tfb;
    //用于订阅海龟的位姿
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr sub;
public:
    PubTF():Node("pubnode"){
        tfb = std::make_shared<tf2_ros::TransformBroadcaster>(this);
        sub = create_subscription<turtlesim::msg::Pose>("/turtle1/pose",
            10,
            [&](turtlesim::msg::Pose::SharedPtr pose){
                geometry_msgs::msg::TransformStamped tf;
                //设置时间戳
                tf.header.stamp = this->now();
                //设置父级坐标id
                tf.header.frame_id = "world";
                //设置子级坐标id
                tf.child_frame_id = "turtle";
                //偏移量
                tf.transform.translation.x = pose->x;
                tf.transform.translation.y = pose->y;
                tf.transform.translation.z = 0.0;
                tf2::Quaternion qtn;
                //传入roll、pitch、yaw，然后会得到对应四元数
                qtn.setRPY(0,0,pose->theta);
                tf.transform.rotation.x  = qtn.x();
                tf.transform.rotation.w  = qtn.w();
                tf.transform.rotation.y  = qtn.y();
                tf.transform.rotation.w  = qtn.z();
                tfb->sendTransform(tf);
        });
    }
};
int main(int argc, char const *argv[])
{
    rclcpp::init(argc,argv);
    auto node = make_shared<PubTF>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

之后就是注册节点，编译运行。

运行这个节点后，我们启动一个小乌龟节点

ros2 run turtlesim turtlesim_node

还可以打开键盘控制，输入

ros2 run turtlesim turtle_teleop_key

此时可以通过键盘控制海归移动，我们再打开rivz2，可以看见里面的坐标也是随着海龟的变化而变化的。

坐标变换监听

如果我们发布了两个坐标关系，分别是摄像头到底盘和雷达到底盘，此时想获取雷达和摄像头的关系，就可以用监听器。

新建sub.cpp文件，在里面输入代码

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "turtlesim/msg/pose.hpp"
using namespace std;
class SubTF:public rclcpp::Node{
private:
    //创建buffer
    shared_ptr<tf2_ros::Buffer> buffer;
    //监听器
    shared_ptr<tf2_ros::TransformListener> listener;
    //创建一个定时器定时发布坐标信息
    rclcpp::TimerBase::SharedPtr timer;
public:
    SubTF():Node("subnode"){
        //监听器会自动把坐标放入buffer中，后续都是用buffer来操作
        //需要传入一个时钟
        buffer = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        //传入buffer和当前节点
        listener = std::make_shared<tf2_ros::TransformListener>(*buffer,this);
        timer = create_wall_timer(1s,[&](){
            try
            {
                //查询坐标变换
                //第一个参数是夫坐标id，第二个参数是子坐标id，在坐标不存在时会抛出异常，以此要捕获一下异常
                geometry_msgs::msg::TransformStamped frame = buffer->lookupTransform("b","c",tf2::TimePoint());
                RCLCPP_INFO(get_logger(),"c相当于b的坐标为：x: %lf,y: %lf,z: %lf",
                frame.transform.translation.x,
                frame.transform.translation.y,
                frame.transform.translation.z
                );
            }
            catch(const tf2::LookupException& e)
            {
                RCLCPP_WARN(get_logger(),"出现异常%s",e.what());
            }
            
        });
    }
};
int main(int argc, char const *argv[])
{
    rclcpp::init(argc,argv);
    auto node = make_shared<SubTF>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

现在来创建坐标，这里就用静态广播了。

ros2 run tf2_ros static_transform_publisher --frame-id a --child-frame-id b --x -1
ros2 run tf2_ros static_transform_publisher --frame-id a --child-frame-id c --x 1 --z 1

然后就是注册节点，编译运行，运行截图如下

还可以用rqt来查看坐标树

Python版本的代码

Python的代码也和C++版本的高度相似，先创建功能包，然后我直接给出代码

ros2 pkg create tf_py --build-type ament_python --dependencies rclpy tf_ros turtlesim geometry_msgs tf_transformations

这里和C++不同的是tf2换成了tf_transformations，如果你没有这个库，你可能需要下载一下

sudo pip3 install transforms3d
sudo apt install ros-$ROS_DISTRO-tf-transformations

发布动态广播的代码

import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler

class PubTF(Node):
    def __init__(self):
        super().__init__("pubnode")
        self.tf = TransformBroadcaster(self)
        self.subscription = self.create_subscription(Pose,"turtle1/pose",self.sub_callback,10)
    def sub_callback(self,pose:Pose):
        msg = TransformStamped()
        msg.header.frame_id = 'world'
        msg.child_frame_id = 'turtle'
        msg.transform.translation.x = pose.x
        msg.transform.translation.y = pose.y
        msg.transform.translation.z = 0.0
        #欧拉角转四元数
        qtn = quaternion_from_euler(0,0,pose.theta)
        msg.transform.rotation.x = qtn[0]
        msg.transform.rotation.y = qtn[1]
        msg.transform.rotation.z = qtn[2]
        msg.transform.rotation.w = qtn[3]
        self.tf.sendTransform(msg)
def main():
    rclpy.init()
    node = PubTF()
    rclpy.spin(node)
    rclpy.shutdown()

监听的代码

import rclpy
import rclpy.logging
from rclpy.node import Node
import rclpy.time
from tf2_ros import TransformListener
from tf2_ros import Buffer
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler
from rclpy.time import Time
class SubTF(Node):
    def __init__(self):
        super().__init__('subnode')
        self.buffer = Buffer()
        self.listenet = TransformListener(self.buffer,self)
        self.timer = self.create_timer(1,self.timer_callback)
    def timer_callback(self):
        if self.buffer.can_transform('b','c',Time()):
            frame = self.buffer.lookup_transform('b','c',Time())
            self.get_logger().info("c相对b的坐标为: x: %lf,y: %lf,z : %lf" %
                                   (frame.transform.translation.x,
                                   frame.transform.translation.y,
                                   frame.transform.translation.z))
        else:
            self.get_logger().info("异常...")
def main():
    rclpy.init()
    node = SubTF()
    rclpy.spin(node)
    rclpy.shutdown()