ROS学习记录11——tf坐标变换与多海龟跟随

零.前言

关于tf的内容，我也没有彻底理解透彻，只能讲述个大概，若有错误，请评论区斧正，谢谢。
在《机器人基础》这个课里，有推导关于机器人坐标变换的说明，当然，我没学，学了也因为当时数学不好没听懂，所以这篇文章也不会讲太多数学层面的东西。

一.概念

1.1 引言

我们知道，物体的位置是相对的。
那么在描述一个系统中例如三个物体的位置关系，我们可以通过假设A为原点，计算B相对于A的坐标，C相对于A的坐标。这样可以通过A+坐标的方式表示B，C的位置。
当然，此时我们也可以把B当作坐标原点，根据刚才的坐标，我们可以得到反推得到A相对于B的坐标，又已知AC间的坐标关系，那么就可以通过代数计算得到C相对于B的坐标。
ROS中的坐标也是如此！

1.2 思想

ROS中，所有的坐标关系是通过一颗树形结构组成的，就像这样：

这样做的好处是：假设A为我们认为的参照物，把它看作为原点，那么BD一起向某个方向移动了相同的距离。那么我们用绝对坐标的方式需要会改变B和D这两个物体的坐标。
但是通过这种树形结构的方式，我们只需要记住D和B的相对位置，那么我们只需要更改B相对于A的位置，在需要时，即可计算出D的位置，这样可以有效地简化坐标系变化时的坐标变动的数目，每个目标只用专心处理自己和目标坐标系的关系，不用考虑其他坐标系的关系。

1.3 概念

两个坐标系间的tf消息组成是这样的：

    header: 
      seq: 0
      stamp: 
        secs: 1621956538
        nsecs: 108681917
      frame_id: "parent"
    child_frame_id: "child"
    transform: 
      translation: 
        x: -0.45
        y: 0.2
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

描述的是从frame_id变换到child_frame_id的坐标关系。很多很多这样的消息就可以构成一棵tf消息树。完成整个系统的坐标描述
上面还有roscore运行的时间戳，能够记录一段时间的实时坐标消息。

1.4 使用

那么我们需要做的：

• 广播tf变换
• 监听tf变换

通过广播，把变动的两个坐标系的关系发布出去，然后由ros底层提供处理，然后并通过话题发布出来，然后需要用到坐标系时，监听某两个坐标系间的关系即可（当然，这个关系可能不是直接得出，得经过计算）。不过，复杂的计算和其他问题的处理我们不需要关心。我们只需要发布，监听即可。

1.5 特点

优点：

1. 各种数值计算的细节，你不用考虑，tf库可以帮你
2. 接口很简洁，会广播和监听就行；
3. 问题找的很准，那就是需要维护坐标系之间的关系，尤其是父子坐标系的关系
4. 提供了很多工具程序
5. 考虑了与时间相关的变换

缺点：

1. 树的结构很简单，但有时候很笨拙。对于同级的坐标系，就需要从下到上找共同先辈，然后从这个先辈再往下找，进而确定二者的关系。
2. 每个订阅器要想获得某两个坐标系的关系都要搜索同一颗树，这样的开销太大，主要是网络传输的负荷比较大。
3. 很难满足实时性的要求，这一点比较显然。这也是为什么TF会将每个变换存10秒钟的数据
4. 虽然整体比较容易上手但是很多细节不易理解。比如，now（）和time（0）；比如，技术文档里的一些术语名词；比如，采用了机器人里的习惯，与飞行器，惯导，车辆里的习惯区别较大，使用时不能想当然。

二.代码实现

2.1 广播程序

    header: 
      seq: 0
      stamp: 
        secs: 1621956538
        nsecs: 108681917
      frame_id: "parent"
    child_frame_id: "child"
    transform: 
      translation: 
        x: -0.45
        y: 0.2
        z: 0.0
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

以上是一个tf消息，我们分析该消息可以得到：我们可能需要修改以下内容的值：stamp frame_id child_frame_id translation: rotation:
通过查阅官方手册，我们可以得知：

• br.sendTransform(); 该函数可以发送我们的tf变换的消息
• tf::StampedTransform(transform, ros::Time::now(), child_frame_id, frame_id); 该函数可以创建一条我们需要发送的tf消息。他会自动更根据ros::Time::now()的时间，为我们添加stamp这样的时间戳。
• transform是一个类，它包含了translation、 rotation这两个tf需要用到的元素。
• transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) ) 可以修改transform.translation的值，参数为相对于父坐标系的x,y,z
• transform.setRotation( tf::Quaternion(0, 0, 0, 1) )可以修改transform.rotation的值，参数为我们学过的2X2的旋转矩阵。
• tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q);但是手动计算这个旋转矩阵太麻烦了，所以我们可以通过创建Quaternion对象，并通过rpy作为参数，实例化该对象，它会自动根据rpy的三个参数创建旋转矩阵。

整个结构像这样：

至此，我们可以通过x,y,z,r,p,y这六个我们认识的参数来创建并广播我们的tf消息了。

2.2 监听程序

监听程序很简单：
listener.lookupTransform(child_frame_id, frame_id, ros::Time(0), transform);

• 参数除了第三个ros::Time(0)，其余的都在广播程序里讲过。
• ros::Time(0)代表 （tf树由后台计算处理） 最新可用的tf树的时间。若此处使用
  整个ROS的时间系统是相对于独立的，使用的是ros的内部时间，所以要实现延迟跟随，得用该方法获得x秒前的时间ros::Time::now(); ros::Time past = now - ros::Duration(x);

三.实战——多海龟跟随

3.1 创建功能包

现在跳过工作空间、工作空间的环境配置。这些是基础，就不赘述。
直接创建功能包：

catkin_create_pkg turtle_follow roscpp geometry_msgs tf turtlesim

3.2 创建乌龟发布节点

生成海龟的原理：是向Spwan这个服务提供一个请求，这个过程可以参考我的
这是海龟生成的服务Spwan.srv

float32 x
float32 y
float32 theta
string name # Optional.  A unique name will be created and returned if this is empty
---
string name

那么根据服务编程的原则，我们可以写出以下代码：

#include <ros/ros.h>
#include <turtlesim/Spawn.h>
 
int main(int argc, char** argv)
{
    int i;
    if (argc < 2)
    {
        ROS_ERROR("Turtle's name can't be empty");
        return -1;
    }
    ros::init(argc, argv, "turtle_new");
    ros::NodeHandle node;
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv_msg;
    for (i = 1; i < argc; ++i)
    {
        srv_msg.request.x = i;
        srv_msg.request.y = i;
        srv_msg.request.theta = 0;
        srv_msg.request.name = *(argv + i);
        add_turtle_client.call(srv_msg);
    }
    return 0;
}

解释一下：

引用的库

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

检测调用该节点是否含有参数，若没有，则报警并退出：

    if (argc < 2)
    {
        ROS_ERROR("Turtle's name can't be empty");
        return -1;
    }

初始化节点：

    ros::init(argc, argv, "turtle_new");
    ros::NodeHandle node;

等待spawn服务创建（因为roslaunch里启动不分顺序，在这里必须等待，确保该服务存在），然后初始化一个客户端，并创建一条srv_msg服务的数据结构，准备访问服务：

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle_client = node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv_msg;

通过循环，更改乌龟的初始坐标和名字，然后进行访问。（乌龟在二维平面上运动，所以只有x轴，y轴，和yaw角）

    for (i = 1; i < argc; ++i)
    {
        srv_msg.request.x = i;
        srv_msg.request.y = i;
        srv_msg.request.theta = 0;
        srv_msg.request.name = *(argv + i);
        add_turtle_client.call(srv_msg);
    }

将上面的总体代码创建为turtle_new.cpp
添加CMakeList:

add_executable(turtle_new src/turtle_new.cpp)
target_link_libraries(turtle_new ${catkin_LIBRARIES})

编译，然后在三个终端里按顺序运行：

roscore
rosrun turtlesim turtlesim_node
rosrun turtle_follow turtle_new A B C D E

得到结果：

然后查看话题，成功创建五只乌龟，还有自带的turtle1，有强迫症的同学，可以通过编写kill服务去杀死那只turtle1。

3.2 创建坐标广播器

首先我们先看一下乌龟的姿态信息：rostopic echo /A/pose

x: 1.0
y: 1.0
theta: 0.0
linear_velocity: 0.0
angular_velocity: 0.0
---

这是我们创建时，相对于world这个坐标系的相对坐标：xy轴，y角，线速度，角速度。
所以，我们要将该/pose话题提供的信息，转换成tf消息并发出。代码如下：

#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::Transform transform;
    tf::Quaternion quaternion;
    
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    quaternion.setRPY(0, 0, msg->theta);
    transform.setRotation(quaternion);
 
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
 
int main(int argc, char** argv)
{

    ros::init(argc, argv, "turtle_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    }
    
    turtle_name = *(argv + 1); // = argv[1]
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
 
    ros::spin();
    
    return 0;
};

解释一下：
主函数里要求程序要有一个参数，也就是乌龟的名字。然后就是基础的节点编程的原理，乌龟发布的位置的话题是：/乌龟的名字/pose，我们需要订阅该话题，该话题发布后，我们就进入回调函数，并广播tf消息。

int main(int argc, char** argv)
{

    ros::init(argc, argv, "turtle_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    }
    
    turtle_name = *(argv + 1); // = argv[1]
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
 
    ros::spin();
    
    return 0;
};

回调函数里，创建了 一个广播器br，姿态信息transorm，和旋转矩阵quaternion。
在2.1里详细的描写了该过程，就不赘述。

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    tf::Quaternion quaternion;
    
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    quaternion.setRPY(0, 0, msg->theta);
    transform.setRotation(quaternion);
 
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

将我们的代码写入turtle_broadcaster.cpp：

添加依赖：

add_executable(turtle_broadcaster src/turtle_broadcaster.cpp)
target_link_libraries(turtle_broadcaster ${catkin_LIBRARIES})

编译运行：

roscore
rosrun turtlesim turtlesim_node
rosrun turtle_follow turtle_new A
rosrun turtle_follow turtle_broadcaster A

此时我们可以通过：rostopic echo /tf查看我们的tf树，或者使用rosrun tf tf_echo [reference_frame] [target_frame]查看任意两个坐标系的变换，或者使用rosrun rqt_tf_tree rqt_tf_tree在rqt里可视化查看：

3.3 创建坐标监听器，并发布速度话题

先rostopic echo /turtle1/cmd_vel看看/cmd_vel 这个话题是啥样的：

无论怎么移动小乌龟，他只有x,z轴数据在变，所以我也懒得找文献是为什么了，直接给这两个速度赋值就行。
那么我们监听器的代码是：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <string>
using namespace std;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtle_listener");
    ros::NodeHandle node;
    string follower_vel("/");
    if (argc != 3)
    {
        ROS_ERROR("Need two names of the turtles!");
        return -1;
    }
    follower_vel.append(argv[1]);
    follower_vel.append("/cmd_vel");
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(follower_vel, 10);
    tf::TransformListener listener;
 
    ros::Rate rate(10.0);

    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            listener.lookupTransform(argv[1], argv[2], ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
 
        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;
};

目前我们坐标系的命名因该是这样的：world、A……
速度消息发布的话题是这样的：/A/cmd_vel 。
等会儿我们处理的时候，最好按着格式来，一个符号都不要差（虽然有些是可以省略）。

解释一下代码， 这里我们除了用到Twist这个消息，还要使用string对字符串拼接.

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <string>

初始化节点，并且初始化我们的/X/cmd_vel这个话题的字符串。因为是跟随，所以我们需要两个参数。

    ros::init(argc, argv, "turtle_listener");
    ros::NodeHandle node;
    string follower_vel("/");
    if (argc != 3)
    {
        ROS_ERROR("Need two names of the turtles!");
        return -1;
    }

添加字符串，完善follower_vel这个字符串，等会儿launch只输入乌龟名即可，并创建Twist消息的发布器和tf监听器

    follower_vel.append(argv[1]);
    follower_vel.append("/cmd_vel");
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>(follower_vel, 10);
    tf::TransformListener listener;

这个是获得两个坐标系的transform信息，用try起来是因为 本身这是个异步过错，容易出错。等会儿运行时就会看到真的会出错。

        tf::StampedTransform transform;
        try
        {
            listener.lookupTransform(argv[1], argv[2], ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

创建消息，通过坐标x，y得到角度差距，并求得y轴的角速度，和欧式距离下的线速度，并发布消息。

        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();

将我们的代码写入：

添加依赖：

add_executable(turtle_listener src/turtle_listener.cpp)
target_link_libraries(turtle_listener ${catkin_LIBRARIES})

这样就完成了。

3.4 编写launch文件

在功能包下创建launch文件夹，并创建launch文件：

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

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 创建一个乌龟节点 -->
    <node pkg="turtle_follow" type="turtle_new" args="A" name="turtle_new" />

    <!-- 创建两个乌龟的tf广播器 -->
    <node pkg="turtle_follow" type="turtle_broadcaster" args="A" name="turtle1_broadcaster" />
 	<node pkg="turtle_follow" type="turtle_broadcaster" args="turtle1" name="turtle2_broadcaster" />
 
    <!-- 第一个参数的乌龟跟随第二个参数的乌龟 -->
    <node pkg="turtle_follow" type="turtle_listener" args="A turtle1" name="listener1" />
 </launch>

这个launch文件就不需要太多的解释了，启动节点，添加参数，不懂的话可以看上一篇文章的内容。
然后运行看效果：

roslaunch turtle_follow follow.launch

看，因为tf坐标系还没来得及建立而产生的错误：

如果没有一直出现红色的错误信息，说明代码无误，在这个终端用方向键移动乌龟：

千辛万苦写出来的代码不来点儿好玩儿的？6只乌龟开火车！：

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

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <!-- 创建五个乌龟节点 -->
    <node pkg="turtle_follow" type="turtle_new" args="A B C D E" name="turtle_new" />

    <!-- 创建六个乌龟的tf广播器 -->
    <node pkg="turtle_follow" type="turtle_broadcaster" args="A" name="turtle1_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="B" name="turtle2_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="C" name="turtle3_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="D" name="turtle4_broadcaster" />
    <node pkg="turtle_follow" type="turtle_broadcaster" args="E" name="turtle5_broadcaster" />
 	<node pkg="turtle_follow" type="turtle_broadcaster" args="turtle1" name="turtle0_broadcaster" />
 
    <!-- 第一个参数的乌龟跟随第二个参数的乌龟 -->
    <node pkg="turtle_follow" type="turtle_listener" args="A turtle1" name="listener1" />
    <node pkg="turtle_follow" type="turtle_listener" args="B A" name="listener2" />
    <node pkg="turtle_follow" type="turtle_listener" args="C B" name="listener3" />
    <node pkg="turtle_follow" type="turtle_listener" args="D C" name="listener4" />
    <node pkg="turtle_follow" type="turtle_listener" args="E D" name="listener5" />
 </launch>

3.5 Rviz显示

在运行节点后，我们启动Rviz:rosrun rviz rviz，并在将参考坐标系调整为world，左下角添加TF可视化的插件，即可在右侧区域看到我们运动的tf坐标树

四.后记

要深入学习tf，还是得多看官方文档、手册。接下来学习了xacro、gazebo后，就可以自己做一个机器人并在仿真的世界里探索了！