ROS TF Tutorial

最新推荐文章于 2025-06-12 08:53:32 发布

NoEdUl

最新推荐文章于 2025-06-12 08:53:32 发布

阅读量291

点赞数

CC 4.0 BY-SA版权

分类专栏： ROS

本文链接：https://blog.csdn.net/u012947309/article/details/104810144

ROS 专栏收录该内容

5 篇文章

订阅专栏

本文深入解析了ROS中的tf系统，详细介绍了tf的工作原理，包括坐标系之间的转换、tf树的构建以及如何通过tf进行时间同步。通过具体代码示例，展示了如何在ROS节点中实现坐标变换，并探讨了tf与传感器数据流的关系。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

跑不同的SLAM总会遇到frame transform的问题。

再加上我一直好奇odom信息为什么从/tf来而不是从/odom来。

于是就找了一下tf_tutorial。

理解可能有完全错误和不到位的地方。

frame 就是一个一个坐标系，他们之间的transform就是各个坐标系之间的转换。类似刚体rigid之间的转换。

一个机器人身上各个固定的sensor和机器人之间的transform是固定的，但是机器人/sensor参考世界坐标系的transform是实时在变化的。

所以/tf话题就是时刻在记录这种变化。

以官方tf_tutorial来举例

键入rqt找到tf tree

可以看到两个frame turtle1 tuetle2各自参考父母frame world。

用tf tf_echo world turtle1 可以看到echo出这个transform是时刻在变化的。

打开node graph

可见我们两个node /turtle2_tf_broadcast 和/turtle1_tf_broadcaster 各自在subscribe topic /turtle2/pose and /turtle1/pose。

topic /turtleX/pose 的数据由node /sim发布。

同时我们的node /lister 在subsribe topic /tf, 而turtleX_tf_broadcast 节点在各自发布信息到/tf话题中。

最后node /listener 根据监/tf 得到的frame transform信息计算出对应的速度信息，发布到/tirtle2/cmd_vel话题，让node /sim接受过去。

完成整个乌龟2追乌龟1的功能。

turtle_tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr &msg)
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  //parameters : transform information, time stamp, ref frame, local frame
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "my_tf_broadcaster");
  if (argc != 2)
  {
    ROS_ERROR("need turtle name as argument");
    return -1;
  };
  turtle_name = argv[1]; //given by launch file

  ros::NodeHandle node;
  //subscribe to /turtleX/pose topic
  ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

turtle_tf_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "my_tf_listener");

  ros::NodeHandle node;

  //call spawn service to spawn second turtle
  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle =
      node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

  //set up this node to publish velocity information to topic /turtle2/cmd_vel
  ros::Publisher turtle_vel =
      node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

  
  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok())
  {
    tf::StampedTransform transform;
    try
    {
      //lookup transform infomration of from turtle2 ref to turtle1.
      //Seems the listener automatically listen to /tf?
      listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
      listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
    }
    catch (tf::TransformException &ex)
    {
      ROS_ERROR("%s", ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    
    //publish velocity cmd to topic /turtle2/cmd_vel
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                    transform.getOrigin().x());
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                  pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};

所以Node和frame什么关系呢？好像完全没关系。

只是可以在node里面用tf namespace 下的某些类的实例直接去操作frame罢了。

下面的代码就是新建了一个node，不停地发布frame carrot1 ref to turtle1的trasnformation。

frame_tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

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

    tf::TransformBroadcaster br;
    tf::Transform transform;

    ros::Rate rate(10.0);
    while (node.ok())
    {
        transform.setOrigin(tf::Vector3(0.0, 2.0, 0.0));
        transform.setRotation(tf::Quaternion(0, 0, 0, 1));
        //send a transform to /tf, ref frame turtl1, local frame carrot1
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
        rate.sleep();
    }
}

tf_tree可以看到多了turtle1多了一个child frame

然后tutorial介绍tf和时间的关系。

因为transform时刻都在变化（哪怕变化量为0），/tf时刻在记录被broadcaster发布出来的transform。好像是记录最近10秒。

所以当我们请求某两个frame之间的变化的时候，我们可以请求某个特定时间的transform。（Nice，做融合，后验都要这个）

要看到这个特性，可以用

      ros::Time now = ros::Time::now();
      //wait for transform from frame turtle2 ref to turtle1, at time "now", wait for up to 3 second
      listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(3.0));
      listener.lookupTransform("/turtle2", "/turtle1", now, transform);

但是一般用time(0)获取buffer里最后一个transform会比较好，毕竟now时刻的transformation不一定已经写入了buffer。

随后教程给了一个，如果要让turtle2去追五秒前的turtle1，要怎么做。

ros::Time past = ros::Time::now() - ros::Duration(5.0);
listener.waitForTransform("/turtle2", "/turtle1", past, ros::Duration(1.0));
listener.lookupTransform("/turtle2", "/turtle1", past, transform);

如果是上述代码，我们只能拿到五秒前turtle2相对于turtle1的关系。并不是现在的turtle2相对于五秒前turtle1的关系。

实际应该这么用，给指定的frame带上时间戳，同时指定一个不随着时间变化的frame作为参考。

      ros::Time now = ros::Time::now();
      ros::Time past = now - ros::Duration(5.0);
      listener.waitForTransform("/turtle2", now,
                                "/turtle1", past,
                                "/world", ros::Duration(1.0));
      listener.lookupTransform("/turtle2", now,
                               "/turtle1", past,
                               "/world", transform);

实际运行过程中可能会有

[ERROR] [1583952187.300482857]: Lookup would require extrapolation into the past. Requested time 1583952181.298486551 but the earliest data is at time 1583952186.696968043, when looking up transform from frame [turtle1] to frame [world]

因为系统刚开始前五秒的时候，是找不到5秒前的turtlr1的transform信息的。

这张图解释了实际我们用的是5秒前turtle1 ref to world和当前turtle2 ref to world 来计算当前turtle2 ref to turtle1 （5sec ago）的。