ROS学习——tf坐标系统

一、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 
#include 
#include 

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(
      "/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 
#include 
#include 
#include 

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();
}

备注：一个机器人系统（一个工作空间）一般来说只能有一棵坐标树，即所有坐标子叶都要与世界坐标系直接或者间接连接，若设置了两棵坐标树，系统只会警告而不会报错。

 

参考

http://wiki.ros.org/tf2/Tutorials