• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

ROS学习——tf2

人工智能 white_Learner 2092次浏览 0个评论

一、TF2简介

Since ROS Hydrotf 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进行变换时忽略点的framestamp信息,以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学习——tf2  

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 )

 

参考

tf2/Tutorials  


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS学习——tf2
喜欢 (0)

您必须 登录 才能发表评论!

加载中……