ROS基础(四)：一文说尽TF坐标变换

本文的目的是，将所有ROS下与tf坐标变换相关的问题一网打尽。

在代码实践部分，当前ros官方在推tf2，未来的tf要被废弃。所以我们的cpp实现，都是基于tf2编写的。

关于与时间相关的TF变换内容，我们放在了：ROS基础(五)：ROS中的时间与TF中的时间
中的第二部分。有兴趣的朋友可以查看。

一、概述

机器人身体上每个自由度的关节都是一个单独的坐标系。除此之外，机器人移动过程中，本体与世界坐标系之间也有相对位姿变换。

上述所有与机器人坐标变换相关的事情，在ros下都是由TF负责。

三、初识TF：乌龟例程

开启两个终端，分别输入如下内容：

第一个终端输入如下内容，开启乌龟仿真界面。

roslaunch turtle_tf turtle_tf_demo.launch 
 

第二个终端输入：rosrun turtlesim turtle_teleop_key来控制其中第一个小乌龟，然后观察现象。

可以看到，第二个乌龟一直follow第一个乌龟。

具体原理就是第二个乌龟一直通过tf订阅第一个乌龟的位置，然后将位置转换为控制指令，控制自己向第一个乌龟当前的位置移动。

二、TF相关工具

1. rqt_tf_tree

这个工具很有用，实时显示当前所有的tf连接，并以树的形式展示出来。

还是小乌龟案例，终端输入：rosrun rqt_tf_tree rqt_tf_tree，我们可以形象的看到他们之间的tf连接关系。

另外还有一个相似的命令：rosrun tf view_frames, 这个命令可以监听5s的tf_tree，然后将其保存成为pdf文档。没啥用，通常都用上一个命令查看tf_tree是否联通。

2. tf_echo

这条命令可以查看指定两个frame坐标系之间的tf变换。格式如下：

rosrun tf tf_echo <target_frame> <source_frame>

命令查询的是source to target 的坐标变换。也就是，在target的坐标系下，source_frame的坐标位置。

我们用小乌龟案例实操下。终端键入命令rosrun tf tf_echo turtle1 turtle2

我们先来预测下，上述命令查询的是相对于target坐标系turtle1而言，turtle2的相对位置。因为turtle2一直在跟着turtle1走，所以当tuetle1沿着某个方向前进的时候，对应的translation应该只有x轴在变，而且是负数。rotation应该是不变的。我们测试下，是否预测成功。

没问题！！

3. tf_monitor

这个工具可以查看tf_tree的发布状态。我感觉没啥大用。简单说下相关命令：

rosrun tf tf_monitor：监控所有的坐标系发布状态
rosrun tf tf_monitor <frame1> <frame2>： 监控指定两个frame之间的发布状态

4. static_transform_publisher

这个工具发布两个frame之间的静态坐标变换，经常用于发布机器人本身传感器或关节之间的刚体变换。

可以在终端和launch文件中使用，通常出现的场景是在launch文件中。

终端使用

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 

举例：

rosrun tf static_transform_publisher 1 2 1 1 3 4 turtle2 turtle4 1

参数解释：

1 2 1 1 3 4：偏移参数 + 欧拉角，如果是七个数字的话，就是偏移参数 + 四元数
turtle2: father_father_id
turtle4: child_frame_id
1: 发布的transform的时间间隔，1ms发布一次turtle4->turtle2的transform

注意：

上述命令发布的是/turtle4相对于/turtle2 的坐标变换，也就是在/turtle2 坐标系下，/turtle4的位置。按照我们之前的定义，就可以将/turtle2 称为target_frame, 将/turtle4称为source_frame

运行之后的效果：

launch文件中的使用

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

具体解释同上。

自己编写ros节点来发布静态TF

1. 撰写node节点：

//
// Created by xu on 2021/1/14.
//

#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. 修改CMakeLists

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
target_link_libraries(static_turtle_tf2_broadcaster  ${catkin_LIBRARIES} )

3. 工作空间catkin_make编译

4. 终端运行：
   rosrun learn_tf2 static_turtle_tf2_broadcaster myworld 1 2 3 4 5 6

5. 查看运行结果：

5.tf in rviz

同样，万能的可视化工具rviz也有tf可视化插件

rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

启动rviz，然后控制turtle1运动，在rviz中查看两龟共舞。

四、TF的API（C++）

1. 对应头文件

#include <tf2_ros/transform_broadcaster.h>

2. 创建TransformBroadcaster对象

  static tf2_ros::TransformBroadcaster br;

3. 创建要传输的对象并填充内容

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

4. 发射

br.sendTransform(transformStamped);

五、手写小乌龟follow实例

首先建立tf_tree: 用一个tf_boardcaster分别发布两个结点相对于world坐标系下的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");

    // parameter server or use args
   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;
}

然后建立一个tf_listener， 监听/turtle1 相对于/turtle2的位置，并将之转换为twist

//
// Created by xu on 2021/1/15.
//

#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char** argv){

    ros::init(argc, argv, "follower");
    // client
    ros::NodeHandle nh("~");
    ros::service::waitForService("/spawn");
    ros::ServiceClient turtle_client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 2;
    spawn.request.y = 2;
    turtle_client.call(spawn);

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

    double scale_linear, scale_angular;
    nh.param("scale_linear", scale_linear, 2.0);
    nh.param("scale_angular", scale_angular, 2.0);

    tf2_ros::Buffer tfBuffer;
    tf2_ros::TransformListener tfListener(tfBuffer);

    ros::Rate rate(10);
    while (ros::ok()){
        geometry_msgs::Twist cmd_vel;
        // tf2 listener: turtle2 to turtle1
        geometry_msgs::TransformStamped transformStamped, transformStamped1;
        try{
            // T_turtle2_turtle1
            transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
        }catch (tf2::TransformException &exception){
            ROS_WARN("wait tf tree:  %s",exception.what());
            ros::Duration(1.0).sleep();
            continue;
        }
        cmd_vel.angular.z = 4 * atan2(transformStamped.transform.translation.y,
                                        transformStamped.transform.translation.x);
        cmd_vel.linear.x =  0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                      pow(transformStamped.transform.translation.y, 2));
        turtle2_pub.publish(cmd_vel);

        rate.sleep();
    }
    return 0;
}

最后撰写launch文件，将以上所有节点都启动起来：

  <launch>
     <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

<!--    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>-->
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <node pkg="learn_tf2" type="turtle_tf2_broadcaster"
          args="/turtle1" name="turtle1_tf2_broadcaster" />
    <node pkg="learn_tf2" type="turtle_tf2_broadcaster"
          args="/turtle2" name="turtle2_tf2_broadcaster" />
      <node pkg="learn_tf2" type="follower" name="turtle_tf2_listener" output="screen"/>

  </launch>