在Jetson Nano上学习ROS的记录（版本Ubuntu18.04，课程来源赵虚左老师的《ROS理论与实践》）第十章-第一节 TF坐标变换（内含PyKDL 和PyInit__tf2功能缺失等解决）

W-ilde

已于 2022-07-07 16:15:08 修改

阅读量1.3k

点赞数 3

分类专栏： ROS学习文章标签： python 嵌入式硬件 linux ubuntu 单片机

于 2022-06-29 23:05:04 首次发布

本文链接：https://blog.csdn.net/layra_liu/article/details/125530838

版权

ROS学习专栏收录该内容

17 篇文章 25 订阅

订阅专栏

系列文章目录

第一章 ROS空间创建、helloworld的实现、开启多个节点
第二章话题通信
第三章服务通信
第四章参数服务器
第五章常用指令
第六章通信机制实操
第七章 ROS通信机制进阶（常用API、Python模块的导入)
第八章元功能包、节点运行管理launch文件（teleop_twist安装方法）
第九章重名问题、分布式通信
第十章-第一节 TF坐标变换（内含PyKDL 和PyInit__tf2功能缺失等解决）
第十章-第二节 TF坐标变换实操
第十章-第三节 rosbag、rqt工具箱
第十一章-第一节机器人系统仿真（URDF相关）
第十一章-第二节机器人系统仿真（Gazebo相关）
第十二章机器人导航(仿真)

前言

现在大二，之前大一有幸参加了2021的国赛，很壮烈的拿了个江苏赛区的二等奖。但发现无人机这个题，真的是往堆钱上走了。不上ROS不行，现在来记录一下一个纯小白学习ROS的过程和遇到的问题。防止学弟、学妹们再走我走过的弯路。板子用的是学长给的Jetson Nano（4GB），版本是Ubuntu18.04（已配置好基础ROS所需配置）

一、ROS常用组件

在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件:

TF坐标变换，实现不同类型的坐标系之间的转换；
rosbag 用于录制ROS节点的执行过程并可以重放该过程；
rqt 工具箱，集成了多款图形化的调试工具。

二、TF坐标变换

当然，根据我们高中学习的知识，在明确了不同坐标系之间的的相对关系，就可以实现任何坐标点在不同坐标系之间的转换，但是该计算实现是较为常用的，且算法也有点复杂，因此在 ROS 中直接封装了相关的模块: 坐标变换(TF)。

tf:TransForm Frame,坐标变换
坐标系:ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。作用在 ROS 中用于实现不同坐标系之间的点或向量的转换。

1. 坐标msg消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped
前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

命令行键入:rosmsg info geometry_msgs/PointStamped
输出

std_msgs/Header header   #头信息
  uint32 seq	#序列号
  time stamp	#时间戳
  string frame_id  #我的坐标点是以哪个参考系为参考物的 所属坐标系的 id
geometry_msgs/Point point	#坐标点的坐标值
  float64 x
  float64 y
  float64 z

命令行键入:rosmsg info geometry_msgs/PointStamped
此命令是描述两个坐标系相关位置信息
输出

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id    #参考系坐标id
string child_frame_id  #子坐标系的 id  B相对于A坐标系的关系。即B是child
geometry_msgs/Transform transform	#坐标信息
  geometry_msgs/Vector3 translation	 #偏移量
    float64 x		 #X 方向的偏移量
    float64 y		 #y 方向的偏移量
    float64 z		 #z 方向的偏移量
  geometry_msgs/Quaternion rotation	#四元数 欧拉角的另一种表现形式
    float64 x
    float64 y
    float64 z
    float64 w

2.静态坐标变换

所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。

需求描述:
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

实现分析:
坐标系相对关系，可以通过发布方发布
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

实现流程:

新建功能包，添加依赖
编写发布方实现
编写订阅方实现
执行并查看结果

创建功能包
依赖: tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
给予权限
修改cmakelists
发布方
代码如下：

#! /usr/bin/env python2.7
#encoding:utf-8
from platform import python_version
print(python_version())
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf.transformations import quaternion_from_euler

"""
    发布方实现:发布两个坐标系的相对关系(车辆底盘———base_link和雷达——laser)
    流程:
        1.导包
        2.初始化ros节点
        3.创建发布对象
        4.组织被发布的数据
        5.发布数据
        6.spin()回旋函数 不使用的话,发布后就结束了
"""


if __name__ == "__main__":
    # 2.初始化ros节点
    rospy.init_node("static_pub_p")

    # 3.创建发布对象
    pub = tf2_ros.StaticTransformBroadcaster()

    # 4.组织被发布的数据 描述两个坐标系相关位置信息rosmsg info geometry_msgs/TransformStamped
    ts = TransformStamped()
    # header
    ts.header.stamp = rospy.Time.now () #时间戳
    ts.header.frame_id = "base_link"#参考系坐标id，即小车底盘id
    #child frame
    ts.child_frame_id = "laser"
    #相对关系（偏移与四元数）
    ts.transform.translation.x = 2.0
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5
    #先从欧拉角转换成四元数
    qtn = quaternion_from_euler(0,0,0)#雷达相对于小车的欧拉角
    #再设置四元数
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    # 5.发布数据
    pub.sendTransform(ts)
    # 6.spin（）#回旋函数 不使用的话，发布后就结束了
    rospy.spin()

这里很有可能出现问题
1：ImportError: dynamic module does not define module export function (PyInit__tf2)
在这里插入图片描述

2：ModuleNotFoundError: No module named ‘defusedxml
3：no modle named PyKDL 等问题
在这里插入图片描述

其实这里是python2与3环境混乱的原因
在ros官网上有给回答：
在这里插入图片描述

GitHub上也给了解答，上面的解决方式是重建python，利用虚拟环境

另外也可以使用博主的方法，设置python3的软链接。
以上均为我试过，然后失败的方法。我使用的硬件是Jetson Nano，版本为Ubuntu18.04。经过尝试，也有一部分凑巧的原因，我发现导入tf_conversions是我问题的源头，于是我直接导入from tf.transformations import quaternion_from_euler并将编译环境指定成python2.7：#! /usr/bin/env python2.7
成功了，且roslaunch turtle_tf2 turtle_tf2_demo.launch亦正常运行。

打开一个命令行：roscore
再开一个命令行

cd 工作空间
source ./devel/setup.bash
rosrun tf01_static demo01_static_pub_p.py

检验方式：
第一种方式：
再开一个命令行：rostopic list
输出：

/rosout
/rosout_agg
/tf_static

rostopic echo /tf_static

输出：

transforms: 
 1. 
    header: 
      seq: 0
      stamp: 
        secs: 1656506787
        nsecs: 767760992
      frame_id: "base_link"
    child_frame_id: "laser"
    transform: 
      translation: 
        x: 2.0
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

第二种方式：
打开一个命令行：rviz
选择base——link
add TF
在这里插入图片描述

订阅方

代码如下：

#! /usr/bin/env python2.7
#encoding:utf-8
import rospy
import tf2_ros
import tf2_geometry_msgs
"""
    订阅方：订阅坐标变换消息，传输被转换的坐标点，调用转换算法

    流程：
        1.导包
        2.初始化
        3.创建订阅对象
        4.组织被转换的坐标点
        5.转换逻辑实现，调用tf封装的算法
        6.输出结果
        7.spin()

"""
if __name__ == "__main__":

    # 2.初始化
    rospy.init_node("static_sub_p")
    # 3.创建订阅对象
    #3-1创建缓存对象
    buffer = tf2_ros.Buffer()
    #3-2创建订阅对象（将缓存存入）
    sub = tf2_ros.TransformListener(buffer)

    # 4.组织被转换的坐标点 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser" #坐标系是雷达坐标系
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0 #障碍物在雷达坐标系下的坐标

    # 5.转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)#循环发送 频率是10
    while not rospy.is_shutdown():
        try:
            #转换实现
            ps_out = buffer.transform(ps,"base_link") #第一个参数：被转换的坐标点；第二个参数：转换到的坐标系
            #输出结果
            rospy.loginfo("转换后的坐标(%.2f,%.2f,%.2f),参考的坐标系:%s",
                                        ps_out.point.x,
                                        ps_out.point.y,
                                        ps_out.point.z,
                                        ps_out.header.frame_id
                                        )
        except Exception as e:
            rospy.logwarn("错误提示：%s",e)

        rate.sleep()

这里顺便记录一下玄学问题，在vscode里我的rospy.loginfo一直显示无效，在循环里。
在这里插入图片描述

新开一个命令行：roscore

新开一个命令行：

cd workspace
source ./devel/setup.bash
rosrun tf01_static demo01_static_pub_p.py

新开一个命令行：

cd workspace
source ./devel/setup.bash
rosrun tf01_static demo02_static_sub_p.py

补充
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser

也建议使用该种方式直接实现静态坐标系相对信息发布。

3.动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述:
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

实现分析:

乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
订阅 turtle1/pose,可以获取乌龟在世界坐标系的x坐标、y坐标、偏移量以及线速度和角速度
将 pose 信息转换成坐标系相对信息并发布

实现流程:

新建功能包，添加依赖
创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
创建坐标相对关系订阅方
执行

创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
发布方
代码如下：

#! /usr/bin/env python2.7
#encoding:utf-8
import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf.transformations import quaternion_from_euler
"""
    发布方：订阅乌龟的位姿信息，转换成坐标系的相对关系，再发布
    准备
            话题:/turtle/pose
            类型:/turtlesim/Pose

    流程：
            1.导包
            2.初始化ROS节点
            3.创建订阅对象
            4.回调函数处理订阅到的消息（核心）
            5.spin()

"""

#回调函数：
def doPose(pose):   #参数是订阅到的消息
    #创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()

    #将pose转换成坐标系相对关系消息
    ts = TransformStamped()
    ts.header.frame_id = "world"    #被参考的坐标系是世界坐标系 父级坐标系
    ts.header.stamp = rospy.Time.now()
    ts.child_frame_id = "turtle1"
    
    #子级坐标系相对于父级坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0 #2D 没有z轴

    # 四元数
    #从欧拉角转换四元数
    """
        乌龟是2D的，不存在x上的翻滚，y上的俯仰，只有z上的偏航
        欧拉角：0 0 pose.theta
    """
    qtn = quaternion_from_euler(0,0,pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    #发布
    pub.sendTransform(ts)

if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("dynamic_pub_p")

    # 3.创建订阅对象
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)    #name话题名称, data_class消息类型, callback=None回调函数, queue_size=None队列长度

    # 4.回调函数处理订阅到的消息（核心）
    # 5.spin()
    rospy.spin()

打开一个命令行：roscore
打开一个命令行：rosrun turtlesim turtlesim_node
打开一个命令行：rosrun turtlesim turtle_teleop_key
打开一个命令行：

cd工作空间
source ./devel/setup.bash 
rosrun tf02_dynamic demo01_dynamic_pub_p.py

打开一个命令行：rviz

订阅方
代码如下：

#! /usr/bin/env python2.7
#encoding:utf-8
import rospy
import tf2_ros
import tf2_geometry_msgs
"""
    订阅方：订阅坐标变换消息，传输被转换的坐标点，调用转换算法

    流程：
        1.导包
        2.初始化
        3.创建订阅对象
        4.组织被转换的坐标点
        5.转换逻辑实现，调用tf封装的算法
        6.输出结果

"""
if __name__ == "__main__":

    # 2.初始化
    rospy.init_node("static_sub_p")
    # 3.创建订阅对象
    #3-1创建缓存对象
    buffer = tf2_ros.Buffer()
    #3-2创建订阅对象（将缓存存入）
    sub = tf2_ros.TransformListener(buffer)

    # 4.组织被转换的坐标点 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
    ps = tf2_geometry_msgs.PointStamped()
    #时间戳 --0
    ps.header.stamp = rospy.Time()  #不设置时间
    ps.header.frame_id = "turtle1" #坐标系是乌龟坐标系
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0 #障碍物在雷达坐标系下的坐标

    # 5.转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)#循环发送 频率是10
    while not rospy.is_shutdown():
        try:
            #转换实现
            ps_out = buffer.transform(ps,"world") #第一个参数：被转换的坐标点；第二个参数：转换到的坐标系
            #输出结果
            rospy.loginfo("转换后的坐标(%.2f,%.2f,%.2f),参考的坐标系:%s",
                                        ps_out.point.x,
                                        ps_out.point.y,
                                        ps_out.point.z,
                                        ps_out.header.frame_id
                                        )
        except Exception as e:
            rospy.logwarn("错误提示：%s",e)

        rate.sleep()

这里需要注意，不设置now时间，以免时间异常报错
打开一个命令行：roscore
打开一个命令行：rosrun turtlesim turtlesim_node
打开一个命令行：rosrun turtlesim turtle_teleop_key
打开一个命令行：

cd工作空间
source ./devel/setup.bash 
rosrun tf02_dynamic demo01_dynamic_pub_p.py

打开一个命令行：

cd工作空间
source ./devel/setup.bash
rosrun tf02_dynamic demo02_dynamic_sub_p.py

4.多坐标变换

需求描述:
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

实现分析:

首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
最后，还要实现坐标点的转换

实现流程:

新建功能包，添加依赖
创建坐标相对关系发布方(需要发布两个坐标相对关系)
创建坐标相对关系订阅方
执行

创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
发布方
为了方便，使用静态坐标变换发布，采用launch文件
代码如下：

<launch>
    <!--发布 son1 相对于world 以及 发布 son2 相对于world 的坐标关系 args前面三个是xyz的偏移量，然后是欧拉角，父级坐标系，子级坐标系-->
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 0 0 0 0 0 /world /son1" output = "screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="3 0 0 0 0 0 /world /son2" output = "screen" />

</launch>

打开一个命令行：

cd workspace
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch

打开一个命令行：rviz

订阅方
代码如下：

#! /usr/bin/env python2.7
#encoding:utf-8
from gettext import translation
from cv2 import transform
import rospy
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":

    # 2.初始化
    rospy.init_node("frame_sub_p")
    # 3.创建订阅对象
    #3-1创建缓存对象
    buffer = tf2_ros.Buffer()
    #3-2创建订阅对象（将缓存存入）
    sub = tf2_ros.TransformListener(buffer)

    # 4.组织被转换的坐标点 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
    ps = tf2_geometry_msgs.PointStamped()
    #时间戳 --0
    ps.header.stamp = rospy.Time()  #不设置时间
    ps.header.frame_id = "son1" #坐标系是son1坐标系
    ps.point.x = 1.0
    ps.point.y = 2.0
    ps.point.z = 3.0 #障碍物在son1坐标系下的坐标

    # 5.转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)#循环发送 频率是10
    while not rospy.is_shutdown():
        try:
            #核心：计算son1相对于son2的坐标关系
            ts = buffer.lookup_transform("son2","son1",rospy.Time(0))   #target_frame: 目标坐标系, source_frame: 原坐标系, time: 0 即时间间隔最近的关系 返回值：son1与son2的坐标系关系
            rospy.loginfo("父级坐标系:%s,子级坐标系:%s,偏移量(%.2f,%.2f,%.2f)",
                                        ts.header.frame_id,
                                        ts.child_frame_id,
                                        ts.transform.translation.x,
                                        ts.transform.translation.y,
                                        ts.transform.translation.z
                                        )
            #转换实现
            ps_out = buffer.transform(ps,"son2") #第一个参数：被转换的坐标点；第二个参数：转换到的坐标系
            #输出结果
            rospy.loginfo("转换后的坐标(%.2f,%.2f,%.2f),参考的坐标系:%s",
                                        ps_out.point.x,
                                        ps_out.point.y,
                                        ps_out.point.z,
                                        ps_out.header.frame_id
                                        )
        except Exception as e:
            rospy.logwarn("错误提示：%s",e)

        rate.sleep()

打开一个命令行：

cd workspace
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch

打开一个命令行：rviz

打开一个命令行：

cd workspace
source ./devel/setup.bash
rosrun tf03_tfs demo01_tfs_p.py

5.坐标系关系查看

在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:
注意我的版本是ubuntu18.04，ros版本是melodic
采用以下命令安装：sudo apt install ros-melodic-tf2-tools

打开一个命令行：

cd workspace
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch

打开一个命令行：rosrun tf2_tools view_frames.py
输出

[INFO] [1656589364.526404]: Listening to tf data during 5 seconds...
[INFO] [1656589369.567883]: Generating graph in frames.pdf file...

会在当前路径下生成pdf文件
输入evince frames.pdf即可查看

总结

以上就是今天要讲的内容，本文仅仅简单记录了TF坐标变换，如果有问题请在博客下留言或者咨询邮箱：layraliu@foxmail.com。

W-ilde

关注

3
点赞
踩
6

收藏

觉得还不错? 一键收藏
2
评论
在Jetson Nano上学习ROS的记录（版本Ubuntu18.04，课程来源赵虚左老师的《ROS理论与实践》）第十章-第一节 TF坐标变换（内含PyKDL 和PyInit__tf2功能缺失等解决）

第一章 ROS空间创建、helloworld的实现、开启多个节点第二章话题通信第三章服务通信第四章参数服务器第五章常用指令第六章通信机制实操第七章 ROS通信机制进阶（常用API、Python模块的导入)第八章元功能包、节点运行管理launch文件（teleop_twist安装方法）第九章重名问题、分布式通信第十章-第一节 TF坐标变换（内含PyKDL 和PyInit__tf2功能缺失等解决）现在大二，之前大一有幸参加了2021的国赛，很壮烈的拿了个江苏赛区的二等奖。但发现无
复制链接

扫一扫