深度相机辅助导航避障(二)：设置坐标变换

上一篇，我们简单介绍了坐标变换，以及TF2可以方便的管理坐标变换树。本篇将介绍在项目开发过程中，如何设置坐标变换。

对于深度相机或者其它固定在机器人底座上不可移动的传感器或部件，需要发布tf2静态变换。

如图所示深度相机安装相对于机器人底座的位置

这里有三种方法来发布静态变换。

设置坐标变换

1.编写代码实现tf2静态变换发布

以C++代码为例，将静态变化发布到tf2，一般需要包含以下头文件

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

geometry_msgs/msg/transform_stamped.hpp使我们可以访问TransformStamped消息类，填充变换数据后将其发布到变换树。

tf2/LinearMath/Quaternion.h提供tf2::Quaternion四元数类，可以方便将欧拉角转换为四元数。

tf2_ros/static_transform_broadcaster.h可以使用StaticTransformBroadcaster发布静态变换消息。

首先创建tf2_ros::StaticTransformBroadcaster

tf_static_broadcaster_ = std::make_unique<tf2_ros::StaticTransformBroadcaster>(*node);

接着创建TransformStamped对象，并填充数据。

geometry_msgs::msg::TransformStamped t;

t.header.stamp = node->get_clock()->now(); //时间戳
t.header.frame_id = "base_link";    //父frame_id，这里为base_link
t.child_frame_id = "camera_link";   //子frame_id，这里为camera_link
/* 填充平移 */
t.transform.translation.x = 0.15;
t.transform.translation.y = 0.0;
t.transform.translation.z = 0.5;
/* 填充旋转 */
tf2::Quaternion q;
q.setRPY( 0.0, 0.78, 0.0);
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

最后广播静态变换

tf_static_broadcaster_->sendTransform(t);

这样即可实现相机坐标静态变换发布。

也可以不用自己写代码，直接使用tf2_ros提供的static_transform_publisher工具来发布静态变换。

2.使用static_transform_publisher实现tf2静态变换发布

tf2_ros提供了一个名为static_transform_publisher的可执行文件，可以用作命令行工具或者添加到启动文件的节点。

以下示例命令行实现使用以米为单位的 x/y/z 偏移量和以弧度为单位的滚动/俯仰/偏航向 tf2 发布相机的静态坐标变换：

#ros2 run tf2_ros static_transform_publisher x  y z yaw pitch roll frame-id child-frame-id 
ros2 run tf2_ros static_transform_publisher 0.15  0.0  0.5  0.0 0.78  0.0 base_link  camera_link

以下示例在launch文件中设置相机的静态坐标变换：

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
             package='tf2_ros',
             executable='static_transform_publisher',
             arguments = [0.15', '0.0', '0.5',  '0.0',  '0.78', '0.0',  'base_link', 'camera_link']
        ),
    ])

3.使用URDF和robot_state_publisher

对于各种传感器很多的复杂的机器人系统，如果一个个加static_transform_publisher，就有点麻烦了，修改起来也不太方便。可以使用robot_state_publisher包来发布变换。

robot_state_publisher可以读取URDF文件（统一机器人描述格式文件），自动处理并发布坐标变换。

URDF是一个代表机器人模型的XML文件，它可以用来描述与机器人几何相关的变换树，也可以通过定义材料和网格等视觉组件，在RVIZ中可视化机器人模型。也可以定义机器人的物理属性，用于gazebo模拟器中的虚拟环境进行仿真运行。

要使用robot_state_publisher需要安装以下软件包：

sudo apt install ros-<ros2-distro>-joint-state-publisher-gui
sudo apt install ros-<ros2-distro>-xacro   #Xacro（XML宏），方便配置重用

对于我们示例的深度相机坐标变换，需要urdf文件中添加以下内容：

<?xml version="1.0"?>
<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${base_length} ${base_width} ${base_height}"/>
      </geometry>
      <material name="red"/>
    </visual>
  </link>
    
  <link name="camera_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="${camera_length} ${camera_width} ${camera_height}"/>
      </geometry>
      <material name="grey"/>
    </visual>
  </link>

  <joint name="camera_joint" type="fixed">
    <parent link="base_link"/>
    <child link="camera_link"/>
    <origin xyz="0.15 0 0.5" rpy="0.0 0.78 0.0"/>
  </joint>
</robot>

编写完URDF，需要创建launch文件，并在launch文件中启动robot_state_publisher节点，读取URDF文件来发布机器人的坐标变换。

更多URDF编写相关的知识可以看navigation.ros.org里的教程：Setting Up The URDF — Navigation 2 1.0.0 documentation (ros.org)

调试坐标变换

使用以下方法可以检查坐标变换是否发布成功。

1.命令行窗口并执行以下命令：

ros2 run tf2_ros tf2_echo base_link camera_link

应该能观察到与以下类似的重复输出。

At time 0.0
- Translation: [0.150, 0.000, 0.500]
- Rotation: in Quaternion [0.000, 0.380, 0.000, 0.925]

2.使用view_frames工具，

ros2 run tf2_tools view_frames

可以在当前路径生成图片文件frames.pdf，打开后可以看到以下输出：

结语

实际开发过程中以上三种坐标变换发布的方法并没有什么优劣之分，主要还是看自己的需求。会灵活使用各种方法，适合自己的才是最好的。