如何使用ROS tf listener获取坐标变换并转换速度数据坐标系

1 报错问题修复 #

你遇到的报错有三个核心原因：

• 你实际运行的监听代码与你贴出的版本存在差异，错误的将话题名rexrov2/pose_gt作为坐标系名传入了lookupTransform的第一个参数，首先修正该参数为目标坐标系rexrov2/base_link
• 你缺少传感器坐标系rexrov2/pose_sensor_link_default到机器人本体坐标系rexrov2/base_link的静态TF广播，该变换是传感器安装位置的固定偏移，可通过启动文件添加静态广播节点：

  <node pkg="tf2_ros" type="static_transform_publisher" name="sensor_static_tf" args="x y z r p y rexrov2/base_link rexrov2/pose_sensor_link_default" />
  

  其中x y z r p y替换为你的传感器实际安装偏移参数
• 广播节点中错误使用ros::Time::now()作为TF时间戳，应改为使用odom消息自带的时间戳，修改广播节点中对应行：

  // 原错误写法
  transformStamped.header.stamp = ros::Time::now();
  // 修正为
  transformStamped.header.stamp = msg->header.stamp;
  

2 速度转换逻辑修复 #

你当前的监听代码逻辑完全不符合需求：没有订阅传感器的速度数据，反而用坐标偏移模长计算速度，正确的转换逻辑为：
速度属于矢量，坐标系间转换只需使用旋转变换，不需要平移分量，将world坐标系下的速度矢量乘world到base_link的旋转矩阵即可得到本体坐标系下的速度

3 修正后的完整监听节点代码 #

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>

ros::Publisher vel_pub;
tf2_ros::Buffer* tf_buffer;

void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
    geometry_msgs::TwistStamped vel_world, vel_base;
    vel_world.header = msg->header;
    vel_world.twist = msg->twist.twist;

    try
    {
        // 查找对应时间戳下world到base_link的变换，转换速度
        tf_buffer->transform(vel_world, vel_base, "rexrov2/base_link");
    }
    catch(const tf2::TransformException& e)
    {
        ROS_WARN("TF转换失败: %s", e.what());
        return;
    }
    
    vel_pub.publish(vel_base.twist);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "velocity_transformer");
    ros::NodeHandle nh;

    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    tf_buffer = &buffer;

    vel_pub = nh.advertise<geometry_msgs::Twist>("robot/vel", 10);
    ros::Subscriber odom_sub = nh.subscribe("rexrov2/pose_gt", 10, odomCallback);

    ros::spin();
    return 0;
}

内容的提问来源于stack exchange，提问作者Bob9710