ROS Melodic 中使用 Python3 和 tf2 实现位姿的坐标变换

Transform - 变换，Frame - 坐标系，Workspace - 工作空间，Subscriber - 订阅者

写在前面

本文用于在 ROS Melodic 中，使用 Python3 虚拟环境（如 Anaconda），将某一坐标系中的位姿变换到另一坐标系中（即坐标变换）。ROS 中实现该功能的库是 tf（transform 的简称），目前更新至第二代（tf2）。

本文同样适用于解决 tf2_py 出现的 dynamic module does not define module export function (PyInit__tf2) 报错，以及 tf2_ros.Buffer().transform 函数出现的 Type ... if not loaded or supported 报错。

待解决的问题

创建坐标系

坐标变换前，tf 树中应存在坐标系，例如存在 map 和 map_local 两个坐标系，从而我们可以将发布到 map_local 中的位姿（例如 header.frame_id = "map_local" 的 geometry_msgs/PoseStamped）变换到 map 中。

创建坐标系 ROS 有官方教程，参见链接。

指定用 Python3 运行节点

针对这个问题 ROS 官方同样发布了教程，参见链接。

核心思想就是修改 Python 文件开头的 #!（英文名称为 shebang），#! 后接的是 Python 可执行文件的路径。如果使用的是 Python 虚拟环境，可以在命令行中激活环境，输入 python，然后运行以下指令即可打印出路径，将打印出的路径加在 #! 后放在文件开头即可，例如 #! /home/easy/miniconda3/envs/test/bin/python。

import sys
print(sys.executable)

重装 tf2_py 和 tf2_geometry_msgs

对于 ROS Melodic，tf2_py 是针对 Python2写的，如果在 Python3 中 import tf2_py，会出现 dynamic module does not define module export function (PyInit__tf2) 的报错，所以需要重装。我们在 Python3 环境中能够通过 import 找到 tf2_py 这个包是因为安装 ROS 时在 .bashrc 中 source 了 /opt/ros/melodic/setup.bash。

tf2_py 的源码位于 Github，链接，注意链接是 ROS Melodic 分支的，需要下载并取出名为 tf2_py 的文件夹，放到 ROS 工作空间下，上不去 Github 可以下载小节末的百度网盘链接。为了编译生成 Python3 可运行的功能包，需要修改 CMakeLists.txt，在 project 后加入三行，分别是 Python 可执行文件路径、Python 头文件路径、以及 Python 库文件。改好后 catkin build 编译即可，会在 devel/lib/python3/dist-packages 路径下生成 tf2_py 文件夹，注意路径中是 python3，这说明成功生成了 Python3 可运行的功能包。

project(tf2_py)

set(PYTHON_EXECUTABLE /home/easy/miniconda3/envs/test/bin/python)
set(PYTHON_INCLUDE_DIR /home/easy/miniconda3/envs/test/include/python3.9)
set(PYTHON_LIBRARY /home/easy/miniconda3/envs/test/lib/libpython3.9.so)

安装完后，系统同时存在两个 tf2_py，为了使 import 优先引入我们编译生成的 Python3 版本，需要在 import 前加入一行代码，用于在 Python 搜索路径的第一位放入刚刚编译生成功能包的绝对路径，例子如下。当然，使用绝对路径是保证能用的简易方案，这里也可以考虑使用相对路径或环境变量。

sys.path.insert(0,'/home/easy/Repository/ros_ws/devel/lib/python3/dist-packages')
import tf2_ros

tf2_ros.Buffer().transform 函数需要 tf2_geometry_msgs 包，如果在实例化 tf2_ros.Buffer() 前不 import tf2_geometry_msgs 的话，transform 函数会出现 Type ... if not loaded or supported 的报错。import 这个包其实是把能够转换的类型写入了 tf2_ros.TransformRegistration() 中，会在实例化 tf2_ros.Buffer() 的过程中用到。

如果 import tf2_geometry_msgs 显示 no module named 'tf2_geometry_msgs' 的话则需要安装，tf2_geometry_msgs 和 tf2_py 在同一个 Github 仓库中，同样需要放到 ROS 工作空间并编译，tf2_geometry_msgs 的 CMakeLists.txt 无需修改。

tf2_py 和 tf2_geometry_msgs 百度网盘 链接（提取码 y6ze）

实例代码

获取两个坐标系的变换关系

情景：tf 树中存在 map 到 map_local 的变换和 map 到 base_link 的变换，需要获取 base_link 到 map_local 的变换。

#! /home/easy/miniconda3/envs/test/bin/python
import rospy
sys.path.insert(0,'/home/easy/Repository/ros_ws/devel/lib/python3/dist-packages')
import tf2_ros
import tf2_geometry_msgs

tf_buffer = tf2_ros.Buffer()
if tf_buffer.can_transform('map_local', 'base_link', rospy.Time(0)):
    trans = tf_buffer.lookup_transform('map_local', 'base_link', rospy.Time(0))

输出的 trans 中包括 trans.transform.rotation（四元数，表示旋转，包含 xyzw，例如 trans.transform.rotation.x）和 trans.transform.translation（位移，包含 xyz）。如果不加 can_transform 判断的话，在程序运行初期容易报错。

将某一坐标系中的位姿变换到另一坐标系中

情景：将订阅到的 header.frame_id = "base_link"、类型为 visualization_msgs/Marker 的话题数据 data 变换到 map_local 坐标系下。其中 onData 是 Subscriber 的回调函数。

#! /home/easy/miniconda3/envs/test/bin/python
import rospy
sys.path.insert(0,'/home/easy/Repository/ros_ws/devel/lib/python3/dist-packages')
import tf2_ros
import tf2_geometry_msgs
from visualization_msgs.msg import Marker
from geometry_msgs.msg import PoseStamped

def onData(self, data: Marker):
    pose = PoseStamped()
	pose.header = data.header
	pose.pose = data.pose
	
	tf_buffer = tf2_ros.Buffer()
	pose_transformed = tf_buffer.transform(pose, 'map_local')

输出的 pose_transformed 中包括 pose_transformed.pose.orientation（四元数，表示旋转，包含 xyzw，例如 pose_transformed.pose.orientation.x）和 pose_transformed.pose.position（位置，包含 xyz）。