版权声明:本文为博主原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
一、tf简介
坐标转换包TF是一个让用户可以实时跟踪多个参考系的功能包,它使用的是树型数据结构,可以根据时间缓冲实时维护多个参考系之间的坐标变换关系,如下图所示。通过TF用户可以在任意时间将点、向量等数据的坐标,完成在两个参考系之间的变换。
二、tf辅助工具
(1)采用tf_monitor,查看当前TF树中所有坐标系的发布状态。
rosrun tf tf_monitor
(2)采用rqt_tf_tree,查看当前所有坐标之间的变换关系,可通过刷新更新当前树的内容。
rosrun rqt_tf_tree rqt_tf_tree
(3)采用tf_echo,获取reference_frame和target_frame之间的坐标变换关系。
rosrun tf tf_echo [reference_frame] [target_frame]
三、tf广播器和接受器
(1)广播器
广播器为动态广播器,通过节点接收坐标转换关系信息,然后通过广播器发布。
#include <geometry_msgs/Twist.h>
#include <ros/ros.h>
#include <tf/transform_broadcaster.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;
Eigen::Quaterniond q =
euler2Quaternion(msg->angular.x, msg->angular.y, msg->angular.z);
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);
//坐标转换广播
tf::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;
transformStamped.transform.rotation.x = 0;
transformStamped.transform.rotation.y = 0;
transformStamped.transform.rotation.z = 0;
transformStamped.transform.rotation.w = 1;
broadcaster.sendTransform(transformStamped);
ros::Rate r(10);
//广播坐标转换关系
while (n.ok()) {
transformStamped.header.stamp = ros::Time::now();
broadcaster.sendTransform(transformStamped);
r.sleep();
ros::spinOnce();
}
}
(2)接收器
接收器监视tf树整体,通过调用tf::TransformListener获取相关坐标系之间的坐标转换关系,实现坐标转换。
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Pose.h>
#include <ros/ros.h>
#include <tf/transform_listener.h>
void transformPoint(const tf::TransformListener &listener) {
//设置原本坐标
geometry_msgs::PointStamped origin_point;
origin_point.header.frame_id = "origin_base";
origin_point.header.stamp = ros::Time();
origin_point.point.x = 1.0;
origin_point.point.y = 0.2;
origin_point.point.z = 0.0;
//坐标转换,显示转换后的坐标
try {
geometry_msgs::PointStamped trans_point;
listener.transformPoint("trans_base", origin_point, trans_point);
ROS_INFO(
"origin_point: (%.2f, %.2f. %.2f) -----> trans_point: (%.2f, %.2f, "
"%.2f) at time %.2f",
origin_point.point.x, origin_point.point.y, origin_point.point.z,
trans_point.point.x, trans_point.point.y, trans_point.point.z,
origin_point.header.stamp.toSec());
} catch (tf::TransformException &ex) {
ROS_ERROR("Received an exception trying to transform a point from "
"\"origin_base\" to \"trans_base\": %s",
ex.what());
}
}
int main(int argc, char **argv) {
ros::init(argc, argv, "tf_listener");
ros::NodeHandle n;
//设置接收器
tf::TransformListener listener(ros::Duration(10));
ros::Timer timer = n.createTimer(
ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));
ros::spin();
}
备注:一个机器人系统(一个工作空间)一般来说只能有一棵坐标树,即所有坐标子叶都要与世界坐标系直接或者间接连接,若设置了两棵坐标树,系统只会警告而不会报错。
参考