ROS2 TF 学习及实现(python & C++)

于 2025-07-26 23:53:30 发布
阅读量1.2k 收藏 20
点赞数 41
CC 4.0 BY-SA版权
分类专栏： ROS c++ python 文章标签： c++ 机器人 python
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/small_jimmy/article/details/149674486

ROS2  TF 学习及实现

1. 什么是 tf？

1.1 tf 的作用

  • tf 是 ROS 中用于 坐标变换 的工具包，可以让不同传感器、机器人部件之间的坐标系保持一致。
  • 它的主要任务是：
    • 发布不同坐标系之间的 变换关系（Translation + Rotation）。
    • 订阅这些关系，并进行坐标点之间的变换。

1.2 tf2 在 ROS2 中的更新

  • ROS1 中使用 tf 包。
  • ROS2 中使用 tf2 系列包（tf2_ros, tf2_geometry_msgs 等）。
  • 主要改进：
    • 更高效的内存管理。
    • 更清晰的 API。

2. 主要概念

2.1 坐标系（Frame）

  • 每个物体 都有一个坐标系，例如：
    • map
    • odom
    • base_link
    • camera_link
  • 坐标系之间通过变换关系连接成 树结构（tf tree）。

2.2 变换（Transform）

  • 包含两部分：
    1. 平移（translation）：x, y, z
    2. 旋转（rotation）：四元数（x, y, z, w）

2.3 Broadcaster 和 Listener

  • Broadcaster：发布坐标系之间的变换。
  • Listener：订阅变换，并计算两坐标系之间的关系。

3. 使用前准备

3.1 安装依赖

sudo apt install ros-humble-tf2-ros ros-humble-tf2-geometry-msgs

3.2 必要包导入

  • Python: 
    import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
import tf2_ros
  • C++: 
    #include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

4. Python 实现

4.1 Transform Broadcaster（发布坐标变换）

代码示例

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
import tf2_ros
from math import sin, cos
import time

class FrameBroadcaster(Node):
    def __init__(self):
        super().__init__('frame_broadcaster')

        # 创建tf广播器
        self.broadcaster = tf2_ros.TransformBroadcaster(self)

        # 定时发布变换
        self.timer = self.create_timer(0.1, self.broadcast_timer_callback)  # 10Hz

    def broadcast_timer_callback(self):
        t = TransformStamped()

        # 时间戳
        t.header.stamp = self.get_clock().now().to_msg()

        # 父子坐标系名称
        t.header.frame_id = 'world'
        t.child_frame_id = 'robot'

        # 平移 (随时间变化模拟运动)
        t.transform.translation.x = 1.0 * cos(time.time())
        t.transform.translation.y = 1.0 * sin(time.time())
        t.transform.translation.z = 0.0

        # 旋转 (四元数)
        t.transform.rotation.x = 0.0
        t.transform.rotation.y = 0.0
        t.transform.rotation.z = 0.0
        t.transform.rotation.w = 1.0

        # 发布
        self.broadcaster.sendTransform(t)


def main(args=None):
    rclpy.init(args=args)
    node = FrameBroadcaster()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

说明

  • TransformStamped 用于描述父子坐标系的变换。
  • frame_id 为父坐标系，child_frame_id 为子坐标系。
  • 广播频率 10Hz。

4.2 Transform Listener（监听并计算坐标）

代码示例

import rclpy
from rclpy.node import Node
import tf2_ros
from geometry_msgs.msg import TransformStamped

class FrameListener(Node):
    def __init__(self):
        super().__init__('frame_listener')

        self.tf_buffer = tf2_ros.Buffer()
        self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)

        self.timer = self.create_timer(0.5, self.lookup_transform)

    def lookup_transform(self):
        try:
            trans = self.tf_buffer.lookup_transform('world', 'robot', rclpy.time.Time())
            self.get_logger().info(
                f"Translation: x={trans.transform.translation.x:.2f}, y={trans.transform.translation.y:.2f}"
            )
        except Exception as e:
            self.get_logger().warn(f"Could not transform: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = FrameListener()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

说明

  • 使用 Buffer 缓存变换数据。
  • lookup_transform(target_frame, source_frame, time) 用于查询两坐标系之间的关系。

5. C++ 实现

5.1 Transform Broadcaster

代码示例

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include <chrono>
#include <cmath>

using namespace std::chrono_literals;

class FrameBroadcaster : public rclcpp::Node
{
public:
    FrameBroadcaster()
    : Node("frame_broadcaster")
    {
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        timer_ = this->create_wall_timer(
            100ms, std::bind(&FrameBroadcaster::broadcast_callback, this));
    }

private:
    void broadcast_callback()
    {
        geometry_msgs::msg::TransformStamped t;

        t.header.stamp = this->get_clock()->now();
        t.header.frame_id = "world";
        t.child_frame_id = "robot";

        t.transform.translation.x = std::cos(this->get_clock()->now().seconds());
        t.transform.translation.y = std::sin(this->get_clock()->now().seconds());
        t.transform.translation.z = 0.0;

        t.transform.rotation.x = 0.0;
        t.transform.rotation.y = 0.0;
        t.transform.rotation.z = 0.0;
        t.transform.rotation.w = 1.0;

        tf_broadcaster_->sendTransform(t);
    }

    rclcpp::TimerBase::SharedPtr timer_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<FrameBroadcaster>());
    rclcpp::shutdown();
    return 0;
}

5.2 Transform Listener

代码示例

#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/msg/transform_stamped.hpp"

class FrameListener : public rclcpp::Node
{
public:
    FrameListener()
    : Node("frame_listener")
    {
        buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);

        timer_ = this->create_wall_timer(
            500ms, std::bind(&FrameListener::lookup_callback, this));
    }

private:
    void lookup_callback()
    {
        try
        {
            geometry_msgs::msg::TransformStamped transformStamped;
            transformStamped = buffer_->lookupTransform("world", "robot", tf2::TimePointZero);
            RCLCPP_INFO(this->get_logger(), "Translation x: %.2f y: %.2f",
                        transformStamped.transform.translation.x,
                        transformStamped.transform.translation.y);
        }
        catch (const tf2::TransformException & ex)
        {
            RCLCPP_WARN(this->get_logger(), "Could not transform: %s", ex.what());
        }
    }

    std::shared_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);
    rclcpp::spin(std::make_shared<FrameListener>());
    rclcpp::shutdown();
    return 0;
}

6. 可视化验证

6.1 使用 rviz2

  • 打开 rviz2：
rviz2
  • 添加 TF 显示插件，可看到坐标系的实时变换。

6.2 使用 tf2_tools

ros2 run tf2_tools view_frames

生成 frames.pdf，查看坐标系树结构。

7. 常见问题

  • 广播频率太低：导致监听不到最新的变换，建议 10Hz 以上。
  • frame_id 命名错误：必须严格匹配，否则监听失败。
  • 四元数不合法：必须归一化，否则会报错。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值