ROS之tf2坐标转换包

seven_ • 2023年3月3日上午9:34 • 技术随笔 • 阅读 241

参考资源：

tf2资源：

官网：http://wiki.ros.org/tf2
http://wiki.ros.org/tf2/Tutorials
API: http://docs.ros.org/jade/api/tf2/html/index.html

在tf2系统中，将包分为tf2和tf2_ros，前者用来进行坐标变换等具体操作，tf2_ros则负责与ROS消息打交道，负责发布tf或订阅tf，即发布者和订阅者是在tf2_ros命名空间下的。
ROS中广播和监听的tf2消息类型是tf2_msgs::TFMessage，其本质是

geometry_msgs/TransformStamped类型的向量形式
geometry_msgs/TransformStamped[] transforms
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

tf2_ros wiki：http://wiki.ros.org/tf2_ros?distro=melodic
tf2_ros API: http://docs.ros.org/kinetic/api/tf2_ros/html/c++/

广播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>

static tf2_ros::TransformBroadcaster br;
  geometry_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);
  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);

监听tf2：

使用tf2消息的大部分工作通过tf2_ros::Buffer类完成，其通过tf2_ros::TransformListener完成对tf2_ros::Buffer类的初始化和构造，并订阅相应tf消息，后续操作都是用的tf2_ros::Buffer类的成员函数完成：

#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::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));
      geometry_msgs::PointStamped world, velo_link;
      tfBuffer.transform<geometry_msgs::PointStamped>(world, velo_link, "velo_link", ros::Duration(1.0));
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
 }