从头手搓一台ros2复合机器人（带机械臂）

一.前言
        

        大家好呀，通过本次学习，我们可以更加直观的看到坐标之间的转化过程，并且我们要学习ros中一个很重要的可视化工具rviz2，话不多说，马上开始！

二.ros2学习

      前11小节内容可以订阅往期博客。

12.可视化坐标变换

首先，我们发布生成B,C,P坐标系之间的关系

打开rviz2

窗口左边的配置选项，全局选项默认选择的FixedFrame为map，这个map就是rviz2默认的坐标系的名字，我们并没有发布map这个坐标系，所以下面Global Status也是红色的错误。

此时我们可以手动的修改以下固定的Frame为B，让默认的坐标系设置成机械臂的基坐标系{B}。

添加TF插件

点击左下角的Add，在弹出的窗口中选择TF点击OK

之后在RVIZ2中就可以看到下图的坐标关系

13.坐标变换发布监听Python实现

接下来我们尝试使用代码来操作tf

TF相关的操作：

• 广播（TransformBroadcaster）：将坐标系与坐标系之间的位姿态关系发布出去
• 监听（TransformListener）：获取坐标系和坐标系之间的位姿关系
• TF(坐标变换TransformStamped)帧：广播出去的一组数据可以称作一个TF帧，包含父坐标系名称、子坐标系名称，父坐标系和子坐标系之间的关系

接着我们就可以编写代码来实现上面对TF帧的发布和监听。

13.1 坐标变换广播

我们根据两个坐标系之间的关系是否会随着时间变化分成以下两种情况：

1. 坐标系之间的关系不随时间推移而改变，称为静态坐标变换，需要使用静态广播发布器（StaticTransformBroadcaster）发布。比如：机器人的两个轮子之间关系，可以认为随时间的变换其相对位置不变。
2. 坐标系之间的关系随时间的推移而改变，称为（动态）坐标变换，使用广播发布器（TransformBroadcaster）发布坐标关系。比如机器人在世界坐标系中的位置，因为机器人会动。

        在手眼系统里，尝试使用广播发布器来发布坐标系之间的关系。 在手眼系统中，机械臂基座和相机坐标系之间的位置是固定不变的，我们可以通过静态广播发布器来发布，而相机坐标系下工件的位置是实时识别得到的，其值会随时间变化，故我们用广播发布器来发布。

13.2 静态广播发布器

在jupyter中输入并运行以下程序，使用方式就像ros2的话题发布，只不过不需要指名发布的话题（发布到系统的TF树上）。

导入rclpy和Node

导入TF帧，使用下面命令查看接口类型：

从tf2_ros包中导入静态坐标变化广播器

初始化节点

构造静态广播发布器

构造TF帧，发布其要发送的数据

这里四元数的值需要通过在线的坐标转换获取，选择角度，绕x轴旋转180，上面就是对应的四元数，x,y,z,w为1,0,0,0

Quaternions - Visualisation--在线坐标转换

使用命令行监听坐标关系

在前面加上！表示在命令行中输入

【*】表示代码正在运行

注意这里输出的At time 0.0 代表任意时刻

13.3 广播发布器（动态）

接着我们来使用广播发布器发布C和工具P之间的关系平移：

x:2 y:1 z:2  旋转:qx:0 qy:0 qz:0 qw:1

需要注意，发布坐标变换时要以一定的频率实时发布，这样当我们获取坐标时才能获取到当前时刻的坐标

以10Hz发布坐标关系

使用命令行获取到C和P之间关系

通过命令行获取到B和P之间关系，完成手眼转换

注意这里的结果中的时间：At time 1709728823.791592781代表具体的某一个时刻，不同时刻坐标之间的平移和旋转可以不同

13.4 坐标变换监听

坐标变换监听就是监听整个系统的坐标变换关系。

通过TransformListener即可获取到整个tf系统中窗口大小为10s的坐标关系，并且我们创建了一个buffer，TransformListener会把收到的坐标关系放入buffer中，我们后面就可以通过buffer的lookup_transform()函数获取到坐标之间的关系。

可以看到最终打印的结果和我们上面用命令行和numpy计算结果一致。