如何使用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"的回调函数,就能看到效果了:)

你可能感兴趣的:(ROS,无人机-a-个人笔记)