ROS TF坐标变换库 & 重写创建高频坐标变换广播器

引言

​ 坐标变换是机器人学中的基础概念，机器人中每个关节的运动，都存在相应的坐标变换关系。坐标变换可以分为位置变换和姿态变换，分别可由位置矢量和旋转矩阵表示，而这两部分可以组成4*4的齐次变换矩阵，可表示如下：
$$\begin{gathered} T=\left[\begin{array}{cc}R& P\\ 0& 1\end{array}\right] \\ R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]P=\left[\begin{array}{ccc}{p}_x& p_y& p_z\end{array}\right] \end{gathered}$$
​ 在ROS中坐标系变换由坐标系变换系统TF库来实现。

简介

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

​ TF可以在分布式系统中进行操作，也就是说一个机器人系统中所有的坐标变换关系，对于所有节点组件都是可用的，所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据。

TF库使用步骤

1. 监听TF变换
   接收并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系。
2. 广播TF变换
   向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入TF树中，不需要再进行同步。

tf_broadcaster

​ tf_broadcaster是TF库中广播坐标变换信息的实现，我们可以看看下面官网给出的广播坐标变换的demo。

#include 
#include 
#include 

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);
  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];

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

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

要使用TransformBroadcaster，我们需要包含tf/transform_broadcaster.h头文件。

其中，最关键的一步为sendTransform，在这里才实现了坐标信息的广播。

重写创建高频坐标变换广播器

​ 由于实际的机器人开发中，实现复杂的功能往往意味着需要满足较高的实时性，而使用ros自带的tf_broadcaster不一定能够满足我们的要求，所以这里计划重写创建一个高频坐标变换广播器，首先要参考tf库中自带tf_broadcaster的实现代码transform_broadcaster.cpp· GitHub。

#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"

#include 

namespace tf {

TransformBroadcaster::TransformBroadcaster():
  tf2_broadcaster_()
{
}

void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
{
  tf2_broadcaster_.sendTransform(msgtf);
}

void TransformBroadcaster::sendTransform(const StampedTransform & transform)
{
  geometry_msgs::TransformStamped msgtf;
  transformStampedTFToMsg(transform, msgtf);
  tf2_broadcaster_.sendTransform(msgtf);
} 

void TransformBroadcaster::sendTransform(const std::vector & msgtf)
{
  tf2_broadcaster_.sendTransform(msgtf);
}

void TransformBroadcaster::sendTransform(const std::vector & transforms)
{
  std::vector msgtfs;
  for (std::vector::const_iterator it = transforms.begin(); it != transforms.end(); ++it)
  {
    geometry_msgs::TransformStamped msgtf;
    transformStampedTFToMsg(*it, msgtf);
    msgtfs.push_back(msgtf);

  }
  tf2_broadcaster_.sendTransform(msgtfs);
} 
  
}

​ 我们可以看见，tf_broadcaster的实现并不复杂，仅仅只是接收StampedTransform或geometry_msgs::TransformStamped>信息，然后通过tf2_ros::TransformBroadcaster发送。

​ 重写思路：这里我们只需要重写sendTransform函数，将tf2_ros::TransformBroadcaster发布消息修改成用realtime_tools::RealtimePublisher发布信息即可。由于在项目开发中只需要广播geometry_msgs::TransformStamped信息，所以只重写相关的函数。

实现

 	/**
     * @brief 广播发送坐标系转换消息
     *
     * @param transform 坐标转换消息
     */
    void realtime_broadcaster::sendTransform(const geometry_msgs::TransformStamped& transform)
    {
        std::vector TransformStampeds;
        TransformStampeds.push_back(transform);
        sendTransform(TransformStampeds);
    }

    void realtime_broadcaster::sendTransform(const std::vector& transforms)
    {
        tf2_msgs::TFMessage message;
        for (const auto& transform : transforms)
        {
            message.transforms.push_back(transform);
        }
        if (realtime_pub_->trylock())
        {
            realtime_pub_->msg_ = message;
            realtime_pub_->unlockAndPublish();
        }
    }

参考

transform_broadcaster.cpp· GitHub

ROS技术点滴 —— tf坐标变换库

后续

 喜欢的话可以关注一下我的公众号技术开发小圈，尤其是对深度学习以及计算机视觉有兴趣的朋友，我会把相关的源码以及更多资料发在上面，希望可以帮助到新入门的大家！