ROS学习总结三：TF变换、TF广播与监听

一、TF变换
TF变换的作用：
1、五秒钟之前，机器人头部坐标系相对于全局坐标系的关系是什么样的？
2、机器人夹取的物体相对于机器人中心坐标系的位置在哪里？
3、机器人中心坐标系相对于全局坐标系的位置在哪里?
TF变换如何实现：
1、广播TF变换
2、监听TF变换
例程：

$ sudo apt-get install ros-kinetic-turtle-tf

这是一个turtlesim的小例程，一般安装ROS的时候会自动安装，没有的话按照上面命令安装一下。然后打开该例程：

$ roscore
#打开新终端
$ roslaunch turtle_tf turtle_tf_demo.launch

这时候能看到一个类似于turtlesim中的海龟例程的画面，但是里面有两只不同的海龟，它们开始时的位置可能不是重叠的，然后其中一只海龟会移动到另一只海龟的位置。

然后我们启动键盘节点移动第一只海龟：

roslaunch turtlesim turtle_teleop_key

这时候会看到第二只海龟一只追着第一只海龟在走。
然后我们使用下列命令看一下两只海龟的坐标间的关系：

rosrun tf view_frames

这时是home文件夹下可以生成一个PDF文件，打开：

这里代表了两只海龟之间的关系。它们通过world世界坐标系关联。或者也可以用下列命令查看它们现在的坐标关系：

rosrun tf tf_echo turtle1 turtle2

这里translation代表平移变换关系，可以看出两只海龟基本是一致的，rotation代表旋转关系，即两只海龟的角度关系，可以看出是有一点区别的，quaternion代表四元数、radian代表弧度、degree代表角度。其实表示的内容是一样的。
或者这里也可以使用可视化的方式：

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

这时会打开rviz，在左边左下角点击“ADD”添加两个“Axes”，reference frame 选择turtle1以及turtle2.这时会报错，这时因为全局框架不存在的问题，把global option中的fixed frame改为turtle1就可以了。然后使用键盘节点移动海龟，可以看到rviz中的两个坐标关系也会发生变化，当你停止移动时两个坐标会逐渐重合，代表两只海龟位置的重合。


二、TF的广播与监听
上面我们通过一个例程了解了TF变换，下面再用一个例程了解TF的广播与监听：
1、在原先的总结一中我们建立了一个文件夹，现在同样我们建立一个文件夹：

$ catkin_create_pkg exper4221 std_msgs rospy roscpp tf

这个文件夹可以放在之前初始化过的exper文件夹下，路径与总结一中相同。注意这里最后的“tf”，在之前的文件夹中是没有的，这是因为这里我们的程序会用到tf变换，如果不加依赖的话编译的时候会出错：

如果一开始没加的话可以打开exper4221下的cmakelist文件，在cpp、py等依赖后面直接加上“tf”就可以了。
2、然后在文件夹中我们建立两个文件：broadcast以及subscriber复制下列代码：
broadcast：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
 
std::string turtle_name;
 
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
     //TF广播器 
     static tf::TransformBroadcaster br;
     
     //根据乌龟当前的位姿。设置相对于世界坐标系的坐标变换
     tf::Transform transform;
     transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));//设置平移变换
     tf::Quaternion q;
     q.setRPY(0,0,msg->theta);
     transform.setRotation(q);//设置角度变换
 
     //发布坐标变换
     br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
}
 
 
 
int main(int argc,char** argv)
{
    ros::init(argc, argv, "my_tf_broadcaster");
    if(argc!=2)
    {
        ROS_ERROR("need turtle name as argument");
        return -1;
    }
    turtle_name=argv[1];
 
    ros::NodeHandle node;
    ros::Subscriber sub=node.subscribe(turtle_name+"/pose",10,&poseCallback);
 
    ros::spin();
 
    return 0;
}

以及subscriber：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
 
int main(int argc,char** argv)
{
    ros::init(argc, argv, "my_tf_listener");
    
    ros::NodeHandle node;
 
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);//生成一个新的乌龟
 
    ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);//发布类型为速度消息类型的速度发布器
 
    tf::TransformListener listener;
   
    ros::Rate rate(10.0);
    while(node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            /* code for Try */
            listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
            listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform); 
            
        }
        catch (tf::TransformException &ex)
        {
            /* code for Catch */
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
 
        geometry_msgs::Twist vel_msg;//速度消息
        vel_msg.angular.z=3.1415*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;
}

这两个程序的源码网上都找得到，也有一些做了修改的，都可以看一下。
然后在cmakelist中同样添加下列语句：

$ include_dirctories(include ${catkin_INCLUDE_DIRS})
#这行好像不加也行
$ add_executable(broadcast src/broadcast.cpp)
$ target_link-libraries(broadcast ${catkin_LIBRARIES})
$ add_executable(subscriber src/subscriber.cpp)
$ target_link-libraries(subscriber ${catkin_LIBRARIES})

然后再catkin_make一下：

$ cd ~/exper
$ catkin_make

如果不报错的话应该就没有问题了。
3、建立启动文件
这里我们使用launch文件启动，在exper/src/exper4221文件夹下建立一个文件夹，名为launch。进入该文件夹，建立一个exper_tf.launch文件，写入下列代码：

<launch>
        <!-- Turtlesim Node-->
        <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
        <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
        
        <node pkg="exper4221" type="broadcast" args="/turtle1" name="broadcast"/>
        <node pkg="exper4221" type="broadcast" args="/turtle1" name="broadcast2"/>
        
        <node pkg="exper4221" type="subscriber" name="subscriber"/>
    </launch>

运行一下：

$ roscore
#新建终端
$ roslaunch exper4221 exper_tf.launch

如果没有问题可以得到与例程一一样的效果。