ROS 学习应用篇（八）ROS中的坐标变换管理之tf广播与监听的编程实现

偶吼吼胜利在望，冲冲冲

老规矩新建功能包

工作空间目录下/src下开启终端输入

catkin_create_pkg learning_tf roscpp rospy tf turtlesim

如何实现tf广播

引入库

 c++                                                                                python  

pose是订阅的消息类型，之前在的订阅那一节我们已经介绍过了。

初始化节点

  c++                                                                                python  

这里python没有设定主函数，直接就在主函数定义段内嵌套了节点的定义，可以发现c++觉python这时候定义的节点名称是不一样的，C++定义的节点名叫my_tf_boradcaster，PYthon的叫 turtle_tf_boradcaster。

海龟位姿话题的订阅

 c++                                                                                python   

话题名的获取

话题的订阅就是需要使用到回调函数的，特别的是这一次的目的是输出向量额坐标系变换时候的平移矩阵和旋转矩阵，所以我们需要定义两条消息。那么就是turtle1和turtle2两个话题消息，我们可以发现在python中利用占位符以及turtlename=rospy.get_param('~turtle')，通过获取参数的方式就可以得到这两个海龟的话题名，而c++则需要turtle_name = argv[1];的方式获取送入的参数名字在加上/pose做区分，二者所检索的话题名都是turtlei/pose。

命名空间

python这句代码的意思是获取turtle的私有命名空间，如果没有~就是全局命名空间。

命名空间的学习见以下链接：

【精选】ROS中节点、话题、参数重命名以及全局、相对、私有命名空间（示例+代码）_ros2 节点 全局变量_肥肥胖胖是太阳的博客-CSDN博客

 argv[1]

对照着看会发现argc是输入参数的个数，argv是具体的输入的参数，数数方式吃从后向前数。

回调函数

有订阅就要有回调函数，这才是重中之重。

创建TF广播器（TransformBroadcaster）

 c++                                                                                python  

数据广播

 c++                                                                                python   

在数据广播出去之前，我们需要初始化数据，定义数据

初始化tf数据

 c++                                                                                python   

这一部分 c++的detOrigin是像transform中传入平移矩阵T，q是由平移矩阵计算出的欧拉角向量，setRotation则是像transform中传入欧拉向量角，此时就得到了旋转平移矩阵transform。

python的就更简单， 这一句的意思就是计算旋转平移矩阵transform。

广播数据

观察上面的代码可以发现两者都有一个time.now()这是输出实时时间用以区分。

 循环等待回调函数

在回调函数需要的消息进来之前，需要执行等待代码

 c++                                                                                python   

监听器的实现

在前面我们已经将两只海龟相对于世界坐标系的位置广播出去了，下面我们需要监听到二者的位置，在计算二者间的旋转平移矩阵。

引入库

这里面roslib是用来导入功能包的，咱也不知道为啥监听器的实现python代码需要多一个链接功能包的语句，所以这暂时是一个遗留问题。

/spawn是创建小海龟客户端的服务类型，这一次python用到的包真的好多。

初始化ROS节点

创建客户端

这一部分就是客户端的代码，生成第二只小海龟

等待/spawn服务指令

创建客户端

这里可以发现srv的数据并没有传入，这因为srv是自带封装好的初始数据的，之前在学习客户端建立的时候，我们初始化了srv并送入了一写数据，这里不送入使用自带的数据也不会报错，不同的是python中是需要指定起始点位置和名称的。

创建发布器

我们在监听到消息后需要发布出去，所以需要先在监听器外层建立一个发布器。

创建监听器

 

延时等待

准备工作做好之后就开始延时等待。

循环

数据监听

这句代码的意思是如果节点开启才执行while循环里的代码。

相较于python监听transform数据来说，c++多了一个等待3秒的代码，catch和except后面都是报错输出。

数据计算与发布

延时循环

 

主函数调用

由于python是直接在主循环里面写的，所以就不用指定主函数了。

CMakeList.txt

编译

工作空间开启终端输入

catkin_make

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

% 这两行代码中，__name是更改节点的名字广播器程序节点的名字在C++中是my_tf_boradcaster，PYthon的叫 turtle_tf_boradcaster。但是如果我们要广播两次的话，就需要对节点的名字进行更改避免冲突，这其实就像是函数调用一样，在设定函数的时候的参数值在使用的使用我们要输入不同的名字以避免冲突。

如果不改名字的化就会提示节点名字冲突。后面的turtle1是需要的输入变量。

rosrun learning_tf turtle_tf_listener % 前几句一直没有反应，只有到这一句开始才会建立海龟，开始追随，不停地发布turtle1的位置给turtle2，然后turtle2再运动，这就是跟随的基本原理，我们知道发布是在监听代码中进行的，发布的消息名称一直是turtle2，也就是给turtle2发运动位置消息。也就是说这个消息是直接发个小海龟2的！。换个思路我们可以turtle1的位置发布给turtle2也是可以实现同样的效果的。额，他好像就是这么实现的哈。把1的位置广播出去然后发布给2.

不对，是广播的两只乌龟的位置，然后给turtle2发布的指令是它往1走的方向。

rosrun turtlesim turtle_teleop_key  % 这个是键盘控制按键，2会跟着1走。

至于python代码的执行如下两行代码需要变更成如下的语句

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcaster _turtle:=turtle1

rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcaster _turtle:=turtle2