2.ROS实用工具：坐标变换

最新推荐文章于 2025-04-28 00:47:31 发布

Alvin_YD

最新推荐文章于 2025-04-28 00:47:31 发布

阅读量923

点赞数 13

分类专栏： ROS学习记录文章标签：人工智能 ROS1 c++

本文链接：https://blog.youkuaiyun.com/qq_74168740/article/details/146567042

版权

ROS学习记录专栏收录该内容

2 篇文章

订阅专栏

ROS实用工具：坐标变换

作用：

在机器人系统中，不同的传感器和执行器可能位于不同的位置和方向，需要通过坐标变换来统一到一个公共的坐标系下，以便进行数据融合、路径规划、运动控制等操作。例如，将激光雷达坐标系下的障碍物信息转换到机器人底盘坐标系或世界坐标系中，使机器人能够准确地感知周围环境并做出决策。

实现方式：

ROS 中通过 tf（Transform）库来实现坐标变换。它允许节点发布和订阅坐标变换信息，通过建立坐标变换树来管理不同坐标系之间的关系。使用 tf 库，开发者可以方便地获取任意两个坐标系之间的变换矩阵，从而实现坐标点或向量在不同坐标系之间的转换。

广播TF变换

新建tf_test_pkg ROS工作包用于后面使用

# 切换到 catkin_ws 工作空间下的 src 目录
cd catkin_ws/src/

# 使用 catkin_create_pkg 工具创建一个名为 tf_test_pkg 的 ROS 包
# 该包依赖于 roscpp（ROS 的 C++ 客户端库）、tf（用于坐标变换的库）和 geometry_msgs（ROS 中常用的几何消息类型库）
catkin_create_pkg tf_test_pkg roscpp tf geometry_msgs

创建tf_broadcaster.cpp

代码如下请放入tf_broadcaster.cpp文件中：

// 包含ROS核心库的头文件，这是使用ROS功能的基础
#include <ros/ros.h> 
// 包含TF（Transform）广播器的头文件，用于发布坐标系之间的变换关系
#include <tf/transform_broadcaster.h>  

int main(int argc, char **argv) {
    // 初始化ROS节点，节点名为 "tf_broadcaster"
    ros::init(argc, argv, "tf_broadcaster");
    // 创建一个ROS节点句柄，用于与ROS系统进行通信
    ros::NodeHandle n;

    // 设置循环频率为 100Hz，即每秒执行100次循环
    ros::Rate loop_rate(100); 

    // 创建一个TF广播器对象，用于发布坐标系之间的变换
    tf::TransformBroadcaster broadcaster;
    // 创建一个TF变换对象，用于表示从 "base_link" 到 "base_laser" 的变换
    tf::Transform base_link2base_laser;

    // 设置变换的原点，即 "base_laser" 相对于 "base_link" 在x、y、z轴上的偏移量
    base_link2base_laser.setOrigin(tf::Vector3(0.1,0.0,0.2));
    // 设置变换的旋转，使用四元数表示，这里表示无旋转
    base_link2base_laser.setRotation(tf::Quaternion(0,0,0,1));

    // 进入主循环，只要ROS节点正常运行就会一直循环
    while (n.ok())
    {
        // 发布从 "base_link" 到 "base_laser" 的变换，包含当前时间戳
        broadcaster.sendTransform(tf::StampedTransform(base_link2base_laser,ros::Time::now(),"base_link","base_laser"));
        // 让程序休眠，以保持设定的循环频率
        loop_rate.sleep();
    }

    return 0;
}

点击CMakeLists.txt 用于CMake 构建系统所使用的配置文件，用于指定项目的构建规则

此代码主要用于配置一个可执行文件的构建过程，该可执行文件名为 tf_broadcaster，其源文件为 src/tf_broadcaster.cpp。同时，它还设定了依赖项和链接库，以保证项目能正确构建与运行

在这个文件最后添加如下代码（记得保存）：

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
add_dependencies(tf_broadcaster  ${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS})
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})

Catkin_make编译

# 切换到 catkin 工作空间目录，catkin_ws 是常见的 catkin 工作空间名称
cd catkin_ws

# 在当前的 catkin 工作空间下编译所有的 ROS（机器人操作系统）包
catkin_make

监听TF变换

同上面的广播TF变换（步骤一致）：

创建tf_listener.cpp文件

#include <ros/ros.h> 
// 包含 tf 变换监听器的头文件，用于监听坐标系之间的变换
#include <tf/transform_listener.h>  
// 包含点消息类型的头文件
#include <geometry_msgs/PointStamped.h> 
#include <iostream>  

int main(int argc, char **argv) {
    // 初始化 ROS 节点，节点名为 "tf_listener"
    ros::init(argc, argv, "tf_listener");
    // 创建 ROS 节点句柄
    ros::NodeHandle n;

    // 设置循环频率为 100Hz
    ros::Rate loop_rate(100); 

    // 创建一个 tf 变换监听器对象
    tf::TransformListener listener;
    // 创建一个在 base_laser 坐标系下的点消息对象
    geometry_msgs::PointStamped laser_pos;
    // 设置点消息的坐标系为 base_laser
    laser_pos.header.frame_id = "base_laser";
    // 设置点消息的时间戳为当前时间
    laser_pos.header.stamp = ros::Time();

    // 设置 base_laser 坐标系下点的 x 坐标
    laser_pos.point.x = 0.3;
    // 设置 base_laser 坐标系下点的 y 坐标
    laser_pos.point.y = 0;
    // 设置 base_laser 坐标系下点的 z 坐标
    laser_pos.point.z = 0;

    // 创建一个在 base_link 坐标系下的点消息对象
    geometry_msgs::PointStamped base_pos;

    // 进入 ROS 主循环，只要节点正常运行就持续循环
    while (n.ok()) {
        // 等待从 base_laser 坐标系到 base_link 坐标系的变换，最多等待 3 秒
        if(listener.waitForTransform("base_link","base_laser",ros::Time(0),ros::Duration(3))) {
            // 将 base_laser 坐标系下的点转换到 base_link 坐标系下
            listener.transformPoint("base_link",laser_pos,base_pos);
            // 输出 base_laser 坐标系下点的坐标
            ROS_INFO("pointpos in base_laser:(%.2f,%.2f,%.2f)",laser_pos.point.x,laser_pos.point.y,laser_pos.point.z);
            // 输出 base_link 坐标系下点的坐标
            ROS_INFO("pointpos in base_link:(%.2f,%.2f,%.2f)",base_pos.point.x,base_pos.point.y,base_pos.point.z);

            // 创建一个带时间戳的变换对象
            tf::StampedTransform laserTransform;
            // 查找从 base_laser 坐标系到 base_link 坐标系的变换
            listener.lookupTransform("base_link","base_laser",ros::Time(0),laserTransform);
            // 输出 base_laser 坐标系相对于 base_link 坐标系原点的 x 坐标
            std::cout<<"laserTransform.getOrigin().getX():"<<laserTransform.getOrigin().getX()<<std::endl;
            // 输出 base_laser 坐标系相对于 base_link 坐标系原点的 y 坐标
            std::cout<<"laserTransform.getOrigin().getY():"<<laserTransform.getOrigin().getY()<<std::endl;
            // 输出 base_laser 坐标系相对于 base_link 坐标系原点的 z 坐标
            std::cout<<"laserTransform.getOrigin().getZ():"<<laserTransform.getOrigin().getZ()<<std::endl;
        }
        // 按照设定的频率休眠
        loop_rate.sleep();
    }
    return 0;
}

在这个文件最后添加如下代码（记得保存）：

add_executable(tf_listener src/tf_listener.cpp)
add_dependencies(tf_listener  ${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS})
target_link_libraries(tf_listener ${catkin_LIBRARIES})

同理CMake编译一下

# 切换到 catkin 工作空间目录，catkin_ws 是常见的 catkin 工作空间名称
cd catkin_ws

# 在当前的 catkin 工作空间下编译所有的 ROS（机器人操作系统）包
catkin_make

编译成功

启动节点管理器

cd 
roscore

继续多开终端（ctrl+shift+T）：

这条命令的作用是运行 tf_test_pkg 包中的 tf_broadcaster 可执行文件，该文件通常会发布坐标系之间的变换信息，让其他节点能够获取并使用这些信息。

rosrun tf_test_pkg tf_broadcaster

再开终端输入：

rosrun tf_test_pkg tf_listener

TF 是 ROS 中用来管理坐标系变换的工具。tf_listener 这个可执行文件的主要作用是监听 TF 系统所发布的坐标系变换信息，并且能够根据需求获取不同坐标系之间的变换关系。

定义坐标系：明确目标坐标系和源坐标系。
base_laser:激光雷达
base_link:源原坐标

再输入:

rqt_graph

rqt_graph 是 ROS（机器人操作系统）中的一个可视化工具，用于显示 ROS 系统中节点（nodes）和话题（topics）之间的连接关系

节点（Nodes）：在图中以矩形框表示，代表 ROS 系统中的各个节点。节点是 ROS 系统中可执行程序的实例，它们可以发布或者订阅话题，提供或者调用服务等。
话题（Topics）：用椭圆表示，话题是节点之间进行消息传递的一种方式。一个节点可以发布消息到某个话题，而其他节点可以订阅这个话题来接收消息。
连接（Connections）：节点和话题之间的连线表示它们之间的连接关系。箭头方向表示消息的流向，从发布者指向订阅者。

再输入：

rosrun rqt_tf_tree rqt_tf_tree

运行 rosrun rqt_tf_tree rqt_tf_tree 之后，会打开一个可视化窗口，此窗口能够展示当前 ROS 系统里所有 TF 坐标系之间的关系。借助这个工具，你可以直观地看到各个坐标系是如何相互关联的，从而更方便地调试和理解机器人的坐标变换系统。

时间戳信息

Recorded at time: 1743066503.6646917
表示数据记录的时间戳，用于标记当前 tf 变换关系的记录时刻。

坐标系变换关系

base_link → base_laser
图中通过箭头连接两个椭圆形节点，表明从机器人基础坐标系 base_link 到激光雷达坐标系 base_laser 的坐标变换关系，即描述激光雷达相对于机器人本体的位置和姿态。

tf 广播器信息

右侧参数描述了 tf_broadcaster（tf 广播器）的工作状态：

Broadcaster: /tf_broadcaster：指明执行该坐标变换广播的节点名称为 /tf_broadcaster。
Average rate: 100.95：表示 tf 变换的平均发布频率，单位为 Hz（每秒约发布 100.95 次变换数据）。
Buffer length: 1.05：tf 变换缓冲的时间长度（单位为秒），即系统会缓存最近 1.05 秒内的坐标变换数据。
Most recent transform: 1743066503.659：最新一次坐标变换的时间戳。
Oldest transform: 1743066502.608：缓冲中最旧一次坐标变换的时间戳，两者时间差体现缓冲数据的时间范围。

最后输入：

rosrun tf tf_echo /base_link base_laser

此命令的主要功能是输出两个坐标系（/base_link 和 base_laser）之间的变换信息，这些信息包含平移和旋转。

执行该命令后，会输出这两个坐标系之间的平移（x、y、z）和旋转（四元数表示）信息，同时还会显示变换的时间戳。了解激光雷达相对于机器人本体的位置和姿态。

以上便是ROS实用工具之一：坐标变换的大致流程，欢迎大家上机测试哈，多多分享，点赞，关注，谢谢。