ROS-坐标变换（包括tf2和tf）

目录

• tf变换
  • 1. ros中的tf消息类型
  • 2. tf创建坐标变换(tf::Transform)，并赋值
  • 3.tf::StampedTransform 是继承自tf::Transform
  • 4.使用tf创建、发布、监听坐标变换
• 1. tf2
  • 1.1 在cpp代码中，发布静态变换
  • 1.2 launch文件中，使用static_transform_publisher进行静态变换
  • 1.3 发布坐标变换：broadcaster
  • 1.4 监听坐标变换
  • 1.5 类方式实现订阅、发布，且利用buffer.transform()实现一个点在不同frame中的坐标变换
• 3. 将tf消息转换为四元数、欧拉角(使用tf2中的方法)
  • 3.1 说明
  • 3.2 代码实例
• 4. buffer.transform()
  • 4.1 功能说明
  • 4.2 代码实例

 

---

回到顶部

tf变换

---

1. ros中的tf消息类型

• geometry_msgs/TransformStamped 带时间戳的tf变换

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

• geometry_msgs/Transform 不带时间戳的tf变换

geometry_msgs/Vector3 translation
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation
    float64 x
    float64 y
    float64 z
    float64 w

2. tf创建坐标变换(tf::Transform)，并赋值

tf::Quaternion q;    // 创建一个四元数
q.setRPY(0, 0, 3.14);  //  通过欧拉角为四元数赋值；

tf::Transform   trans;
trans.setOrigin(tf::Vector3(x,y,z))   // 参数为一个3维度矢量
trans.setRotation(q);                      // 参数为一个四元数

3.tf::StampedTransform 是继承自tf::Transform

tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name))
由于tf::StampedTransform继承自tf::Transform
所以，一般Transform中的方法，在StampedTransform中，都可以使用；

ros wiki: tf::Transform中的可操作方法

getBasis()
getOrigin()
getRotation()
inverse()
setIdentity()
setOrigin()
setRotation()
等等方法

4.使用tf创建、发布、监听坐标变换

4.1 创建不带时间戳的tf、带时间戳的tf

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv){
    ros::init(argc, argv, "my_tf_broadcaster");

    tf::Transform trans;  
    tf::Quaternion q;
    q.setRPY(30,46,0);
    trans.setRotation(q);                      // 参数为一个四元数
    trans.setOrigin(tf::Vector3(10,5,0));      // 参数为一个3维度矢量
    tf::StampedTransform stamp_trans = tf::StampedTransform(trans, ros::Time::now(), "turtle1", "carrot1");
    return 0;
}

4.2 发布坐标变换

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;            // 创建一个broadCaster
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
          // 发布transform
    rate.sleep();
  }
  return 0;
};

4.3 监听坐标变换

#include <tf/transform_listener.h>

  tf::TransformListener listener; // 创建一个tf监听；
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform; // 创建一个坐标变换对象或者消息
    try{
      listener.lookupTransform("/turtle2", "/turtle1",  ros::Time(0), transform);           
	  // 将两个坐标系的变换存在transform中
    }
    catch (tf::TransformException ex){
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }

    turtlesim::Velocity vel_msg;
    vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
                                transform.getOrigin().x());
    vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }

回到顶部

1. tf2

1.1 在cpp代码中，发布静态变换

1. 创建一个静态static_broadcaster
2. 创建一个坐标变换消息对象
3. 为这个消息对象的消息赋值
4. 使用step1中的broadcaster进行坐标变化的发布

tf2_ros::StaticTransformBroadcaster static_broadcaster; // 创建
geometry_msgs::TransformStamped static_transformStamped; // tf消息
static_broadcaster.sendTransform(static_transformStamped); // 发布坐标变换

// 重点关注tf2需要的头文件
#include <ros/ros.h>
#include <cstdio>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Quaternion.h>

// 省略其他代码
static tf2_ros::StaticTransformBroadcaster static_broadcaster;
geometry_msgs::TransformStamped static_transformStamped;

// 设置header
static_transformStamped.header.stamp = ros::Time::now();
static_transformStamped.header.frame_id = "world";
static_transformStamped.child_frame_id = static_turtle_name;

// 设置transform分量
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;
// 使用setRPY()方法，将欧拉角（roll滚转，pitch俯仰，yaw偏航）转换为四元数
quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
// 为坐标转换消息赋值，得到四元数的四个分量，为消息的rotation的四个分量赋值
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);

1.2 launch文件中，使用static_transform_publisher进行静态变换

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1" />
</launch>

1.3 发布坐标变换：broadcaster

1. 发出world和turtle_name之间的坐标变换；

static tf2_ros::TransformBroadcaster br;
// 创建一般的tfbr
geometry_msgs::TransformStamped transformStamped;
// 创建消息对象
br.sendTransform(transformStamped);
// 发布tf

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  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);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");

  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
    
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

1.4 监听坐标变换

• 注意：关于tfBuffer.lookupTransform()获取坐标变换的说明
  （1）transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",ros::Time(0));
  （2）transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",ros::Time(0), ros::Duration(3.0));
• tfBuffer.lookupTransform()的第四个参数ros::Duration(3.0)，可以设置一个超时时间，
  当获取两个frame之间的变换时，如果没有正常获取到，则会按照第四个参数，等待超时，超时之后，才会进入异常捕获流程catch{}。
  如果能够正常获取到两个坐标系之间的坐标变换，则不需要等待；
• 第三个参数 ros::Time(0)也可以使用ros::Time::now()，官方wiki说，优先使用ros::Time(0).

tf2_ros::Buffer tfBuffer;
// 创建一个buffer
tf2_ros::TransformListener tfListener(tfBuffer);
// 利用buffer创建一个监听
geometry_msgs::TransformStamped transformStamped;
// 创建一个消息，保存监听到的tf变换
try{ transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",ros::Time(0));
}
catch (tf2::TransformException &ex) {}
// 监听tf变换,捕获异常

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_listener");

  ros::NodeHandle node;

  ros::service::waitForService("spawn");
  ros::ServiceClient spawner =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn turtle;
  turtle.request.x = 4;
  turtle.request.y = 2;
  turtle.request.theta = 0;
  turtle.request.name = "turtle2";
  spawner.call(turtle);

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

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

    geometry_msgs::Twist vel_msg;

    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};

• buffer.lookupTransform的说明：
  tfbuffer.lookupTransform("turtle1", "turtle2", ros::Time(0));
  1. lookupTransform()可以从Buffer中查询两个坐标系之间的坐标变化消息；
  2. 以第一个参数的坐标系为参考坐标系，上述获取到的坐标变化是【第二个坐标系】在【第一个坐标系】中的位置，所以此处就是要获取在“turtle1”坐标系中“turtle2”的位置
  3. 如下图（左图）所示：x(红色)，y(绿色)， transform.translation.x<0; transform.translation.y>0【在turtle1中的第二象限】，绕Z轴的旋转为偏航角yaw，规定沿着逆时针旋转为正,且规定角度的范围在(-180°，180°)，所以transform.rotation(yaw)<0

1.5 类方式实现订阅、发布，且利用buffer.transform()实现一个点在不同frame中的坐标变换

/*  功能说明：turtle_tf2_listener_02.cpp
    生成一个turtle2，
    不跟随turtle1；
    重点是查看三个坐标系 /world /turtle1 /turtle2三个坐标系之间的转换关系
*/
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <tf2/LinearMath/Matrix3x3.h>

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>
#include <geometry_msgs/PoseStamped.h>

class tf2_listener_case02{
public:
    tf2_listener_case02(tf2_ros::Buffer& buffer);
    ~tf2_listener_case02(){};
private:
    // ########## 数据成员
    ros::NodeHandle node;
    tf2_ros::Buffer& tf_;
    // define publisher
    ros::Publisher turtle1_pub_geometryPose ;
    ros::Publisher turtle2_pub_geometryPose ;
    ros::Subscriber turtle1_subscriber, turtle2_subscriber;
    ros::Subscriber turtle1_sub02;
    // ros::Subscriber turtle2_subscriber = node.subscribe("/turtle2/pose", 1, turtle2_subscriberCb);

    // ########## 成员函数
    // ## 01.callback function
    void turtle1_subscriberCb(const turtlesim::Pose::ConstPtr& msg);
    void turtle2_subscriberCb(const turtlesim::Pose::ConstPtr& msg);    
    void turtle1_geometryPoseCb(const geometry_msgs::PoseStamped::ConstPtr& msg);    
    
    // ## 02.call spawn service function
    void call_spawn_service(std::string turtlename=std::string("turtle2"));
};

// 构造函数
tf2_listener_case02::tf2_listener_case02(tf2_ros::Buffer& buffer):tf_(buffer){
    // call spawn service create turtle
    call_spawn_service();    

    turtle1_pub_geometryPose = node.advertise<geometry_msgs::PoseStamped>("turtle1_geometryPose", 1);// 定义一个话题发布器
    turtle2_pub_geometryPose = node.advertise<geometry_msgs::PoseStamped>("turtle2_geometryPose", 1);// 定义一个话题发布器   
    turtle1_subscriber = node.subscribe("/turtle1/pose", 1, &tf2_listener_case02::turtle1_subscriberCb, this);
    turtle2_subscriber = node.subscribe("/turtle2/pose", 1, &tf2_listener_case02::turtle2_subscriberCb, this);
    turtle1_sub02 = node.subscribe("/turtle1_geometryPose", 1, &tf2_listener_case02::turtle1_geometryPoseCb, this);
    // turtle2_subscriber = node.subscribe("/turtle2/pose", 1, turtle2_subscriberCb);
}
// 回调函数1
void tf2_listener_case02::turtle1_subscriberCb(const turtlesim::Pose::ConstPtr& msg){
    geometry_msgs::PoseStamped geo_pose_msg;
    geo_pose_msg.header.frame_id = "world";
    geo_pose_msg.header.stamp = ros::Time::now();
    geo_pose_msg.pose.position.x = msg->x;
    geo_pose_msg.pose.position.y = msg->y;
    // turtle1 publish geometry_msgs/PoseStamped
    turtle1_pub_geometryPose.publish(geo_pose_msg);
}
void tf2_listener_case02::turtle2_subscriberCb(const turtlesim::Pose::ConstPtr& msg){
    geometry_msgs::PoseStamped geo_pose_msg;
    geo_pose_msg.header.frame_id = "world";
    geo_pose_msg.header.stamp = ros::Time::now();
    geo_pose_msg.pose.position.x = msg->x;
    geo_pose_msg.pose.position.y = msg->y;
    // turtle1 publish geometry_msgs/PoseStamped
    turtle2_pub_geometryPose.publish(geo_pose_msg);
}
void tf2_listener_case02::turtle1_geometryPoseCb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    geometry_msgs::PoseStamped target_Pose;
    tf_.transform(*msg, target_Pose, "turtle2");
    std::cout << target_Pose.header.frame_id<<":\t";
    std::cout << target_Pose.pose.position.x << "," << target_Pose.pose.position.y <<"\n\n";
}
// 创建一个小乌龟turtle2
void tf2_listener_case02::call_spawn_service(std::string turtlename){   
    ros::service::waitForService("spawn");
    ros::ServiceClient spawner = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn turtle;
    turtle.request.x = 2;
    turtle.request.y = 2;
    turtle.request.theta = 0;
    turtle.request.name = turtlename;
    spawner.call(turtle);
    std::cout << "--------- turtle2 create successful." << std::endl<<std::endl;
}

/* --------------------------- main -------------------------------------------------*/
int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_tf2_listener");    
    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    tf2_listener_case02 tf2_listenerNode(tfBuffer);    // 实例中进行订阅和发布       

    ros::Rate rate(10);
    while (ros::ok())
    {
        geometry_msgs::TransformStamped transformStamped;
        try
        {
            transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time::now(), ros::Duration(10.0));
        }
        catch (tf2::TransformException &ex)
        {
            ROS_WARN("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }               

        std::cout << "----------- turtle1 frame in turtle2 frame: \n";
        std::cout <<"translation:\t" <<transformStamped.transform.translation.x << "," <<transformStamped.transform.translation.y << "\n";
        std::cout <<"   Rotation:\t" <<transformStamped.transform.rotation.x << "," <<transformStamped.transform.rotation.y << ","
                                     <<transformStamped.transform.rotation.z << "," <<transformStamped.transform.rotation.w << "\n";

        tf2::Quaternion quat_tf;
        tf2::fromMsg(transformStamped.transform.rotation, quat_tf);
        double roll,pitch,yaw;
        tf2::Matrix3x3(quat_tf).getRPY(roll,pitch,yaw);

        std::cout <<"   Rotation:\t" << roll << "," << pitch << "," << yaw*180.0/3.1415926<< "\n";
        std::cout << std::endl;

        ros::spinOnce(); // 10hz的频率执行spinOnce()
        rate.sleep();
    }
    return 0;
};

回到顶部

3. 将tf消息转换为四元数、欧拉角(使用tf2中的方法)

3.1 说明

1. tf2::Quaternion // TF2中四元数对象
2. tf2::fromMsg(msg_qua, qua); // msg -> Quaternion
3. tf2::Matrix3x3(quat_tf).getRPY(roll,pitch,yaw); // 得出四元数中的欧拉角

3.2 代码实例

#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
    ...
    tf2::Quaternion quat_tf;  // tf2的四元数对象
    tf2::fromMsg(transformStamped.transform.rotation, quat_tf);  
    // 将消息中的四元数（geometry_msgs/Quaternion）转换为一个四元数对象；
    double roll,pitch,yaw;
    tf2::Matrix3x3(quat_tf).getRPY(roll,pitch,yaw);
    // 将四元数对象，转换为欧拉角

回到顶部

4. buffer.transform()

4.1 功能说明

• 参考链接
• Use tf2_ros::BufferInterface::transform() to apply a transform on the tf server to an input frame.
  • Or, check if a transform is available with tf2_ros::BufferInterface::canTransform().
  • Then, call tf2_ros::BufferInterface::lookupTransform()to get the transform between two frames.
• transform()存在六中模板形式，如下实例中使用的是第二种形式!
  
  • Transform an input into the target frame and convert to a specified output type. It is templated on two types: the type of the input object and the type of the transformed output. For example, the template types could be Transform, Pose, Vector, or Quaternion messages type (as defined in geometry_msgs). The function will calculate the transformation and then convert the result into the specified output type. Compilation will fail if a known conversion does not exist bewteen the two template parameters.

4.2 代码实例

void tf2_listener_case02::turtle1_geometryPoseCb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    geometry_msgs::PoseStamped target_Pose;
    tf_.transform(*msg, target_Pose, "turtle2");
    std::cout << target_Pose.pose.position.x << "," << target_Pose.pose.position.y <<"\n\n";
}

1. callback函数中，msg是geometry_msgs::PoseStamped消息
2. 定义一个同类型的消息对象；
3. 利用buffer接口transform()进行位置Pose在指定坐标系下的转换；
4. 代码说明：msg是在/world下的pose，而target_pose是转换后的在/turtle2下的pose，他们在空间中是同一个点；