如何使用tf::transform进行简单的不同frame间的pose转换

摘自：https://my.oschina.net/u/4288583/blog/3413736

如何使用tf::transform进行简单的不同frame间的pose转换

osc_22to9se2

2019/08/27 15:57

阅读数 315

tf转换，分为两部分：broadcaster和listener。前者是tf的发布者，后者是接收者。我们如果要建立一个完整的tf体系，需要自己先生成tf信息用broadcaster发布出去，然后再在需要tf信息的节点使用listner接收tf信息后进行转换。

但是大部分情况下，我们都是在一个已经有完善体系的框架下工作，并不需要自己管理tf。这时我们只需要了解怎样用listner接收tf进行pose转换就可以了。本文即是简述这种方法。

首先，要添加头文件。listner的定义在这个文件中：

#include 

然后是listener的变量定义：

tf::TransformListener listener;

下来就是核心函数了，核心函数的定义是这样的：

void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const

参数名字的定义对功能的说明还是很明显的，target_frame就是你要把源pose转换成哪个frame上的pose。假如你的源pose的frame_id是"odom"，你想转到"map"上，那么target_frame写成“map”就可以了。stamped_in就是源pose，而stamped_out就是目标数据了，也就是转换完成的数据。需要注意的是，从参数上来看，转换时是不需要指定源frame_id的，这是因为它已经包含在了stamped_in中，换句话说，就是这个函数一个隐含的使用条件是，stamped_in中必须指明源pose属于哪个frame。

那么最后就是准备源数据变量和目标数据变量了，如下：

geometry_msgs::PoseStamped pose_odom;
pose_odom.header = odom->header;
pose_odom.pose = odom->pose.pose;

geometry_msgs::PoseStamped pose_map;

这里是要把odom上的数据转换到map上，所以源变量就是pose_odom，而目标变量就是pose_map。

以上这些都是局部代码片断，都只能看个大概意思，这回我们把它们都连在一起形成一个完整的程序来看看意思：

#include 
#include 
#include void odomReceived(const nav_msgs::Odometry::ConstPtr& odom)
{
    // transform from "odom" to "map"
    tf::TransformListener listener;

    geometry_msgs::PoseStamped pose_odom;
    pose_odom.header = odom->header;
    pose_odom.pose = odom->pose.pose;

    geometry_msgs::PoseStamped pose_map;

    try{
        listener.transformPose("map", pose_odom, pose_map);
    }
    catch( tf::TransformException ex)
    {
        ROS_WARN("transfrom exception : %s",ex.what());
        return;
    }

    ROS_INFO_STREAM(pose_map);
}

好了，将这段程序嵌入进你的代码中，作为subscribe topic "odom"的回调函数，就能看到效果了:)