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 特点
优点：
- 各种数值计算的细节，你不用考虑，tf库可以帮你
- 接口很简洁，会广播和监听就行；
- 问题找的很准，那就是需要维护坐标系之间的关系，尤其是父子坐标系的关系
- 提供了很多工具程序
- 考虑了与时间相关的变换
缺点：
- 树的结构很简单，但有时候很笨拙。对于同级的坐标系，就需要从下到上找共同先辈，然后从这个先辈再往下找，进而确定二者的关系。
- 每个订阅器要想获得某两个坐标系的关系都要搜索同一颗树，这样的开销太大，主要是网络传输的负荷比较大。
- 很难满足实时性的要求，这一点比较显然。这也是为什么TF会将每个变换存10秒钟的数据
- 虽然整体比较容易上手但是很多细节不易理解。比如，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,ztransform.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 解释一下：
引用的库
#include 检测调用该节点是否含有参数，若没有，则报警并退出：
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 解释一下：
主函数里要求程序要有一个参数，也就是乌龟的名字。然后就是基础的节点编程的原理，乌龟发布的位置的话题是：/乌龟的名字/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 目前我们坐标系的命名因该是这样的：world、A……
速度消息发布的话题是这样的：/A/cmd_vel 。
等会儿我们处理的时候，最好按着格式来，一个符号都不要差（虽然有些是可以省略）。
解释一下代码， 这里我们除了用到Twist这个消息，还要使用string对字符串拼接.
#include 初始化节点，并且初始化我们的/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" />
<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" />
<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后，就可以自己做一个机器人并在仿真的世界里探索了！