ROS海龟跟随-理解TF坐标变换

本篇我们在海龟仿真器中，通过一个例程（turtle_tf）来理解TF的作用，并且熟悉之前学到的TF工具。该例程的功能包turtle_tf可以使用如下命令进行安装：

sudo apt-get install ros-melodic-turtle-tf
ROUTEROS

安装完成后，就可以使用如下命令运行例程了：

roslaunch turtle_tf turtle_tf_demo.launch
APPLESCRIPT

海龟仿真器打开后会出现两只小海龟，并且下方的小海龟自动向中心位置的小海龟移动

键盘控制一只海龟移动，另一只会跟随着一起移动

另外一只海龟总是会跟随我们控制的那只海龟运行。在这个例程中，TF是如何运用的呢？我们首先使用TF工具来看一下这个例程中的TF树是什么样的：

rosrun tf view_frames
EBNF

在当前系统中存在三个坐标系：world、turtle1、turtle2。world是世界坐标系，作为系统的基础坐标系，其他坐标系都相对该坐标系建立，所以world是TF树的根节点。相对于world坐标系，又分别针对两只海龟创建了两个海龟坐标系，这两个坐标系的原点就是海龟在世界坐标系下的坐标位置。
现在要让turtle2跟随turtle1运动，相当于turtle2坐标系向turtle1坐标系移动，这就需要知道turtle2与turtle1之间的坐标变换。三个坐标系之间的变换关系可以使用如下公式描述：

使用tf_echo工具在TF树中查找海龟坐标系之间的变换关系：

rosrun tf tf_echo turtle1 turtle2
JBOSS-CLI

也可以通过rviz的图形界面更加形象的看到这三者之间的坐标关系：

rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
GRADLE

得到turtle2与turtle1之间的坐标变换后，就可以计算两只海龟间的距离和角度，即可控制turtle2向turtle1移动了。
接下来，我们以这个例程为目标，学习如何实现TF的广播和监听功能。

创建TF广播器

首先，我们需要创建一个发布海龟坐标系与世界坐标系之间TF变换的节点，实现源码turtle_tf_broadcaster.cpp的具体内容如下：

#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];

// 订阅海龟的pose信息
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

ros::spin();

return 0;
}
REASONML

以上代码的关键部分是处理海龟pose消息的回调函数poseCallback。在广播TF消息之前需要定义tf::TransformBroadcaster广播器，然后根据海龟当前的位姿，设置tf::Transform类型的坐标变换，包含setOrigin设置的平移变换以及setRotation设置的旋转变换。
然后使用广播器将坐标变换插入TF树并且发布，这里发布的TF消息类型是tf::StampedTransform，不仅包含tf::Transform类型的坐标变换、时间戳，而且还需要指定坐标变换的源坐标系（parent）和目标坐标系（child）。

创建TF监听器

TF消息广播之后，其他节点就可以监听该TF消息，从而获取需要的坐标变换了。
目前我们已经将海龟相对于world坐标系的TF变换广播，接下来需要监听TF消息，并从中获取turtle2相对于turtle1坐标系的变换，从而控制turtle2移动。实现源码turtle_tf_listener.cpp的详细内容如下：

#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;

// 通过服务调用，产生第二只乌龟turtle2
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);

// 定义turtle2的速度控制发布器
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

// tf监听器
tf::TransformListener listener;

ros::Rate rate(10.0);
while (node.ok())
{
tf::StampedTransform transform;
try
{
// 查找turtle2与turtle1的坐标变换
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需要运动的线速度和角速度
// 并发布速度控制指令，使turtle2向turtle1移动
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;
}
CPP

该节点首先通过服务调用产生海龟turtle2，然后声明控制turtle2速度的Publisher。在监听TF消息之前，需要创建一个tf::TransformListener类型的监听器，创建成功后监听器会自动接收TF树的消息，并且缓存10秒。
然后在循环中就可以实时查找TF树中的坐标变换了，这里需要调用的是tf::TransformListener中的两个接口：

waitForTransform(const std::string &target_frame, const std::string&source_frame, const ros::Time&time, const ros::Duration &timeout)
REASONML

给定的源坐标系（source_frame）和目标坐标系（target_frame），等待两个坐标系之间指定时间（time）的变换关系，该函数会阻塞程序运行，所以需要设置超时时间（timeout）。

lookupTransform(const std::string &target_frame,const std::string &source_frame, const ros::Time & time,StampedTransform &transform)
REASONML

给定的源坐标系（source_frame）和目标坐标系（target_frame），得到两个坐标系之间指定时间（time）的坐标变换（transform），ros::Time(0) 表示我们想要的是最新一次的坐标变换。
通过以上两个接口的调用，就可以获取turtle2相对于turtle1的坐标变换了。然后根据坐标系之间的位置关系，计算得到turtle2需要运动的线速度和角速度，并发布速度控制指令，使turtle2向turtle1移动。

实现海龟跟随运动

现在小海龟跟随例程的所有代码都已经完成，我们来编写一个launch文件，将所有节点运行起来，start_demo_with_listener.launch：

<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>

<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

<!-- 两只海龟的tf广播 -->
<node pkg="tf_demo" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />

<node pkg="tf_demo" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

<!-- 监听tf广播，并且控制turtle2移动 -->
<node pkg="tf_demo" type="turtle_tf_listener"
name="listener" />

</launch>
XML

运行该launch文件，就可以看到与之前例程类似的两只海龟的界面了，在终端中通过键盘控制turtle1移动，turtle2也跟随移动。

设置不同颜色区分海龟的轨迹

如果想用不同颜色区分小海龟的轨迹可以在新终端打开rqt

rqt
EBNF

然后在service下拉选项选择/turtle1/set_pen设置第一个小海龟轨迹的颜色

然后点击call完成设置

然后键盘控制海龟移动，海归的轨迹显示了不同的颜色

参考文章

参考文章