关于ros --tf2的使用

在tf2系统中，将包分为tf2和tf2_ros。

tf2用来进行坐标旋转，以及tf、msg两种四元素数据结构的变换；

tf2_ros负责与ROS数据通信打交道，负责发布tf或订阅tf，即发布者和订阅者是在tf2_ros命名空间下的。

1、tf2 (姿态变换和tf格式转换)
1.1 四元数的组成
一个四元素有4个成员（x,y,z,w）。不绕xyz轴旋转的常用单位四元数为（0,0,0,1)。

注意： w 是最后一个，但是一些库像 Eigen 把 w 放在第一的位置。

#include <tf2/LinearMath/Quaternion.h>
tf2::Quaternion q;
q.setRPY(0,0,0);//把绕x、y、z轴旋转的角度转化为 四元数
//q.x()==0;
//q.y()==0;
//q.w()==0;
//q.w()==1;

四元数的大小应为1。如果数值错误导致四元数的大小不为1，ROS将显示警告。为避免这些警告，需要对四元数进行标准化：

q.normalize();

1.2 ROS中四元素的数据类型
ROS 有两种四元素数据类型：msg 和 tf。

geometry_msgs::Quaternion q_msg; // msg 类型
tf2::Quaternion q_tf; // tf 类型
在tf2_geometry_msgs.h文件中提供了数据类型转换的函数:#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion q_tf;
geometry_msgs::Quaternion q_msg;
tf2::convert(q_msg, q_tf); //①msg类型转换为tf类型
tf2::fromMsg(q_msg, q_tf); //②msg类型转换为tf类型
q_msg = tf2::toMsg(q_tf);  //tf类型转换为msg类型

1.3 通过四元素做旋转
一个刚体进行旋转，求该刚体旋转后的姿态。只需要将刚体原姿态的四元数乘以旋转的四元数。注意，乘法的顺序很重要。如：

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>//q_ori  是原姿态转换的tf格式的四元数
//q_rot   旋转四元数
//q_new   旋转后的姿态四元数
tf2::Quaternion q_orig, q_rot, q_new;
// msg.pose.orientation 假设是订阅的topic 是一个刚体姿态的msg格式四元数
tf2::convert(msg.pose.orientation ,q_ori);//通过tf2::convert()将msg格式转换成tf格式double r=3.14, p=0, y=0;// 设置绕x轴旋转180度
q_rot.setRPY(r, p, y);//求tf格式的旋转四元数
q_new = q_rot*q_ori;  //旋转后的姿态 = 旋转的四元素* 原姿态
q_new.normalize(); // 归一化
msg.pose.orientation = tf2::toMsg(q_new);//旋转后的姿态由tf格式转换成msg格式

(四元素的逆矩阵把w参数设成负的即可)

类似的，假如在一个坐标系下有两个姿态用四元数表示为q_1和q_2，那如何求这两个姿态的旋转四元数q_r呢。其实也可以看成q_2如何旋转为q_1:

q_2 = q_r*q_1
1
那么：q_r = q_2*q_1_inverse;//q_1_inverse是q_1的逆矩阵
1
如：q1_inv[0] = q1.pose.orientation.x;
q1_inv[1] = q1.pose.orientation.y;
q1_inv[2] = q1.pose.orientation.z;
q1_inv[3] = -q1.pose.orientation.w; //注意这个负号q2[0] = c_pose.pose.orientation.x;
q2[1] = c_pose.pose.orientation.y;
q2[2] = c_pose.pose.orientation.z;
q2[3] = c_pose.pose.orientation.w;q_r = tf.transformations.quaternion_multiply(q2, q1_inv);

2、tf2_ros (tf广播和监听)
ROS官网tf广播和监听教程

2.1、消息格式
ROS中广播和监听的tf2消息类型是tf2_msgs::TFMessage，其本质是：

geometry_msgs/TransformStamped类型的向量形式
geometry_msgs/TransformStamped[] transformsstd_msgs/Header headeruint32 seqtime stampstring frame_idstring child_frame_idgeometry_msgs/Transform transformgeometry_msgs/Vector3 translationfloat64 xfloat64 yfloat64 zgeometry_msgs/Quaternion rotationfloat64 xfloat64 yfloat64 zfloat64 w

2.2 、广播tf2

#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PointStamped.h>tf2_ros::TransformBroadcaster br;//TransformBroadcastergeometry_msgs::TransformStamped transformStamped;transformStamped.header.stamp = ros::Time::now();transformStamped.header.frame_id = "world";transformStamped.child_frame_id = turtle_name;transformStamped.transform.translation.x = msg->x;transformStamped.transform.translation.y = msg->y;transformStamped.transform.translation.z = 0.0;tf2::Quaternion q;q.setRPY(0, 0, msg->theta);//RPY转成tf格式的四元素transformStamped.transform.rotation.x = q.x();transformStamped.transform.rotation.y = q.y();transformStamped.transform.rotation.z = q.z();transformStamped.transform.rotation.w = q.w();br.sendTransform(transformStamped);//sendTransform()

2.3 监听tf
2.3.1 tf监听


```cpp
#include <tf/transform_listener.h>tf::TransformListener tfl;
tf::StampedTransform odom_global_tf;  // tf sub,string odom_frame_id = "odom_combined";
string global_frame_id = "map";
try {tfl.waitForTransform(odom_frame_id, global_frame_id, ros::Time(0), ros::Duration(1.0));  // child,parenttfl.lookupTransform(odom_frame_id, global1_frame_id, ros::Time(0), odom_global_tf);  // child,parent}
catch (tf::TransformException &ex) {ROS_ERROR("%s", ex.what());ros::Duration(1.0).sleep();}


2.3.2 tf2监听
使用tf2消息的大部分工作通过tf2_ros::Buffer类完成，其通过tf2_ros::TransformListener完成对tf2_ros::Buffer类的初始化和构造，并订阅相应tf消息，后续操作都是用的tf2_ros::Buffer类的成员函数完成：```cpp
#include <tf2_ros/transform_listener.h>//与tf监听有些区别
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PointStamped.h>
....//创建TransformListener对象。一旦创建了侦听器，它就开始通过连接接收tf2转换，并对它们进行长达10秒的缓冲tf2_ros::Buffer tfBuffer;tf2_ros::TransformListener tfListener(tfBuffer);ros::Rate rate(10.0);while (node.ok()){geometry_msgs::TransformStamped transformStamped;try{transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",ros::Time(0), ros::Duration(3.0));//lookupTransform(“target frame”, “source frame”,ros::Time(0));第三个参数ros::Time(0)代表从buffer中获取“最新可获取时间”的变换。 每一个listener都有一个buffer储存来自来自不同tf2广播者的坐标系变换关系。这些变换进入缓冲区需要一段很小的时间，所以第三个参数不应该为ros::Time::now(),一般使用ros::Time(0)就很好。geometry_msgs::PointStamped world, velo_link;tfBuffer.transform<geometry_msgs::PointStamped>(world, velo_link, "velo_link", ros::Duration(1.0));//transform}catch (tf2::TransformException &ex) {ROS_WARN("%s",ex.what());ros::Duration(1.0).sleep();continue;}}

2.4、tf相关工具命令
2.4.1 查看两个frame之间的变换关系

rosrun tf tf_echo source_frame(参考) target_frame

查看target_frame在source_frame下的位姿。

（source_frame是parent，target_frame是child）

2.4.2 根据当前的tf树创建一个pdf图

rosrun tf view_frames

这个工具首先订阅 /tf ，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图

2.4.3 查看当前的tf树

rosrun rqt_tf_tree rqt_tf_tree

2.5、tf2_ros::MessageFilter
使用tf2_ros::MessageFilter去处理待有时间戳的数据类型。

private：message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;tf2_ros::MessageFilter<geometry_msgs::PointStamped> tf2_filter_;
public：tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0){point_sub_.subscribe(n_, "turtle_point_stamped", 10);tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );}

MessageFilter将订阅任何带有报头的ros消息，并对其进行缓存，直到能够确保将其转换为target_frame坐标系下为止，然后调用回调函数，将数据转换到taget_frame坐标系下。

3、tf的C++数据结构
基本的数据类型有：

QuaternionVectorPointPoseTransform

其中Quaternion 表示四元数，vector3是一个3*1 的向量，point表示一个点坐标，Pose是位姿（包括点坐标以及方向），Transform是一个转换的模版。

3.1 tf::Stamped
tf::Stamped 是一种包含除了Transform的其他几种基本的数据结构的一种数据结构:

template <typename T>    //模版结构可以是tf::Pose tf:Point 这些基本的结构
class Stamped : public T{public:ros::Time stamp_;    //记录时间std::string frame_id_;   //IDStamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocationStamped(const T& input, const ros::Time& timestamp, const std::string & frame_id);void setData(const T& input);
};

3.2 tf::StampedTransform
TF::stampedtransform是TF的一种特殊情况：它需要frame_id和stamp以及child_frame_id。

/** \brief The Stamped Transform datatype used by tf */
class StampedTransform : public tf::Transform
{public:ros::Time stamp_; // 时间戳std::string frame_id_; //定义转换坐标框架的frameID，parentstd::string child_frame_id_; ///的坐标系变换定义的id ，childStampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };/** \brief Default constructor only to be used for preallocation */StampedTransform() { };/** \brief Set the inherited Traonsform data */void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};
};

3.3 tf::Pose/tf::StampedTransform获取位姿元素

tf::Pose odom;
//或tf::StampedTransform odom;
odom.getOrigin().x();
odom.getOrigin().y();
tf::getYaw(odom.getRotation());
//四元素
odom.getRotation().getW();
odom.getRotation().getX();
odom.getRotation().getY();
odom.getRotation().getZ();odom.setOrigin( tf::Vector3(msg->x, msg->y, 0.0));
odom.setRotation( tf::Quaternion(0, 0, 0, 1) );//源码
namespace tf{typedef tf::Transform Pose;
}

官网

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

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。

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

关于ros --tf2的使用相关推荐

ROS TF2 中的四元数基础部分
ROS TF2 中的四元数基础部分 1.四元数的组成 2.将 RPY坐标系下的角度转换为四元数 3.如何通过四元数做旋转 4.四元数转置 5.求两个姿态(四元数)的旋转 5. 完毕这 ...
ROS TF2静态坐标发布
ROS TF2静态坐标发布安装 CPP方式 launch文件方式利用终端的方式实现我们经常需要进行坐标的变换,往往很多时候需要发布一个固定的坐标系转换例如从ENU转换到NED.对于这种转换是固定 ...
ROS————tf2介绍及教程
本文参考资料: tf2 - ROS Wikihttp://wiki.ros.org/tf2 目录前言一.tf2的作用以及为什么要使用tf2 1.安装demo 2.运行demo 3.上面的例子代表着 ...
idea 到阿伯快捷键_阿伯泰邓迪大学
idea 到阿伯快捷键 Yesterday I made the trip by train up to Dundee to speak to the students at the Universi ...
ROS专题----tf和tf2坐标变换
ROS专题----tf和tf2坐标变换 ---- 工作区设置如果您尚未创建用于完成教程的工作区, 请单击此处查看一些简要说明 . 从tf1迁移到tf2 转换数据类型这是对转换数据类型的语法更改的快 ...
ROS之tf2坐标转换包
参考资源: tf2资源: 官网:http://wiki.ros.org/tf2 http://wiki.ros.org/tf2/Tutorials API: http://docs.ros.org/j ...
ROS学习——tf2
一.TF2简介 Since ROS Hydro, tf has been "deprecated" in favor of tf2. tf2 is an iteration on ...
ros进阶--tf2的使用
在tf2系统中,将包分为tf2和tf2_ros. tf2用来进行坐标旋转,以及tf.msg两种四元素数据结构的变换: tf2_ros负责与ROS数据通信打交道,负责发布tf或订阅tf,即发布者和订阅者 ...
install ros indigo tf2
sudo apt install ros-indigo-tf2 转载于:https://www.cnblogs.com/sea-stream/p/10060327.html

关于ros --tf2的使用

关于ros --tf2的使用相关推荐

最新文章

热门文章