10.TF发布与监听

10、tf发布与监听

10.1、tf功能包

10.1.1、tf是一个让用户随时间跟踪多个坐标系的功能包，它使用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间、坐标系间完成点、向量等坐标变换。

10.1.2、使用步骤

1）、监听tf变换

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

2）、广播tf变换

向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的tf变换广播。每个广播都可以直接将坐标变换关系插入tf树中，不需要再进行同步。

10.2、tf坐标系广播与监听的编程实现

10.2.1、创建并且编译功能包

xxxxxxxxxx cd ~/catkin_ws/src catkin_create_pkg learning_tf rospy roscpp turtlesim tf cd .. catkin_make

10.2.2、如何实现一个tf广播器

1）、定义tf广播器（TransformBroadcaster）；

2）、初始化tf数据，创建坐标变换值；

3）、发布坐标变换（sendTransform）；

10.2.3、如何实现一个tf监听器

1）、定义TF监听器（TransformListener）；

2）、查找坐标变换（waitForTransform、lookupTransform）

10.2.4、C++语言实现tf广播器

1）、在功能包learning_tf的src文件夹,创建一个c++文件（文件后缀为.cpp），命名为turtle_tf_broadcaster.cpp

2）、把下边的程序代码复制粘贴到turtle_tf_broadcaster.cpp文件中

xxxxxxxxxx #include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> ​ std::string turtle_name ; ​ void poseCallback ( const turtlesim::PoseConstPtr & msg ) { static tf::TransformBroadcaster br ; // 创建tf的广播器 ​ // 初始化tf数据 tf::Transform transform ; transform . setOrigin ( tf::Vector3 ( msg -> x , msg -> y , 0.0 ) ); //设置xyz坐标 tf::Quaternion q ; q . setRPY ( 0 , 0 , msg -> theta ); //设置欧拉角：以x轴，y轴，z轴旋转 transform . setRotation ( q ); ​ br . sendTransform ( tf::StampedTransform ( transform , ros::Time::now (), "world" , turtle_name )); // 广播world与turtle坐标系之间的tf数据 } ​ int main ( int argc , char** argv ) { ros::init ( argc , argv , "turtle_world_tf_broadcaster" ); // 初始化ROS节点 ​ if ( argc != 2 ) { ROS_ERROR ( "Missing a parameter as the name of the turtle!" ); return \- 1 ; } turtle_name = argv [ 1 ]; // 输入参数作为海龟的名字 ​ // 订阅海龟的位姿话题/pose ros::NodeHandle node ; ros::Subscriber sub = node . subscribe ( turtle_name \+ "/pose" , 10 , & poseCallback ); ​ // 循环等待回调函数 ros::spin (); ​ return 0 ; };

3）、程序流程图

4）、代码解析

首先，先订阅小海龟的/pose位姿话题，倘若有该话题发布，那么就进入回调函数。再回调函数里边，首先先创建了tf的广播器，然后初始化tf数据，数据的值就是订阅/pose话题传过来的数据。最后，通过br.sendTransform把小海龟对于世界坐标的变换发布出去，这里说下sendTransform这个函数。有4个参数，第一个参数表示tf::Transform类型的坐标转换（也就是之前初始化的tf数据），第二个参数是时间戳，第三个和第四个是变换的源坐标系和目标坐标系。

10.2.4、C++语言实现tf监听器

1）、在功能包learning_tf的src文件夹,创建一个c++文件（文件后缀为.cpp），命名为turtle_tf_listener.cpp

2）、把下边的程序代码复制粘贴到turtle_tf_listener.cpp文件中

xxxxxxxxxx /** * 该例程监听tf数据，并计算、发布turtle2的速度指令 turtle2->turtle1 = world->turtle*world->turtle2 */ ​ #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 , "turtle1_turtle2_listener" ); // 初始化ROS节点 ​ ros::NodeHandle node ; // 创建节点句柄 ​ // 请求服务产生turtle2 ros::service::waitForService ( "/spawn" ); ros::ServiceClient add_turtle = node . serviceClient < turtlesim::Spawn > ( "/spawn" ); turtlesim::Spawn srv ; add_turtle . call ( srv ); ​ // 创建发布turtle2速度控制指令的发布者 ros::Publisher vel = node . advertise < geometry_msgs::Twist > ( "/turtle2/cmd_vel" , 10 ); ​ tf::TransformListener listener ; // 创建tf的监听器 ​ ros::Rate rate ( 10.0 ); ​ while ( node . ok ()) { // 获取turtle1与turtle2坐标系之间的tf数据 tf::StampedTransform transform ; try { listener . waitForTransform ( "/turtle2" , "/turtle1" , ros::Time ( 0 ), ros::Duration ( 3.0 )); listener . lookupTransform ( "/turtle2" , "/turtle1" , ros::Time ( 0 ), transform ); } catch ( tf::TransformException & ex ) { ROS_ERROR ( "%s" , ex . what ()); ros::Duration ( 1.0 ). sleep (); continue ; } ​ // 根据turtle1与turtle2坐标系之间的位置关系，通过数学计算公式，得出角速度和线速度，发布turtle2的速度控制指令 geometry_msgs::Twist turtle2_vel_msg ; ​ turtle2_vel_msg . angular . z = 6.0 * atan2 ( transform . getOrigin (). y (), transform . getOrigin (). x ()); turtle2_vel_msg . linear . x = 0.8 * sqrt ( pow ( transform . getOrigin (). x (), 2 ) \+ pow ( transform . getOrigin (). y (), 2 )); vel . publish ( turtle2_vel_msg ); ​ rate . sleep (); } return 0 ; };

3）、程序流程图

4）、代码解析

首先，通过服务调用产生另外一只小乌龟turtle2，然后创建turtle2速度控制发布者；接着创建一个监听器，监听和查找turtle1和tuetle2的左边变换，这里涉及到两个函数waitForTransform和lookupTransform

waitForTransform(target_frame,source_frame,time,timeout):两个frame分别表示目标坐标系和源坐标系，time表示等待两个坐标系之间变换的时间，因为坐标变换是阻塞程序，所以需要设置timeout，表示超时时间。

lookupTransform(target_frame,source_frame,time,transform):给定源坐标系(source_frame)和目标坐标系(target_frame)，得到两个坐标系之间指定时间(time)的坐标变换(transform)。

经过lookupTransform我们得到了坐标变换的结果transform，然后通过transform.getOrigin().y(),transform.getOrigin().x()得到x,y的值，接着通过数学运算得到角速度angular.z和线速度linear.x，最后发布出去，让turtle2做运动。

10.2.5、修改CMakelist.txt和编译

1）、修改修改CMakelist.txt

修改功能包下的CMakelist.txt，添加下边内容

xxxxxxxxxx add_executable(turtle_tf_listener src/turtle_tf_listener.cpp) target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES}) ​ add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp) target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

2）、编译

xxxxxxxxxx cd ~/catkin_ws catkin_make source devel/setup.bash #需要配置环境变量，否则系统无法找到运行程序

10.2.6、启动与运行效果演示

1）、启动

xxxxxxxxxx roscore rosrun turtlesim turtlesim_node rosrun learning_tf turtle_tf_broadcaster __name := turtle1_tf_broadcaster / turtle1 rosrun learning_tf turtle_tf_broadcaster __name := turtle2_tf_broadcaster / turtle2 rosrun learning_tf turtle_tf_listener rosrun turtlesim turtle_teleop_key #打开海龟键盘控制程序，控制海龟运动

2）、效果展示

3）、程序说明

启动roscore后，开启小海龟节点，这时候终端会出现一只小海龟；然后我们发布两个tf变换，turtle1->world，turtle2->world，因为要想知道turtle2与turtle1之间的变化，则需要知道他们跟world之间的变换；然后，开启tf监听程序，这时候会发现终端产生了另外一只小海龟turtle2，并且turtle2会向turtle1移动；然后，我们打开键盘控制，通过按下方向键来控制turtle1移动，然后turtle2会追着turtle1移动。

10.2.7、Python语言实现tf广播器

1）、在功能包learning_tf,创建一个文件夹script，切换到该目录下，新建一个.py文件，命名为turtle_tf_broadcaster.py

2）、把下边的程序代码复制粘贴到turtle_tf_broadcaster.py文件中

xxxxxxxxxx #!/usr/bin/env python # -*- coding: utf-8 -*- ​ import roslib roslib . load_manifest ( 'learning_tf' ) import rospy ​ import tf import turtlesim . msg ​ def handle_turtle_pose ( msg , turtlename ): br = tf . TransformBroadcaster () #定义一个tf广播 #广播world与输入命名的turtle之间的tf变换 br . sendTransform (( msg . x , msg . y , 0 ), tf . transformations . quaternion_from_euler ( 0 , 0 , msg . theta ), rospy . Time . now (), turtlename , "world" ) ​ if __name__ == '__main__' : rospy . init_node ( 'turtle1_turtle2_tf_broadcaster' ) #初始化ros节点 turtlename = rospy . get_param ( '~turtle' ) #从参数服务器中获取turtle的名字 #订阅/pose话题数据，也就是turtle的位姿信息 rospy . Subscriber ( '/%s/pose' % turtlename , turtlesim . msg . Pose , handle_turtle_pose , turtlename ) rospy . spin ()

3）、程序流程图

10.2.8、Python语言实现tf监听器

1）、在功能包learning_tf的script文件夹,创建一个python文件（文件后缀为.py），命名为turtle_tf_listener.py

2）、把下边的程序代码复制粘贴到turtle_tf_listener.py文件中

xxxxxxxxxx #!/usr/bin/env python # -*- coding: utf-8 -*- ​ import rospy import math import tf import geometry_msgs . msg import turtlesim . srv ​ if __name__ == '__main__' : rospy . init_node ( 'turtle_tf_listener' ) #初始化ros节点 ​ listener = tf . TransformListener () #初始化一个监听者 ​ rospy . wait_for_service ( 'spawn' ) #调用服务产生另一只海龟turtle2 spawner = rospy . ServiceProxy ( 'spawn' , turtlesim . srv . Spawn ) spawner ( 8 , 6 , 0 , 'turtle2' ) #声明一个发布者，用来发布turtle2的速度 turtle_vel = rospy . Publisher ( 'turtle2/cmd_vel' , geometry_msgs . msg . Twist , queue_size = 1 ) ​ rate = rospy . Rate ( 10.0 ) while not rospy . is_shutdown (): try : #查找turtle2和turtle1之间的tf变化 ( trans , rot ) = listener . lookupTransform ( '/turtle2' , '/turtle1' , rospy . Time ( 0 )) except ( tf . LookupException , tf . ConnectivityException , tf . ExtrapolationException ): continue #通过数学计算，算出线速度和角速度，然后发布出去 angular = 6.0 * math . atan2 ( trans [ 1 ], trans [ 0 ]) linear = 0.8 * math . sqrt ( trans [ 0 ] ** 2 \+ trans [ 1 ] ** 2 ) cmd = geometry_msgs . msg . Twist () cmd . linear . x = linear cmd . angular . z = angular turtle_vel . publish ( cmd ) rate . sleep ()

3）、程序流程图

10.2.9、启动与运行效果展示

1）、编写一个launch文件

在功能包目录下，新建一个文件夹launch，切换至launch里边，新建一个launch文件，命名为start_tf_demo_py.launch，把以下内容复制到里边，

xxxxxxxxxx < launch > ​ <!-- 海龟节点--> < node pkg = "turtlesim" type = "turtlesim_node" name = "sim" /> <!--广播turtle1->world--> < node name = "turtle1_tf_broadcaster" pkg = "learning_tf" type = "turtle_tf_broadcaster.py" respawn = "false" output = "screen" > < param name = "turtle" type = "string" value = "turtle1" /> </ node > <!--广播turtle2->world--> < node name = "turtle2_tf_broadcaster" pkg = "learning_tf" type = "turtle_tf_broadcaster.py" respawn = "false" output = "screen" > < param name = "turtle" type = "string" value = "turtle2" /> </ node > <!--监听--> < node pkg = "learning_tf" type = "turtle_tf_listener.py" name = "listener" /> <!--海龟键盘控制节点--> < node pkg = "turtlesim" type = "turtle_teleop_key" name = "teleop" output = "screen" /> </ launch > ​

2）、启动

xxxxxxxxxx roslaunch learning_tf start_tf_demo_py.launch

程序运行后，鼠标点击运行launch的窗口，按下方向键，turtle2会跟着turtle1移动。

3）、运行结果如下图