ROS中使用TF变换

TF消息类型

geometry_msgs/TransformStamped[] transforms
    std_msgs/Header header
            uint32 seq
            time stamp
            string frame_id
    string child_frame_id
    geometry_msgs/Transform transform
            geometry_msgs/Vector3 translation
                    float64 x
                    float64 y
                    float64 z
            geometry_msgs/Quaternion rotation
                    float64 x
                    float64 y
                    flaot64 z
                    float64 w

接收和发布TF

tf::TransformBroadcaster类（广播器）

TransformBroadcaster()
void sendTransform(const StampedTransform &transform)
void sendTransform(const std::vector<StampedTransform> &transforms)
void sendTransform(const geometry_msgs::TransformStamped &transform)
void sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms)

Exp:

br.sendTransform(tf::StampedTransform(①, ②, ③, ④));
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), “map”, ”base_link“));

①存储变换关系的变量

②广播tf使用的时间戳

③ 父坐标系的名称

④ 子坐标系的名称。

tf::TransformListener类（监听器）

void lookupTransform(const std::string &target_frame,const std::string &source_frame, const ros::Time &time,StampedTransform &transform)
bool canTransform()
bool waitForTransform() const 

Exp:
listener.lookupTransform(①, ②, ③, ④);

try
{
    listener.lookupTransform("base_link", "map",ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
    continue;
}

监听从①坐标系到②坐标系的转换

ros::Time(0)指最近时刻存储的数据

转换关系存入变量 tf::StampedTransform transform

TF一些操作函数

创建TF变换

tf::Transform transform;

transform.setOrigin( tf::Vector3(x, y, z) );
tf::Quaternion q;
q.setRPY(0, 0, theta);
transform.setRotation(q);

使用TF变换

transform.getOrigin().x()
transform.getOrigin().y()
transform.getRotation().getW();
transform.getRotation().getX();
transform.getRotation().getY();
transform.getRotation().getZ();

TF等待

listener.waitForTransform(①, ②, ros::Time(0), ros::Duration(10.0));

监听从①坐标系到②坐标系的转换

ros::Time(0)指最近时刻存储的数据

ros::Duration(10.0)指十秒内的缓冲区

一个坐标系转换到另一个坐标系

listener.transformPoint(①, ②, ③)；
listener.transformPose(①, ②, ③);
......

① 要转换到的Frame ID

② 待转换的pose

③ 转换后的pose

Exp:

#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
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);
}

tf::Stamped< tf::Pose >转换到 geometry_msgs::Pose

static void tf::poseTFToMsg (const Pose &bt,geometry_msgs::Pose &msg)