TF坐标变换

Ros先后提供了两个和tf相关的功能包：tf、tf2。开发者应该使用tf2，放弃用tf。
tf使用了ros的topic机制。要发布tf的node，发布名叫“/tf”或“/static_tf”的话题。对“/static_tf”，为确保接收者能够收到，往往采用周期性发送。
订阅者收到/tf、/tf_static后，BufferCore::setTransform会确保让存在child_frame_id的那个frame，并把此个变换存储到该frame。对header.frame_id，则只是frameIDs_增加一项，不能保证有存储对象。
对BufferCore来说，对/tf_static、/tf的不一样处理表现在不同的存储方式。/tf_static时，StaticCache的storage_就是个TransformStorage，新变换来时整个替换。/tf时，TimeCache的storage_是个按时间戳排序的std::deque，队列头是最近发生的变换，队列中单元有着一样的child_frame_id_，但frame_id_可能不一样。（如果出现不一样是不是表示编程出错？）
tf常出现两种变量名：target_id/frame、source_id/frame，它们用于表示一次坐标变化涉及的到两个坐标系。具体到tf树中位置，靠近根的是target_id，远离根的是source_id。在geometry_msgs::TransformStamped，target_id对应header.frame_id，source_id对应child_frame_id。
只要target_frame、source_frame都在tf树中，那它们就是可变换，canTransform返回true。对两个frame参数，并没有要求target_frame一定要离根更近的那个，但用离根更近那个的话，可以些许提高些效率。

TF是一个让用户随时间跟踪多个坐标系的功能包。它使用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间、坐标系间完成点、向量等坐标的变换。

一、topic机制

tf的发布和接收使用了ros的topic机制。要发布tf的node，发布名叫“/tf”或“/static_tf”的话题，要接收tf变换的则订阅这两个话题。随着各自订阅不断收到tf，那些个接收tf的node都自个存储了“整个”tf树，也就实现了去中心化。“整个”加引号是因为对“/tf”，某个node只能收到它开始订阅“/tf”后的那个些tf，当然对使用tf目的来说，关心的往往是最近很短时间内的tf，丢失些历史tf不影响使用。“/static_tf”只有一种变换，而且会周期发送，实际使用中反而不会丢。

对要接收tf的node，第一步是构造一个tf::TransformListener对象。构造时会依次执行两个操作。

订阅“/tf”、“/static_tf”
启动一个线程：TransformListener::dedicatedListenerThread。

为调用“/tf”、“/static_tf”设置的回调函数，TransformListener使用CallbackQueue机制，对应的变量是tf_message_callback_queue_。是在dedicatedListenerThread线程调用tf_message_callback_queue_.callAvailable，因而也是在该线程执行收到tf后的回调函数。

二、“/tf”还是“/static_tf”

tf发布者发布变换可使用两种话题：“/tf”、“/static_tf”。使用哪种的判断依据是此坐标变换是否会随着时间而变动。

/tf：动态的位置关系发布到此topic。动态指的是运行时会发生变动的坐标系变换。
/tf_static：固定的位置关系发布到此topic，固定指是运行时不会发生变动的坐标系变换。

在图2，[map, odom]、[odom, base_footprint]，它们的坐标变换会随着时间而变动，使用“/tf”，其它的则使用“/static_tf”。

使用“/static_tf”会有个疑问：如果tf发布时刻，希望接收该tf变换的node还没订阅“/static_tf”，那该node是否就接收不到这个tf变换？——极有可能是。举个例子：/base_link_to_laser发布[base_link, laser]，/amcl希望知道这个变换，如果/base_link_to_laser发布时/amcl还没订阅“/static_tf”，那/amcl极可就收不到那次变换。以下是原因。

“/static_tf”使用的是topic机制，topic没有缓存历史记录功能。
topic不能说没缓存，若advertise时参数latch设为true，会缓存最后一次记录。而“/static_tf”恰是把latch设为了true。所以会存在这么个现象，如果/amcl订阅“/static_tf”时刻，[base_link, laser]是最后一次发布的，那/amcl会收到，否则就收不到。

实际编程时，很难保证/acml订阅时发布的最后一个“/static_tf”是啥，还有，系统在运行多线程，不能奢望/amcl订阅时/base_link_to_laser一定已发布那次tf了。为解决这些问题，“/static_tf”往往不会是只发送一次，会是采取周期性发送。static_transform_publisher中的period_in_ms就是设置那发送间隔用的。

三、存储tf变换：BufferCore::setTransform

不论是“/tf”，还是“/static_tf”，订阅者收到变换消息后，会调用BufferCore::setTransform，setTransform是存储变换的核心函数。

由于会同时存在N个tf::TransformListener，虽然某个tf只发布了一次，但是会调用了N次BufferCore::setTransform存储这个tf，这N次的BufferCore应该是不同对象。

<libros>/geometry2/tf2/src/buffer_core.cpp
------
@transform_in。发布者发布的坐标变换。
@authority。发布者在connection_header["callerid"]字段设置的名称。如果没指定名称，取固定值“unknown_publisher”。
@is_static。true：来自/tf_static话题。false：来自/tf话题。
bool BufferCore::setTransform(const geometry_msgs::TransformStamped& transform_in, 
        const std::string& authority, bool is_static)
{
  geometry_msgs::TransformStamped stripped = transform_in;
  ...
  // 判断transform_in参数是否合法，不合法就返回false。
  if (error_exists)
    return false;
  
  {
    boost::mutex::scoped_lock lock(frame_mutex_);
    CompactFrameID frame_number = lookupOrInsertFrameNumber(stripped.child_frame_id);
    TimeCacheInterfacePtr frame = getFrame(frame_number);
    if (frame == NULL)
      frame = allocateFrame(frame_number, is_static);

    std::string error_string;
    if (frame->insertData(TransformStorage(stripped, 
        lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number), &error_string))
    {
      frame_authority_[frame_number] = authority;
    }
    else
    {
      CONSOLE_BRIDGE_logWarn((error_string+" for frame %s at time %lf according to authority %s").c_str(), 
           stripped.child_frame_id.c_str(), stripped.header.stamp.toSec(), authority.c_str());
      return false;
    }
  }

  testTransformableRequests();

  return true;
}

在分析setTransform前，让解释BufferCore常出现的两个变量名：target_id、source_id。一次坐标变化会涉及到两个坐标系，target_id、source_id就是此次变换涉及到的两个坐标系，值是string类型，像图1中的“map”、“laser”、“base_link”，等等。具体到某个变换，像laser和odom之间，哪个是target_id，哪个是source_id？——靠近根的是target_id，远离根的是source_id。为什么target_id更靠近根？——在tf树，叶子更多表示可直接获取数据源的传感器，像激光雷达(lawer)、IMU(imu_link)，这些不同坐标系，终究要“归一化”到某个坐标系，一个称为世界的坐标系。实际使用时，世界坐标系可能不叫world，像图1就叫“map”。但不影响“map”是那个终级坐标系，是那个最后的target，于是在变化时，能更靠近“map”的归为target_id。

transform_in类型是geometry_msgs::TransformStamped，它里面存着两个坐标系：header.frame_id和child_frame_id，以下是它们和target_id、source_id的对应关系。

geometry_msgs::TransformStamped	习惯名称
header.frame_id	target_id
child_frame_id	source_id

接下分析setTransform执行的操作

2.1、判断参数是否合法

主要判断的参数是transform_in，像transform_in中存储旋转的rotation必须是归一化后的四元数。参数不合法就忽略此次变换，返回false。

2.2、查找child_frame_id，没有就添加

lookupOrInsertFrameNumber负责执行查找并添加。

typedef uint32_t CompactFrameID;
class BufferCore
  typedef std::unordered_map<std::string, CompactFrameID> M_StringToCompactFrameID;
  M_StringToCompactFrameID frameIDs_;

  typedef std::vector<TimeCacheInterfacePtr> V_TimeCacheInterface;
  V_TimeCacheInterface frames_;
};

<libros>/geometry2/tf2/src/buffer_core.cpp
------
CompactFrameID BufferCore::lookupOrInsertFrameNumber(const std::string& frameid_str)
{
  CompactFrameID retval = 0;
  M_StringToCompactFrameID::iterator map_it = frameIDs_.find(frameid_str);
  if (map_it == frameIDs_.end())
  {
    retval = CompactFrameID(frames_.size());
    frames_.push_back(TimeCacheInterfacePtr());//Just a place holder for iteration
    frameIDs_[frameid_str] = retval;
    frameIDs_reverse.push_back(frameid_str);
  }
  else
    retval = frameIDs_[frameid_str];

  return retval;
}

lookupOrInsertFrameNumber以frameid为key在frameIDs_中查找，找到就返回该key对应的整数标识，否则向frameIDs_新增一个[key, value]，并返回value。

frameIDs_数据结构是unordered_map，即不排序的map。key是tf变化的frame_id，value是一个整数值。frameIDs_固定有一个key是“NO_PARENT”的frame，它的value是0。value=0是个特殊值，1）它总是frameIDs_第一个单元的值。2）函数lookupFrameNumber(frameid_str)返回时，它用于表示参数frameid_str对应的frame不存在。

frameIDs_中的单元数只会增长，不会erase。

2.3、确保frames_中有对应的TimeCacheInterfacePtr对象

第二步添加到frameIDs_，只是记录child_frame_id这个字符串的frame来过了，接下是保存此次变换的数据。那数据保存在哪里？——frames_。

frames_数据结构是std::vector<TimeCacheInterfacePtr>，正如图2显示，长度总和frameIDs_一致，索引则对应frameIDs_中的value值。

变换数据将存储在frames_[n]，根据is_static是true还是false，frames_[n]可能是两种类型：StaticCache、TimeCache。

TimeCacheInterfacePtr BufferCore::allocateFrame(CompactFrameID cfid, bool is_static)
{
  TimeCacheInterfacePtr frame_ptr = frames_[cfid];
  if (is_static) {
    frames_[cfid] = TimeCacheInterfacePtr(new StaticCache());
  } else {
    frames_[cfid] = TimeCacheInterfacePtr(new TimeCache(cache_time_));
  }

  return frames_[cfid];
}

已确保frames_[n]有效后，接下就是调用frame->insertData把此次变换放进TimeCacheInterfacePtr。

frame->insertData(TransformStorage(stripped, lookupOrInsertFrameNumber(stripped.header.frame_id), frame_number), 
     &error_string)

/tf_static时，StaticCache把数据存放在“TransformStorage storage_”，storage_就是个TransformStorage，insertData直接整个替换。

/tf时，storage_是个按时间戳排序的std::deque，队列头是最近发生的变换。对deque中的单元，因为frames_[n]中的n是child_frame_id，所以它们的child_frame_id_是一样的，但frame_id_对应header.frame_id，有可能不一样。

三、清空缓存：BufferCore::clear()

BufferCore::clear()只是清空缓存，不清空存储frame信息的成员变量，像frameIDs_、frames_。

只清空缓存也是有用的。launcher在一次导航结束后，就会调用clear()清空缓存，避免再次启动导航时，这些过时的tf导致各坐标系间依旧可转换。接下会说，即使BufferCore::frmaeIDs_已存在target_frame、source_frame，只要缓存是空，这时canTransform会也是返回false。

四、BufferCore::canTransform

tf的一个常用操作是要判断树中两个frame之间是否可转换。Buffer、BufferCore都提供了canTransform，作为派生类的Buffer多带一个参数timeout。

time。要求tf的必须发生时刻。time不是0时，缓存中存在发生时刻>=time的tf才算有tf。time是0时，树中有过一个tf就行，不必在乎发生时刻。如果source_frame到target_frame的所有变化都是静态变化，忽略该参数。
timeout。如果当前坐标不可变换，最多等待的时间。内部会以timeout/100的时间片进行sleep。如果timeout是0，则不等待，只看第一次BufferCore::canTransform的处理结果。
返回值。如果target_frame、source_frame之间可变换，返回true，否则false。BufferCore::frmaeIDs_已存在target_frame、source_frame，刚调用过BufferCore::clear()把tf缓存清空了，这时canTransform会返回成功吗？——不会，由于参数time。time不是0时，要求缓存必须存在发生时刻>=time的tf。time是0时，要调用getLatestCommonTime得到缓存中最新发生的tf，由于没有tf，这函数要返回失败。也就是说，不论time是何值，都要求缓存不能是空。

bool Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, 
                     const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
{
  ...
  // poll for transform if timeout is set
  ros::Time start_time = now_fallback_to_wall();
  const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE;
  while (now_fallback_to_wall() < start_time + timeout && 
         !canTransform(target_frame, source_frame, time) &&
         (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) &&  //don't wait when we detect a bag loop
         (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
    {
      sleep_fallback_to_wall(sleep_duration);

while块的作用就是延时，最坏情况延时timeout秒。为做到更精准延时，不能一个sleep(timeout)就了事。要把它分为粒度更小的时间片sleep_duration。CAN_TRANSFORM_POLLING_SCALE用于决定怎么个分法，默认值0.01。举个例子，timeout值是1秒，时间片sleep_durationy就是10毫秒。假设100毫秒后source_frame-->target_frame变得可转换，那理论上10次while就够了，延时时间可基本等于100毫秒。

计算条件时要执行不带参数timeout的canTransform，和while块后的canTransform一样，是基类BufferCore的BufferCore::canTransform。

now_fallback_to_wall()值就是Time::now()，因而当参数timeout是0时，第一次while就不满足条件了。换句话说，timeout是0时，只会执行一次BufferCore::canTransform。

    }
  bool retval = canTransform(target_frame, source_frame, time, errstr);
  conditionally_append_timeout_info(errstr, start_time, timeout);
  return retval;
}

从上面可看到，判断是否可转换的核心逻辑是BufferCore::canTransform，后者主要通过walkToTopParent。

f：函数执行过程中需要调用些会有“特定于场景”的操作，像gather、accum、finalize。f是一个封装了这些操作的一个对象。目前有两种F，用于只判断是否可转换(BufferCore::canTransform)的CanTransformAccum、用于获取tf(BufferCore::lookupTransform)的TransformAccum。BufferCore把F定义为模板类。依我做法，会把F定义一个基类，然后有虚函数gather、accum、finalize。CanTransformAccum、TransformAccum则是它的派生类。。
target_id：target_frame在frameIDs_中的value。
source_id：source_frame在frameIDs_中的value。

/geometry2/tf2/src/buffer_core.cpp
------
template
int BufferCore::walkToTopParent(F& f, ros::Time time, CompactFrameID target_id,
    CompactFrameID source_id, std::string* error_string, std::vector
    *frame_chain) const
{
  if (frame_chain)
    frame_chain->clear();
 
  // Short circuit if zero length transform to allow lookups on non existant links
  if (source_id == target_id)
  {
    f.finalize(Identity, time);
    return tf2_msgs::TF2Error::NO_ERROR;
  }
 
  //If getting the latest get the latest common time
  if (time == ros::Time())
  {
    // canTransform、lookupTransform的参数time是0，改为搜索最近发生的那个tf。
    int retval = getLatestCommonTime(target_id, source_id, time, error_string);
    if (retval != tf2_msgs::TF2Error::NO_ERROR)
    {
      return retval;
    }
  }
 
  // Walk the tree to its root from the source frame, accumulating the transform
  // 第一次溯源，从“应该”更远离根的source_id开始。
  CompactFrameID frame = source_id;
  CompactFrameID top_parent = frame;
  uint32_t depth = 0;
 
  std::string extrapolation_error_string;
  bool extrapolation_might_have_occurred = false;
 
  while (frame != 0)
  {
    TimeCacheInterfacePtr cache = getFrame(frame);
    if (frame_chain)
      frame_chain->push_back(frame);
 
    if (!cache)
    {
      // There will be no cache for the very root of the tree
      top_parent = frame;
      break;
    }
 
    // 可以这么认为，f.gather返回BufferCore::setTransform时，child_frame_id是frame时的header.frame_id
    CompactFrameID parent = f.gather(cache, time, error_string ? &extrapolation_error_string : NULL);
    if (parent == 0)
    {
      // 此个frame没有父frame。有三种情况。
      // 1）它没法到达该tf树的根。2）它就是tf树的根。3）是/tf，时间戳不符合条件。
      // Just break out here... there may still be a path from source -> target
      top_parent = frame;
      extrapolation_might_have_occurred = true;
      break;
    }
 
    // Early out... target frame is a direct parent of the source frame
    if (frame == target_id)
    {
      // 根幸运，第一次溯源就成功了。
      // 为让第一次就成功，source_id要设置为远离根的frame，这样溯源时就能找到离根部更近的target_id。
      f.finalize(TargetParentOfSource, time);

只是判断是否可变换时(canTransform)，f.finalize是空操作。获取tf时(lookupTransform)，f.finalize作用是把最终tf赋值给result_vec（平移）、result_quat（旋转），见“7.6”。

      return tf2_msgs::TF2Error::NO_ERROR;
    }
 
    f.accum(true);

只是判断是否可变换时(canTransform)，f.accum是空操作。获取tf时(lookupTransform)，f.accum会用于计算合tf。如果是多阶段，在一次lookupTransform，f.accum会被调用多次，见“7.2、7.3、7.4、7.5”。

    top_parent = frame;
    // 下一while从此次找到的parent开始
    frame = parent;

    ++depth;
    if (depth > MAX_GRAPH_DEPTH)
    {
      if (error_string)
      {
        std::stringstream ss;
        ss << "The tf tree is invalid because it contains a loop." << std::endl
           << allFramesAsStringNoLock() << std::endl;
        *error_string = ss.str();
      }
      return tf2_msgs::TF2Error::LOOKUP_ERROR;
    }
  }
  ...
}

只要target_frame、source_frame都在tf树中，那它们就是可变换。这里就有两种可能：1）target_frame是source_frame的父frame，像图1中的odom和laser。2）target_frame和source_frame拥有同一个父frame，像图1中的laser、imu_link。

需要的话，walkToTopParent会进行两次溯源，第一次从“应该”更远离根的source_id开始，第二次从target_id。对BufferCore::canTransform中的参数target_frame、source_frame，并没有要求target_frame一定要离根更近的那个，但用离根更近那个的话，可以让在walkToTopParent时只须一次溯源。举个例子，要判断odom、laser之间是否可变换，虽然target_frame=laser、source_frame=odom时，canTransform也会返回true，但此时walkToTopParent须要两次溯源，若改为target_frame=odom，则只须一次。

walkToTopParent执行过程中，需要调用些会有“特定于场景”的操作，f是CanTransformAccum类的对象。有两种典型场景：1）只是判断两frame间是否可转换：CanTransformAccum。2）获取两frame间变换：TransformAccum。

CanTransformAccum中gather方法是返回参数cache的父frame。这个父frame是BufferCore::setTransform时，传下的header.frame_id。但动态tf时，gather要依赖target_time，当中要涉及TimeCache::findClosest。

以时间戳target_time为参数，在缓存队列中找寻时间上最接近的变换。
uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, 
        std::string* error_str);
返回值是三种。0：没有找到。1：精确匹配，找到的变化存放在one。2：区间匹配，变化在[one, tow]之间，时间上tow > one。
target_time.isZero时，取最近收到的变换。
缓存中只有一个变化时，该变化的时间必须等于target_time，否则查找失败。
假设缓存最早的时间是earliest_time，最近时间是latest_time，那target_time必须在区间[earliest_time, latest_time]，否查找失败。

五、获取tf：BufferCore::lookupTransform

5.1 BufferCore::lookupTransform

lookupTransform用于获取source_frame-->target_frame的tf。通过这个tf，假设a是source_frame下坐标，tf*a就是target_frame下的坐标。当然，它可用于计算多阶段的合tf，像计算图1中base_footprint-->map的tf。类似canTransform，Buffer、BufferCore都提供了lookupTransform，作为派生类的Buffer多带一个参数timeout，timeout语意和canTransform时的一模一样。

参数

target_frame。通常是靠近根的那个坐标系。
source_frame。通常是离根远的那个坐标系。
time。要求tf的必须发生时刻。time不是0时，要获取的tf发生时刻必须>=time，此时存在两种情况。一是缓存中找到个tf，它的发生时刻等于time，那这个tf正是要返回的tf。二是缓存中找不到发生时刻等于time的，但找到两个相邻tf：one、tow，one的发生时刻小于time，tow则大于time，称为区间匹配。这时要根据one、two按比例生成一个新位姿，由这个新位姿生成要返回的tf。time是0时，取最新发生的那个tf。另外，如果source_frame到target_frame的所有变化都是静态变化，忽略该参数。

返回值。geometry_msgs::TransformStamped类型的tf。因为一旦获取失败，都会抛出异常，所以只要正常返回那就是查找成功。

tf.header.stamp。指示这个tf生产者产生它的时刻，和tf何时存入tf没有关系。值来自参数time。参数time不是0时，header.stamp就等于time。是0时，值等于缓存中最新tf的stamp。图3有说明为什么区间匹配时，tf.header.stamp等于参数time。要计算的是多阶段合tf时，取当中每个阶段tf.header.stamp最小值。也就是说，tf有效时刻取决于当中各子tf最早发生的那个时刻。
tf.pose。不是区间匹配时，tf的位姿值等于缓存中对应那个tf。是区间匹配时，是由one、two按比例算出的位姿。怎么生成新位姿见图3。

geometry_msgs::TransformStamped BufferCore::lookupTransform(const std::string& target_frame,
                                                            const std::string& source_frame,
                                                            const ros::Time& time) const
{
  boost::mutex::scoped_lock lock(frame_mutex_);

  if (target_frame == source_frame) {
    ...

target_frame和source_frame是同一个坐标系

    return identity;
  }

  //Identify case does not need to be validated above
  CompactFrameID target_id = validateFrameId("lookupTransform argument target_frame", target_frame);
  CompactFrameID source_id = validateFrameId("lookupTransform argument source_frame", source_frame);

在frameIDs_映射中，根据key查找出CompactFrameID类型的value。在图2，当作为key的target_frame=="map"，那作为value的target_id是6。

  std::string error_string;
  TransformAccum accum;
  int retval = walkToTopParent(accum, time, target_id, source_id, &error_string);

要从tf树得到tf，不是简单地像canTransform判断能不能变换了，F需要复杂点的TransformAccum。经过walkToTopParent，retval是TF2Error::NO_ERROR时表示成功得到tf，此时accum存储着得到的tf。

  if (retval != tf2_msgs::TF2Error::NO_ERROR) {

walkToTopParent失败，在这里会用throw抛出异常

  }

  geometry_msgs::TransformStamped output_transform;
  transformTF2ToMsg(accum.result_quat, accum.result_vec, output_transform, accum.time, target_frame, source_frame);

walkToTopParent成功，此时accum.result_quat存储tf的旋转数值，accum.result_vec则是tf的偏移数值。accum.time是参数time经过调整的值，具本如何赋值见上面“返回值”描述中的tf.header.stamp。

transformTF2ToMsg逻辑很简单，5个参数直接赋值，最终生成要返回的geometry_msgs::TransformStamped类型的tf。

  return output_transform;
}

void transformTF2ToMsg(const tf2::Quaternion& orient, const tf2::Vector3& pos, 
    geometry_msgs::TransformStamped& msg, ros::Time stamp, const std::string& frame_id, const std::string& child_frame_id)
{
  transformTF2ToMsg(orient, pos, msg.transform);
  msg.header.stamp = stamp;
  msg.header.frame_id = frame_id;
  msg.child_frame_id = child_frame_id;
}

为加深理解walkToTopParent是如何生成tf，让查看个区间匹配时现场。区间匹配时，TimeCache::findClosest返回等于2，发生时间在[one, tow]之间，时间上tow > one。

lookupTransform参数time的值是1670640126.75983800，正如图3所示，它赋值给了interpolate的参数time。在缓存TimeCache，找到time是在[one(1670640126.73683000), two(1670640126.80179300)]之间。那TimeCache::interpolate是如何生成要返回的tf？即参数中TransformStorage类型的output。

output.translation_。tf平移部分用setInterpolate3，基于one、two按比例生成。
output.rotation_。tf旋转部分用slerp，基于one、two按比例生成。
output.stamp_ = time。时间戳赋值为lookupTransform给定的参数time。

5.2 返回值tf中的tf.header.stamp

上面已说过，tf.header.stamp指示这个tf生产者产生它的时刻，和tf何时存入tf没有关系。如果tf是一次变换，像从base_footprint-->odom，或odom-->map，那stamp较好理解。对应到代码，就是上面BufferCore::setTransform第一个参数transform_in中的header.stamp，

如果tf是两个甚至多个相乘出来，像base_footprint-->map，取当中每个阶段tf的最小值。也就是说，tf有效时刻取决于当中各子tf最早发生的那个时刻。

调用者有时须要读取这时间戳。举个例子，机器人坐标是base_footprint，世界坐标系是map。对base_footprint-->map的tf，如果最新tf已经距离现在有段时间了，而机器人是动的，间隔一长，tf就不能反应当前位姿。这时遇到太老的tf就要认为无效。Costmap2DROS::getRobotPose就有这逻辑，变量transform_tolerance_存储着间隔阈值，默认0.5秒。

<libros>/include/tf2_ros/buffer_interface.h
------
class BufferInterface
{
  template <class T>
    T& transform(const T& in, T& out, 
		 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
  {
    // do the transform
    tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
    return out;
  }
};

<libros>/costmap_2d/src/costmap_2d_rose.cpp
------
bool Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
{
  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
  geometry_msgs::PoseStamped robot_pose;
  tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);

以上这个tf2::toMsg作用的是让位姿robot_pose.pose等于“0”。“0”位姿时，坐标position等于0，对旋转部分，用四元数orientation表示0是(0, 0, 0, 1)。

  robot_pose.header.frame_id = robot_base_frame_;
  robot_pose.header.stamp = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later

getRobotPose目的是要求出机器人中心在/map坐标系下的坐标及角度。怎么求这值呢？

robot_base_frame_是“base_footprint”，global_frame是“map”，要求的是base_footprint-->map的tf。

  // get the global pose of the robot
  try
  {
    // use current time if possible (makes sure it's not in the future)
    if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
    {
      geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
      tf2::doTransform(robot_pose, global_pose, transform);

进这个入口，意味着在tf树中，至少有存在着发生时间>=current_time的base_footprint-->map的tf。但是，current_time是正是此刻，要突然收到tf，基本没可能。当然，如果有，那就用这个tf计算global_pose。

    }
    // use the latest otherwise
    else
    {
      tf_.transform(robot_pose, global_pose, global_frame_);

回看上面的BufferInterface::transform，此个入口逻辑和上面if块是一样的，只是lookupTransform时把current_time改为ros::Duration(0.0)。换句话说，告诉tf模块，没发生时刻限制了，返回最新的那个tf就行。

两个入口的区别：第一个要求tf发生时间>=current_time，第二个只要求tf最新。由于很难保证tf必须发生在现在，实际运行时往往都进第此个入口。

    }
  } catch (...) {
    return false;
  }
  // check global_pose timeout
  if (current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_) {

最新tf距现在超过transform_tolerance_(0.5)秒，而机器人是动的，认为tf已不能反应当前位姿，让此次getRobotPose失败。

    return false;
  }

  return true;
}

六、Buffer::checkAndErrorDedicatedThreadPresent

checkAndErrorDedicatedThreadPresent的作用是判断该TransformListener订阅“/tf”、“/static_tf”时，是否新建一个线程，然后使用了一个独立的CallbackQueue。

bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const
{
  if (isUsingDedicatedThread())
    return true;
  
  if (error_str)
    *error_str = tf2_ros::threading_error;
 
  ROS_ERROR("%s", tf2_ros::threading_error.c_str());
  return false;
}

tf在判断两坐标系是否可转换时（Buffer::canTransform），会先调用它进行判断，是false的话，即没有专门线程、专门CallbackQueue接收tf话题，就强制认为不可转换。为什么？

假设没有专门线程（using_dedicated_thread_=false）。node主线程调用了Buffer::canTransform，为判断是否可转换须要接收tf话题的新msg，因为接收tf msg不在新线程，那只有一种方法，node主线程主动调用cbqueue.callAvailable，可这时要是收到的不是/tf、/tf_static的msg，那该怎么办？举个例子，处理那个话题msg回调中又调用了Buffer::canTransform，会造成递归调用cbqueue.callAvailable，造成不可预料后果。

为此需要有专门线程，并且在那线程只接收/tf、/tf_static。

七、TransformAccum::accum、TransformAccum::finalize。一个lookupTransform示例

TransformAccum::accum在获取tf时执行。“accum”是“accumulate”缩写，字意是累积，accum作用是计算多阶段的合tf。lookupTransform时，只要source_frame、target_frame不一样，至少会调用一次accum，如果出现多阶段，那会多次调用。

TransformAccum::finalize在获取tf时执行。作用是把最终tf赋值给result_vec（平移）、result_quat（旋转）。有了这个结果后，对照看到上的BufferCore::lookupTransform，最后会调用“transformTF2ToMsg(accum.result_quat, accum.result_vec, ...)，result_vec、result_quat将做为得到的tf返回给调用者。不管是单阶段，还是多阶段，一次lookupTransform时，finilize只会调用一次。

<libros>/geometry2/tf2/src/buffer_core.cpp
------
struct TransformAccum
{
  void accum(bool source)
  {
    if (source)
    {
      source_to_top_vec = quatRotate(st.rotation_, source_to_top_vec) + st.translation_;
      source_to_top_quat = st.rotation_ * source_to_top_quat;

进这个入口，意味着此个从tf树中新搜到的tf（值st），是挨着source端找到的。

这两条语句进行一次位姿变换，可认为等价“tf2::doTransform(source_to_top, source_to_top, st)”，第一个source_to_top是转换前的位姿，第二个source_to_top是左乘st变换后，得到的位姿。

    }
    else
    {
      target_to_top_vec = quatRotate(st.rotation_, target_to_top_vec) + st.translation_;
      target_to_top_quat = st.rotation_ * target_to_top_quat;
    }
  }

  void finalize(WalkEnding end, ros::Time _time)
  {
    switch (end)
    {
    case Identity:
      break;
    case TargetParentOfSource:
      result_vec = source_to_top_vec;
      result_quat = source_to_top_quat;

下面示例会进这个入口。它就是将accum(source)计算出的结果赋值给result_vec（平移）、result_quat（旋转）。有了这个结果后，对照看到上的BufferCore::lookupTransform，最后会以result_vec、result_quat调用“transformTF2ToMsg(accum.result_quat, accum.result_vec, ...)。

      break;
    ......
    };

    time = _time;
  }
}

一个多关节机械臂。

关节	origin.xyz	origin.rpy	axis.xyz	parent	child（关节坐标系）
joint2	0.0005 0.0227 0.0315	0 0 0	0 -1 0	link1	link2
joint3	0 0.001 0.105	0 0 0	0 1 0	link2	link3
joint4	0.0010378 -0.0015 0.0975	0 0 0	0 1 0	link3	link4
joint5	-0.027 -0.0222 0.0432	0 0 0	0 0 -1	link4	link5

为直观，从上表机械臂各关节坐标系为例，看计算link5到link1的tf，是怎么个过程。这过程要考虑joint5、joint4、joint3、joint2这4个tf。

geometry_msgs::TransformStamped transform = tf2_buffer.lookupTransform("link1", "link5", ros::Time());

7.1 初始时

source_to_top_vec: (0, 0, 0, 0)
source_to_top_quat: (0, 0, 0, 1)

source_to_top_vec对应tf中平移部分，因为只有x、y、z，第四个分量总是0。source_to_top_quat对应tf中的旋转部分，是个四元数。

7.2 第一轮accum。st是joint5

st.translation: (-0.027 -0.0222 0.0432)
st.rotation_: (0, 0, 0, 1)

st是此次从tf树中新搜到的tf。因为旋转是0，结果就是平移相加。计算后。

source_to_top_vec: (-0.027 -0.0222 0.0432, 0)
source_to_top_quat: (0, 0, 0, 1)

7.3 第二轮accum。st是joint4

st.translation(0.0010378 -0.0015 0.0975)
st.rotation_(0, 0, 0, 1)

计算后

source_to_top_vec(-0.0259622 -0.0237 0.1407, 0)
source_to_top_quat(0, 0, 0, 1)

7.4 第三轮accum。st是joint3

st.translation(0 0.001 0.105)
st.rotation_(0, 0, 0, 1)

计算后

source_to_top_vec(-0.0259622 -0.0227 0.2457, 0)
source_to_top_quat(0, 0, 0, 1)

7.5 第四轮accum。st是joint2

st.translation(0.0005 0.0227 0.0315)
st.rotation_(0, 0, 0, 1)

计算后

source_to_top_vec(-0.0254622 0 0.2772, 0)
source_to_top_quat(0, 0, 0, 1)

7.6 f.finalize(TargetParentOfSource, time)

result_vec = source_to_top_vec;
result_quat = source_to_top_quat;

于是

result_vec: (-0.0254622 0 0.2772)
result_quat: (0, 0, 0, 1)

有了这个结果后，对照看到上的BufferCore::lookupTransform，它最后会调用“transformTF2ToMsg(accum.result_quat, accum.result_vec, ...)作为得到的tf返回给调用者。

TF坐标变换

全部评论: 0 条