ROS中TF广播和监听个人理解及消息查找

ROS学习古月居TF使用总结

大佬链接

最全的广播整理:
官方的transform介绍
四元数与欧拉角
ROS学习的另一个大佬的总结

整理了半年tf相关内容，然后古老师在最后的广播这块讲的也不是很详细，所以我查了很多资料，在这里记录一下我此时的看法和对坐标的理解给未来的我看。

总代码目录

The Code of TFboardcast

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
	static tf::TransformBroadcaster br;//定义一个广播
	//--------初始化坐标系数据--------
	tf::Transform transform;//定义一个transform类
	transform.setOrigin( tf::Vector3(msg ->x, msg->y, 0.0) );
	//设置transform坐标原始值
	
	tf::Quaternion q//定义一个四元数
	q.setRPY(0, 0, msg->theta);//设置四元数类中欧拉角的三个
	transform.setRotation(q);  //设置transform中的四元数的值
	//----------为什么这么初始化在后面总结有--------------
	br.sendTransform( tf::StampedTransform( transform, ros::Time::now(),"world",turtle_name) );
	//将该StampedTransform消息广播出去（注：不只是transform，还发送了world等信息）
	//该广播的名字就是turtle_name
}

int main( int argc, char** argv )
{
	ros::init(argc, argv, "my_tf_broadcaster" );
	//初始化节点，节点名字叫做my_tf_broadcaster
	ros::NodeHandle node；//创造一个节点句柄
	
	if( argc != 2)
	{
	  //判断给main函数输入的变量个数，2的时候说明有输入某值，也就是创造的海归的i拿工资
	  ROS_INFO("NEED TURTLE NAME");
	  return -1;
	 }
	 
	turtle_name = argv[1];//输入的第一个变量赋值给turtle_name
	
	ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
	//创造一个订阅者，因为turtlesim_node里会自动发布海龟的位置坐标
 	ros::spin();
 	return 0;
}

The Code of TFlistener

#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");
//创造一个节点，节点名字叫做my_tf_listener
ros::NodeHandle node;
//节点句柄，控制节点用的

//----创造一个新的小乌龟-----------------------------------------------
ros::service::waitForService("/spawn");//等待/spawn的服务出现，该服务由turtlesim_node提供
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
//创建一个新的客户端
turtlesim::Spawn srv;//创建新的变量
srv.request.name ="turtle2";//设置小乌龟的名字 后面分析会说为什么这名字我搞成这鬼样
add_turtle.call(srv);//将该数据向服务端发送
//----新的小乌龟创建完毕并在小地图中显示出来------------------------------

ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);
//设置一个发布者，发布消息类型为geometry_msgs::Twist，topic为/turtle2/cmd_vel的消息
//这个publisher是用来控制乌龟2号的行动的 
 
tf::TransformListener listener;//创建一个监听者
ros::Rate rate(10.0)；

while(node.ok())
{
 tf::StampedTransform tr;//初始化一个Stampedtransform的类
  try
  {
   listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
   //等待两个名字叫做/turtle2和/turtle1的坐标发布
   listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),tr);
   //将turtle2和turtle1的坐标间的变化，存放到tr里，tr其实就是一个transform
  }
  catch
  {
   ROS_ERROR("%s",ex.what());
   ros::Duration(1.0).sleep();
   continue;
  }
  
geometry_msgs::Twist vel;//定义要乌龟2号的控制变量
vel.angular.z=4.0 * atan2(tr.getOrigin().y(),tr.getOrigin().x());
vel.linear.x= 0.5 * sqrt( pow(tr.getOrigin().x(), 2) + pow(tr.getOrigin().y(),2) );
//计算，理解分析里写
turtle_vel.publish(vel);//持续发布对turtle2的控制信息
rate.sleep();
}
return 0;
}

The Code of launch

<launch>
	<node pkg="turtlesim" type="turtlesim_node"    name ="turtle_node" output ="screen"/>
	<!--- 打开世界地图，也就是turtlesim_node --->
	<node pkg="szhtest"   type="turtle_tf_br"      name ="turtle1_br" args="/turtle1"/>
	<!--- 打开节点名字叫turtle1_br，并广播，输入参数/turtle1--->
	<node pkg="szhtest"   type="turtle_tf_br"      name ="turtle2_br" args="/turtle2"/>
	<!--- 用同一段代码打开节点名字改了叫turtle2_br，输入参数为/turtle2 --->
	<!--- 到这里为止已经有两个广播在准备广播两只海龟的信息了 --->	
	<node pkg="szhtest"   type="turtle_tf_ls"      name ="mytflistener"  />
	<!--- 打开监听者，同时创建了新的小海龟2，并指挥它开始活动靠近海龟1--->
	<node pkg="turtlesim" type="turtle_teleop_key" name ="teleop_key" output ="screen" />
	<!--- 打开键盘控制控制海龟1的变化 --->
</launch>

广播和监听者的使用总结

广播的创建和使用

首先是创建一个广播，要先包含tf函数包

#include <tf/transform_broadcaster.h>

创建广播器

static tf::TransformBroadcaster br;

广播信息

br.sendTransform( tf::StampedTransform( transform, ros::Time::now(),"world",turtle_name) );

广播一个坐标信息，坐标信息包含在transform，而transform包含着坐标以及四元数，world是指该坐标是相对于world这个坐标系而言的一个点，也就是在world上移动的坐标，然后名字叫做turtle_name。在rviz上反应就是一个大地图上，有一个叫turtle_name的坐标在移动或者旋转（要吧fixed frame中的map改成world）。

然后对于sendTransform这个函数，里面可以发送的信息类型还有很多参见官网：
http://docs.ros.org/en/noetic/api/tf/html/c++/classtf_1_1TransformBroadcaster.html

监听的创建和使用

创建一个监听所要包含的函数包

#include <tf/transform_listener.h>

创建一个监听者

tf::TransformListener listener;

监听广播

   listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
   //等待两个名字叫做/turtle2和/turtle1的坐标发布
   listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),tr);
   //将turtle2和turtle1的坐标间的变化，存放到tr里，tr其实就是一个transform
  

ros::Time(0)就是从使用这个代码开始的时间，因为监听时间需要记录还是什么的，总之需要一个开始时间

ros::Duration(3.0)是一个超时函数，在监听者这句话多次没有得到两个坐标的广播，在超过这个时间将会发出警告：Warning 大概内容就是 Losing of /turtle1 什么什么的。

lookupTransform的理解：我的理解是这样子的，因为程序的目标是实现，海龟2追着1跑，所以这个函数的意思就是，得到海龟1相对于海龟2的位置，也就是将海龟2视作坐标原点时，海龟1的坐标。因为这里还是一个平面二维坐标，所以这个时候tr里存的就是以海龟2为坐标原点，海龟1的坐标，之前两者的坐标都是相对于world而言的。

对整个程序的总结

整个程序的逻辑顺序

首先是turtlesim_node 通过Publisher传出两只小海龟的geometry::Pose信息，然后程序将Pose信息装入transform，通过tf广播出去，监听者在收到两个小海龟的广播后把海龟2相对海龟1的坐标信息发送过来，然后通过publisher中的/turtle2/cmd_vel话题来对海龟2的移动做出命令，让海龟2开始向海龟1靠近。

关于transform这个类

从ros官网可以找到关于tf类的大部分信息，如下图：

从图中可以看出，transform其实只是一个四元数加上一个三维的坐标，图中的Vector3是一个三维的列向量，也就是一个坐标。

然后对于四元数，我起初是懵逼的，看了一堆材料也看不懂，所以我在这里就直接取四元数的结果，它只是一个表示该坐标相对于世界坐标的姿态。稍微了解一下坐标变换的可以知道，三维坐标是可以通过按着一定顺序绕着某个轴旋转，来改变自己的位置姿态，这就不得不说到欧拉角了，稍微百度一下就知道欧拉角的概念了，欧拉角和四元数之间是可以相互转换的，四元数可以避免万向锁，虽然本身很抽象，但是方便坐标的旋转变换。所以四元数只是用四个数字来表示一个东西的位置姿态，也可以说这个坐标的整体朝向，更贴切点说，就是在当前坐标下海龟头的朝向。

关于数据的初始化

首先把transform的初始化代码放在这里：

tf::Transform transform;//定义一个transform类
transform.setOrigin( tf::Vector3(msg ->x, msg->y, 0.0) );
tf::Quaternion q//定义一个四元数
q.setRPY(0, 0, msg->theta);//设置四元数类中欧拉角的三个
transform.setRotation(q);  //设置transform中的四元数的值
 

这段函数是从订阅者也就是subscriber的回调函数中取出的而subscriber订阅的是/turtle_name/pose的话题，也就是说订阅的消息类型是turtlesim::Pose。

如何知道这个topic下面的消息类型是geometry::pose呢？
因为这个话题是由turtlesim_node发布的，所以我们可以通过打开这个node，然后再另一个端口使用rostopic和rosmsg相配合来得到我们想知道的消息类型内容，例如：

rostopic list //来得到node里所有的消息类型
rostopic info topicname //显示topic的相关信息,Type就是消息类型，以及来源也就是include文件
rosmsg show message//显示该消息的内部包含类型，message可以替换成geometry::pose

//需要查看相关服务的时候
rosservice list//以此类推
//如果实在忘记了函数的用法可以在函数后面加-h查看函数帮助
//for example
rostopic -h
rosservice -h

以上程序可以产生如图效果：

info之后的内容是通过list找到的。
这个方法在读取未知的程序时也有很好的效果还有就是他们之间的关系图

rosrun rqt_graph rqt_graph

就可以得到上面程序总结里的关系图。

回到之前的话题，我们通过rostopic和rosmsg知道了turtlesim::Pose里的消息类型如图：

可以看出，这个专门为海龟定制的消息类新就包括了五个变量
分别代表它的二维坐标，它头的朝向，前进的线速度和角速度。
这个时候回头去看transform的初始化就变得简单了：

transform.setOrigin( tf::Vector3(msg ->x, msg->y, 0.0) );
//因为只是二维空间平面，所以z轴肯定复位0，msg->x就是把海龟的xy赋值。
tf::Quaternion q//定义一个四元数
q.setRPY(0, 0, msg->theta);
//因为是二维平面，海龟它自身是趴着的也不旋转，头也不抬也不低，只会傻不拉几的乱转。
//所以同样只要给欧拉角的第三个参数赋值就好了
transform.setRotation(q);  //设置transform中的四元数的值

关于对海龟2控制的算法

源代码长这样：

geometry_msgs::Twist vel;
vel.angular.z=4.0 * atan2(tr.getOrigin().y(),tr.getOrigin().x());
vel.linear.x= 0.5 * sqrt( pow(tr.getOrigin().x(), 2) + pow(tr.getOrigin().y(),2) );

看到这个算法一开始是一脸懵逼的，那么同样和上面一样，先找geometry_msgs::Twist是什么消息类型，因为这个消息是发送给话题为/turtle1/cmd_vel的，所以可以看这个话题的消息information，就可以得到如下图：
看到这里其实我也不懂为什么只对z和x赋值，这个算法我还不太懂，希望有大佬看到的时候评论我指点迷津。我只是在这里得到结论：因为它是拿lookuptransform传出来的值计算的，然后观察这个计算公式，第一行算的是arctany/x，那把就是斜率吗，那就是找到海龟1相对与2的角度，同理，第二行公式算的就是他们之间的距离，pow显然平方的意思。所以lookuptransform就是找到turtle2为原点，turtle1的坐标。最后把这两个值为什么放到线速度和角速度的这个位置…我也是一脸懵逼的，希望有大佬有幸看到这一行评论指点我QAQ。

关于Stampedtransform消息类型

关于这行发送为什么要用这个类型，源代码如下：

br.sendTransform( tf::StampedTransform( transform, ros::Time::now(),"world",turtle_name) );

从这个网页–官网可以找到这个消息类型：

在官网的最上方可以看出 StampedTransform 这个类是从transform继承过来的，只是比transform多了一个frame和child_frame_id。这两个的理解就是，这个transform里的所有数据都是相对frame而言的，也就是world这个坐标为原点而标注的坐标，这个坐标的名字叫做child_frame_id。

最终总结

记录一下解读代码的全过程，就这