ROS学习—

一、TF2简介

Since ROS Hydro, tf has been “deprecated” in favor of tf2. tf2 is an iteration on tf providing generally the same feature set more efficiently. As well as adding a few new features.

tf2是TF1的新版本，tf2包分为tf2和tf2_ros，前者用来进行坐标变换等具体操作，后者负责与ROS消息打交道。

二、TF2使用

2.1 broadcaster

2.1.1 静态

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <cstdio>
#include <tf2/LinearMath/Quaternion.h>std::string static_turtle_name;int main(int argc, char **argv)
{ros::init(argc,argv, "my_static_tf2_broadcaster");if(argc != 8){ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");return -1;}if(strcmp(argv[1],"world")==0){ROS_ERROR("Your static turtle name cannot be 'world'");return -1;}static_turtle_name = argv[1];static tf2_ros::StaticTransformBroadcaster static_broadcaster;geometry_msgs::TransformStamped static_transformStamped;static_transformStamped.header.stamp = ros::Time::now();static_transformStamped.header.frame_id = "world";static_transformStamped.child_frame_id = static_turtle_name;static_transformStamped.transform.translation.x = atof(argv[2]);static_transformStamped.transform.translation.y = atof(argv[3]);static_transformStamped.transform.translation.z = atof(argv[4]);tf2::Quaternion quat;quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));static_transformStamped.transform.rotation.x = quat.x();static_transformStamped.transform.rotation.y = quat.y();static_transformStamped.transform.rotation.z = quat.z();static_transformStamped.transform.rotation.w = quat.w();static_broadcaster.sendTransform(static_transformStamped);ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());ros::spin();return 0;
};

2.1.2 动态

广播器为动态广播器，通过节点接收坐标转换关系信息，然后通过广播器发布。

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>geometry_msgs::TransformStamped transformStamped;void dataCallback(const geometry_msgs::Twist::ConstPtr &msg) {transformStamped.header.frame_id = "origin_base";transformStamped.child_frame_id = "trans_base";transformStamped.transform.translation.x = msg->linear.x;transformStamped.transform.translation.y = msg->linear.y;transformStamped.transform.translation.z = msg->linear.z;tf2::Quaternion q;q.setRPY(0, 0, 0);transformStamped.transform.rotation.x = q.x();transformStamped.transform.rotation.y = q.y();transformStamped.transform.rotation.z = q.z();transformStamped.transform.rotation.w = q.w();
}int main(int argc, char **argv) {ros::init(argc, argv, "tf_broadcaster");ros::NodeHandle n;//转换信息订阅器ros::Subscriber sub = n.subscribe<geometry_msgs::Twist>("/transform", 10, dataCallback);//坐标转换广播static tf2_ros::TransformBroadcaster broadcaster;//初始转换参数transformStamped.header.frame_id = "origin_base";transformStamped.child_frame_id = "trans_base";transformStamped.transform.translation.x = 0;transformStamped.transform.translation.y = 0;transformStamped.transform.translation.z = 0;tf2::Quaternion q;q.setRPY(0, 0, 0);transformStamped.transform.rotation.x = q.x();transformStamped.transform.rotation.y = q.y();transformStamped.transform.rotation.z = q.z();transformStamped.transform.rotation.w = q.w();broadcaster.sendTransform(transformStamped);ros::Rate r(10);//广播坐标转换关系while (n.ok()) {transformStamped.header.stamp = ros::Time::now();broadcaster.sendTransform(transformStamped);r.sleep();ros::spinOnce();}
}

2.2 listener

接收器监视tf树整体，通过调用tf2_ros::TransformListener获取相关坐标系之间的坐标转换关系，实现坐标转换。

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>void transformPoint(const tf2_ros::Buffer &buffer)
{//设置原本坐标geometry_msgs::PointStamped base_point;base_point.header.frame_id = "base";base_point.header.stamp = ros::Time::now();base_point.point.x = 0.0;base_point.point.y = 0.0;base_point.point.z = 0.0;//坐标转换，显示转换后的坐标try{geometry_msgs::PointStamped world_point;// 方式一：坐标变换关系后，再调用doTransform变换// geometry_msgs::TransformStamped base_to_world;// base_to_world = buffer.lookupTransform("world", "base", past, ros::Duration(1.0));// tf2::doTransform(base_point, world_point, base_to_world);// 方式二：直接变换buffer.transform(base_point, world_point, "world");ROS_INFO("base_point: (%.2f, %.2f. %.2f) -----> world_point: (%.2f, %.2f, ""%.2f) at time %.2f",base_point.point.x, base_point.point.y, base_point.point.z, world_point.point.x, world_point.point.y,world_point.point.z, world_point.header.stamp.toSec());}catch (tf2::TransformException &ex){ROS_ERROR("Received an exception trying to transform a point from ""\"base_point\" to \"world_point\": %s",ex.what());}
}int main(int argc, char **argv)
{ros::init(argc, argv, "tf_listener");ros::NodeHandle n;// 设置接收器tf2_ros::Buffer tfBuffer;tf2_ros::TransformListener tfListener(tfBuffer);// 定时获取坐标变换ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(tfBuffer)));ros::spin();
}

2.3 获取历史变换

坐标树随着时间不端变化，但tf2会存储一段时间的快照（默认最大为10s），因此我们可以获取特定时间的坐标变换信息。

直接获取历史坐标变换结果

void transformPoint(const tf2_ros::Buffer &buffer)
{//设置原本坐标geometry_msgs::PointStamped base_point;base_point.header.frame_id = "base";base_point.header.stamp = ros::Time::now()- ros::Duration(1.0);base_point.point.x = 0.0;base_point.point.y = 0.0;base_point.point.z = 0.0;//坐标转换，显示转换后的坐标try{geometry_msgs::PointStamped world_point;//获取变换，计算base的点在world坐标系下的位置buffer.transform(base_point, world_point, "world");ROS_INFO("base_point: (%.2f, %.2f. %.2f) -----> world_point: (%.2f, %.2f, ""%.2f) at time %.2f",base_point.point.x, base_point.point.y, base_point.point.z, world_point.point.x, world_point.point.y,world_point.point.z, world_point.header.stamp.toSec());}catch (tf2::TransformException &ex){ROS_ERROR("Received an exception trying to transform a point from ""\"base_point\" to \"world_point\": %s",ex.what());}
}

获取历史变换

此处通过lookupTransform获取变换关系，在调用doTransform进行变换时忽略点的frame和stamp信息，以TransformStamped为准。

ros::Time past = ros::Time::now() - ros::Duration(1.0);
ros::Time now = ros::Time::now();// 1. past的turtle1相对past的turtle2的坐标变换
geometry_msgs::TransformStamped turtle1_to_turtle2 = buffer.lookupTransform("turtle2", "turtle1", past, ros::Duration(1.0));// 2. past的turtle1相对now的turtle2的坐标变换
geometry_msgs::TransformStamped turtle1_to_turtle2 = buffer.lookupTransform("turtle2", now, "turtle1", past, ros::Duration(1.0));// 使用变换进行坐标转换，计算turtle1的点在turtle2坐标系上的位置
tf2::doTransform(turtle1_point, turtle2_point, turtle1_to_turtle2);

2.4 tf2_geometry_msgs

tf2的数据格式带有更好的封装，可以获取原点，四元数，逆矩阵和插补等，在计算时更友好。

tf2为独立包，其数据格式可以通过tf2_geometry_msgs与ros消息进行转换。

void tf2::fromMsg(msg,out)：将ros消息msg转换到对应的tf数据格式out
msg tf2::toMsg(in)：将tf数据格式in转换到对应的ros消息msg

ros message type	tf2 datatype
geometry_msgs::Quaternion	tf2::Quaternion
geometry_msgs::QuaternionStamped	tf2::Stamped<tf2::Quaternion>
geometry_msgs::Transform	tf2::Transform
geometry_msgs::Pose	tf2::Transform
geometry_msgs::Vector3	tf2::Vector3
geometry_msgs::Vector3Stamped	tf2::Stamped<tf2::Vector3>
geometry_msgs::Point	tf2::Vector3
geometry_msgs::PointStamped	tf2::Stamped<tf2::Vector3>

2.5 旋转变换

欧拉角->四元数

  tf2::Quaternion q;q.setRPY(0, 0, 0);transformStamped.transform.rotation.x = q.x();transformStamped.transform.rotation.y = q.y();transformStamped.transform.rotation.z = q.z();transformStamped.transform.rotation.w = q.w();

四元数->欧拉角

tf2::Quaternion quat(x,y,z,w);/*** @brief Return the yaw, pitch, roll of anything that can be converted to a tf2::Quaternion The conventions are the usual ROS ones defined in tf2/LineMath/Matrix3x3.h* * @param q tf2::Quaternion* @param yaw yaw* @param pitch pitch* @param roll roll*/
void tf2::impl::getEulerYPR (const tf2::Quaternion &q, double &yaw, double &pitch,double &roll )

3.6 MessageFilter

tf2_ros::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（进入回调函数）。

tf2_ros::MessageFilter的初始化需要message_filters::Subscriber，tf转换，目标坐标系，等待时间。当message_filters::Subscriber的消息能够由tf转换到目标坐标系时，调用回调函数，回调函数由tf::MessageFilter::registerCallback()进行注册。

参考

tf2/Tutorials

ROS学习——tf2相关推荐

ROS学习笔记之小乌龟跟随
ROS学习笔记之小乌龟跟随说明:整个案例是跟着赵虚左老师的视频和文档资料学习的,特此感谢赵虚左老师和Autolabor官方文档地址视频地址学习案例之前的预备知识:TF坐标变换大体实现流程: ...
ROS 学习笔记坐标变换
TF坐标变换 TF:TransForm Frame,坐标变换. 坐标系:ROS 中是通过坐标系统开标定物体的,确切的将是通过右手坐标系来标定的. 注:在ROS中坐标变换最初对应的是tf,不过在 hyd ...
ROS学习汇总(3D摄像头）
1.ros-摄像头usb_cam以及rviz显示 https://developer.orbbec.com.cn/forum_plate_module_details.html?id=1461 2.R ...
ROS学习手记 - 2.1: Create and Build ROS Package 生成包（Python）
ROS学习手记 - 2.1: Create and Build ROS Package 生成包(Python) ROS学习手记 - 2.1: Create and Build ROS Package ...
ROS学习：创建机器人的urdf
ROS学习之路08:创建机器人的urdf(xacro)模型并通过rviz可视化_Hi, Robotics-CSDN博客_urdf可视化 1 创建工作空间 mkdir -p ~/catkin_ws/sr ...
ROS学习之日志消息
ROS学习之日志消息 ROS日志系统的核心思想,就是使程序生成一些简短的文本字符流,这些字符流便是日志消息. 0.1严重级别 ROS中,日志消息分为五个不同的严重级别,也可简称为严重性或者级别.按照严 ...
ROS学习总结一ROS组织框架与几个关键词
本人一个ROS学习初学者,在经过三天的学习基础教程,对ROS有了一个基本的了解,在此,笔者就自己的学习的理解做一个总结,如有错误还请各位指出. 一.组织框架 catkin_ws() --build - ...
ROS 学习笔记（三）：自定义服务数据srv+server+client 示例运行
ROS 学习笔记(三):自定义服务数据srv+Server+Client 示例运行一.自定义服务数据: 1.向功能包添加自定义服务文件(AddTwoInts.srv) cd ~/catkin_ws/ ...
ROS 学习笔记（二）：自定义消息msg+Publisher+Subscriber 示例运行
ROS 学习笔记(二):自定义消息msg+Publisher+Subscriber 示例运行一.自定义消息: 1.新建msg文件夹,创建定义Person.msg 文件 mkdir -p ~/catk ...

ROS学习——tf2