ROS坐标系管理工具包-tf2

简介

ROS中存在许多不同的坐标系，例如传感器坐标系、机器人本体系、世界坐标系、地图坐标系等。虽然我们可以手动管理不同坐标系之间转换关系，但在大型项目中难免会出现混乱，为此ROS提供了坐标系管理工具包tf和tf2，两者的简介和区别可以参考官方文档ROS tf2简介。

对于新用户来说，官方建议使用上手直接使用接口定义更为规范的tf2。本文是基于ROS官方tf2教程的记录，便于各位在学习过程中参考。

编写静态tf2发布器

对于静止不动的坐标变换关系，可以使用静态tf2发布器，该发布器属于tf2_ros工具包，支持欧拉角和四元数两种格式的调用：

// 欧拉角形式
rosrun static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
// 四元数形式
rosrun static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

其中，x, y, z为子坐标系（child_frame）原点在父坐标系（frame）中的坐标；欧拉角调用中yaw(Z), pitch(Y), roll(X)为父坐标系旋转到子坐标系的旋转角度，转序为Z-Y-X；四元数调用中qx, qy, qz为四元数的矢量部分，qw为四元数的标量部分。

编写动态tf2发布器

相比静态变换（static transformation），实际中使用更多的是动态变换，例如需要将IMU的量测信息由传感器坐标系变换到世界坐标系，而传感器坐标系是固联在体系上，随体系一起转动的，节点demo如下：

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h> // tf2四元数头文件
#include <tf2_ros/transform_broadcaster.h> //tf2 broadcaster头文件
#include <geometry_msgs/TransformStamped.h>// 带时戳的变换消息
#include <turtlesim/Pose.h>

std::string turtle_name; //子坐标系名字

void poseCallback(const turtlesim::PoseConstPtr& msg){
  //构建tf2 broadcaster
  static tf2_ros::TransformBroadcaster br;
  //构建用于tf2 broadcaster的带时戳变换消息
  geometry_msgs::TransformStamped transformStamped;
  // 构建消息头
  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  // 子坐标系ID（注意其不在消息头中）
  transformStamped.child_frame_id = turtle_name;
  // 子坐标系原点在world下的三轴坐标
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  // 子坐标系和world之间的姿态（转序为zyx)
  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("~");
  //检查命令行或launch中是否给定了turtle name
  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;
};

编写tf2接收器

上一节我们通过tf2_broadcaster发布了一组变换关系，本节演示如何获取这一变换关系以实现ROS tf2简介中的turtle2自动跟随turtle1效果。

#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, "tf2_listener");
    ros::NodeHandle nh;
    // 获取服务生成另一个turtle
    ros::service::waitForService("spawn");
    ros::ServiceClient spawner = nh.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);
    // 创建第二个turtle的速度消息publisher
    ros::Publisher turtle_vel = nh.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
    // 创建tf缓存器和tf_listener(10s)
    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    ros::Rate rate(10.0);
    
    while(nh.ok()){
        geometry_msgs::TransformStamped tf_stamped;
        // 在最近的tfBuff中寻找是否存在turtle2和turtle1之间的变换
        try{
            tf_stamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0)); 
        }
        catch(tf2::TransformException &ex){
            ROS_WARN("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
 		// 根据制导率和接收到的tf消息计算速度并发布
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(tf_stamped.transform.translation.y, 
                                        tf_stamped.transform.translation.x);
        vel_msg.linear.x = 0.5 * sqrt(pow(tf_stamped.transform.translation.x, 2) +
                                      pow(tf_stamped.transform.translation.y, 2));
        turtle_vel.publish(vel_msg);
        rate.sleep();
    }
    return 0;
}

代码中核心部分主要包括以下几部分：

1. 创建tf2_ros::TransformListener对象，接受参数为tf2_ros::Buffer对象。该对象从创建开始自动向tf2_ros::Buffer添加所有的发布的geometry_msgs::TransformStamped和geometry_msgs::Transform消息，默认维持数据量为最近的10s；

// 创建tf缓存器和tf_listener(10s)
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);

2. 使用在tf2_ros::Buffer的lookupTransform函数在当前tf2_ros::Buffer对象中寻找指定的变换关系：

 tf_stamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0)); 
 	// 参数1： 目标frame
 	// 参数2： 参考frame
 	// 参数3： 所获取tf对应的时刻，若为ros::Time(0)则表示获取最新的tf

lookupTransform中的时间参数

在编写tf2_listener时，我们注意到lookupTransform函数的第三个参数为时间参数ros::Time，其最易混淆的两种用法是ros::Time::now()和ros::Time(0)，区别列举如下：

• ros::Time::now()：该参数表示从tf_buffer中获取和当前时戳一致的tf，由于传输延迟的存在，期望的tf很可能并不存在；
• ros::Time(0)：该参数表示从tf_buffer中获取时戳最新的tf，期望的tf一定存在，但由于传输延迟的存在，该tf并不一定是当前时刻的tf；

实际上，完整的lookupTransform共包括6个入口参数，说明如下：

lookupTransform(string& target_frame, ros::Time& target_time
				string& source_frame, ros::Time& source_time
				string& fixed_frame, ros::Time& timeout)
	// target_frame:目标坐标系名称
	// target_time：目标坐标系的时戳
	// source_frame:原坐标系名称
	// source_time:原坐标系时戳
	// fixed_frame:固定坐标系名称
	// timeout:等待时间

在默认调用中，target_time和source_time默认一致，固定坐标系默认为世界坐标系"/world"。