07 ROS的TF坐标管理工具

一、TF介绍

TF 即 Transform 转换之意
通过坐标变换可以得到一个机器人中不同坐标系之间的关系
A坐标系下的位置和姿态可以通过平移和旋转，转化成B坐标系下的位姿
订阅TF消息的节点将会缓冲一份一段时间内所有坐标系变换关系的数据

二、使用TF功能包的方式

1. 监听TF变换 Listen

接收、缓存系统发布的所有坐标变换数据，从中查询所需的坐标变换关系

2. 广播TF变换 Broadcast

向系统内广播坐标系之间的变换关系
广播可以将坐标关系存入TF树，不需要再与其他不同部分同步

定义TF广播器
创建坐标变化值并封装
发布坐标变换

三、TF工具的使用

1. 查看TF树中所有坐标的发布状态

为可选参数
可以单独查看两个坐标之间的发布状态

rosrun tf tf_monitor <source_frame> <target_frame>

2. 查看两个坐标系的变换关系

rosrun tf tf_echo turtle1 turtle2

结果：打印出了用RPY和四元数两种方式描述坐标系之间的关系的数据

At time 1647863106.937

• Translation: [0.000, 0.000, 0.000]
• Rotation: in Quaternion [0.000, 0.000, 0.243, 0.970]
  in RPY (radian) [0.000, -0.000, 0.491]
  in RPY (degree) [0.000, -0.000, 28.144]

3. 发布相对静态坐标系的坐标变换

设置两相对静止坐标系间的偏移参数和旋转参数
两种表示方式
1.弧度制 2.使用四元数表示旋转角度

绕z轴的偏航角yaw 绕y轴的俯仰角pitch 绕x轴的翻滚角roll

rosrun tf static_transform_publisher x y z yaw pitch roll father_frame_id child_frame_id period_in_ms

rosrun tf static_transform_publisher x y z qx qy qz qw father_frame_id child_frame_id period_in_ms

在launch文件中使用该命令

<launch>
    <node 
        pkg="tf"
        type="static_transform_publisher"
        name="link1_to_link2_TF_Broadcaster"
        args="1 0 0 0 0 0 1 link1_parent link1 100"
    />
</launch>

4. 可视化查看TF关系树

可视化TF树并生成pdf文件

rosrun tf view_frames

pdf文件可以直接查看，也可以

evince frames.pdf

5. 三维坐标可视化工具Rviz

可以用三维坐标可视化工具Rviz来查看turtle_tf例程中的两龟关系

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

四、创建TF广播器 TF Broadcaster

广播器将TF信息插入TF树并发布

/*
Node Function:
create tf data , calculate and publish velocity command to turtle2
*/
#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>

std::string turtle_name;
//回调函数
void PoseCallBackFunction(const turtlesim::PoseConstPtr& msg)
{
   
    //create TF_Broadcaster
    //广播器
    static tf::TransformBroadcaster br;

    //TF_data INIT
    //设置turtle相对于world的坐标变换
    tf::Transform transform;
    //setOrigin设置平移变换
    transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));
    tf::Quaternion qt;
    qt.setRPY(0,0,msg->theta);
    //setOrigin设置旋转变换
    transform.setRotation(qt);

    //Broadcast TFData between world and turtle Coordinate System
    //发布坐标变换
    //该类型的TF消息不仅包含tf::Transform的坐标变换与时间轴，而且要指定父坐标系和子坐标系
    br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));    
}

//char** argv is used to figure out turtle1 and turtle2
//but the Node name should be unique and only
//how to set different Node name for turtle1 and turtle2
//it's called Remap
int main(int argc,char** argv)
{
   
    //ROSINIT
    ros::init(argc,argv,"my_tf_broadcaster");

    //INIT NodeHandle
    ros::NodeHandle n;

    //name the turtle
    if(argc!=2)
    {
   
        ROS_ERROR("Need Turtle Name as Argument");
        return -1;
    }

    turtle_name= argv[1];

    //Create a subscriber for Pose of turtle
    //turtle_name could be turtle12345678
    ros::Subscriber sub=n.subscribe(turtle_name+"/pose",10,&PoseCallBackFunction);

    //Waiting for CBF
    ros::spin();

    return 0;
}

五、创建TF监听器 TF Listener

turtle1相对于world的TF信息已经插入树，Listener从TF树处监听TF消息，并从中获取turtle2关于turtle1的坐标变换，从而指导turtle2的运动

/*
Node Function:
Listen to TFDATA,calculate and pub turtle2 velocity command
*/
#include<ros/ros.h>
#include<tf/transform_listener.h>
#include<geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc,char** argv)
{
   
    //ROSINIT
    ros::init(argc,argv,"my_tf_listener");
    ros::NodeHandle n;

    //request service /spawn to create turtle2
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle =n.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    //Create velocity command Publisher for turtle2
    ros::Publisher turtle_vel=n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);

    //Create TF Listener
    tf::TransformListener listener;
    ros::Rate rate(10.0);//监听TF树，缓存10.0s
    while(n.ok())
    {
   
        //get TF Data between turtle1 and turtle2 Coordinate System
        tf::StampedTransform transform;
        //Duration :3s overtime
        //lookup relation between turtle1 and turtle2
        //result saves in Variable transform
        try
        {
   
            listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);
            //获取turtle2和turtle1的变换关系并存在transform里
        }
        catch(tf::TransformException &ex)
        {
   
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        //pub turtle2 velocity command based on relationship between t1 and t2 Coordinate System
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z=4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());
        vel_msg.linear.x=0.5*sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));
        turtle_vel.publish(vel_msg);
        rate.sleep();
    }
    return 0;
}

六、配置launch文件

<launch>
<!-- turtlesim Node -->
    <node
        pkg="turtlesim"
        type="turtlesim_node"
        name="sim_node"
    />
<!-- turtlesim keyboard Node -->    
    <node
        pkg="turtlesim"
        type="turtle_teleop_key"
        name="keyboard_node"
    />
<!-- turtle1 and turtle2 TF Broadcaster Node -->
    <node
        pkg="learning_tf"
        type="turtle_tf_broadcaster"
        name="Turtle1_TF_Broadcaster"
        args="/turtle1"
    />

    <node
        pkg="learning_tf"
        type="turtle_tf_broadcaster"
        name="Turtle2_TF_Broadcaster"
        args="/turtle2"
    />
<!-- TF Listener for controling  velocity of turtle2-->
    <node
        pkg="learning_tf"
        type="turtle_tf_listener"
        name="Turtle_Listener"
    />
</launch>