在Jetson Nano上学习ROS的记录（版本Ubuntu18.04，课程来源赵虚左老师的《ROS理论与实践》）第十章-第二节 TF坐标变换实操

系列文章目录

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

---

前言

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

需求描述:
程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

实现分析:
乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

• 启动乌龟显示节点
• 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
• 编写两只乌龟发布坐标信息的节点
• 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

实现流程:

• 新建功能包，添加依赖
• 编写服务客户端，用于生成一只新的乌龟
• 编写发布方，发布两只乌龟的坐标信息
• 编写订阅方，订阅两只乌龟信息，生成速度信息并发布
• 运行

准备工作:
1.了解如何创建第二只乌龟，且不受键盘控制
创建第二只乌龟需要使用rosservice,话题使用的是 spawn
键盘是无法控制第二只乌龟运动的，因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息
2.了解如何获取两只乌龟的坐标
是通过话题 /乌龟名称/pose 来获取的

一、创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

二、服务客户端(生成乌龟)

代码如下：

#! /usr/bin/env python
#encoding:utf-8
import rospy
from turtlesim.srv import *
"""
    需求：向服务器发送请求生成一只乌龟
        话题：/spawn
        消息：turtlesim/Spawn

        1.导包
        2.初始化ros节点
        3.创建服务的客户端对象
        4.组织数据并发送请求
        5.处理响应结果
"""

if __name__ == "__main__":
    # 2.初始化ros节点
    rospy.init_node("service_call_p")
    # 3.创建服务的客户端对象 参数：name：话题名称, service_class:消息名称
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 4.组织数据并发送请求
    request = SpawnRequest()#创建请求数据对象
    request.x = 4.5
    request.y = 2.0
    request.theta = -3 #向右转-3个弧度
    request.name = "turtle2"
    #判断服务器状态，再发送
    client.wait_for_service()
    try:
        response = client.call(request)      #发送请求
        # 5.处理响应结果
        rospy.loginfo("生成乌龟到名字叫：%s",response.name)
    except Exception as e:
        rospy.logerr("请求处理异常")

三、发布方(发布两只乌龟的坐标信息)

代码如下：

#! /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
import sys
"""
    发布方：订阅乌龟的位姿信息，转换成坐标系的相对关系，再发布
    准备
            话题:/turtle/pose
            类型:/turtlesim/Pose

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

"""
# 接收乌龟名称的全局变量
turtle_name = ""

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

    #将pose转换成坐标系相对关系消息
    ts = TransformStamped()
    ts.header.frame_id = "world"    #被参考的坐标系是世界坐标系 父级坐标系
    ts.header.stamp = rospy.Time.now()

    #修改2：子级坐标系名称-----------------------------------------------------------------------------------------
    ts.child_frame_id = turtle_name
    #修改2：子级坐标系名称-----------------------------------------------------------------------------------------

    #子级坐标系相对于父级坐标系的偏移量
    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")

    # 解析传入的参数（文件全路径 + 传入的参数 + 节点名称 + 日志文件路径）
    if len(sys.argv)!=4:
        rospy.loginfo("参数个数不对")
        sys.exit
    else:
        turtle_name = sys.argv[1]

    # 3.创建订阅对象

    #修改1：话题名称-----------------------------------------------------------------------------------------
    sub = rospy.Subscriber(turtle_name + "/pose",Pose,doPose,queue_size=100)    #name话题名称, data_class消息类型, callback=None回调函数, queue_size=None队列长度
    #修改1：话题名称-----------------------------------------------------------------------------------------

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

四、订阅方(解析坐标信息并生成速度信息)

代码如下：

#! /usr/bin/env python2.7
#encoding:utf-8
import rospy
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped,Twist
import math

if __name__ == "__main__":

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

    #4.创建速度消息发布对象 键盘是无法控制第二只乌龟运动的，
    #因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=100)#话题；消息类型；queue_size

    # 5.转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)#循环发送 频率是10
    while not rospy.is_shutdown():
        try:
            #核心：计算turtle1相对于turtle2的坐标关系
            ts = buffer.lookup_transform("turtle2","turtle1",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
                                        )

            #组织Twist消息
            twist = Twist()
            #发布线速度和角速度
            #线速度 = 系数*两个坐标系原点的间距 = 系数*(x^2+y^2)再开方
            #角速度 = 系数*两个坐标系之间的夹角=系数*arctan(y,x)
            twist.linear.x = 0.5 * math.sqrt(math.pow(ts.transform.translation.x,2)+math.pow(ts.transform.translation.y,2))#线速度
            twist.angular.z=4*math.atan2(ts.transform.translation.y,ts.transform.translation.x)
            #发布消息
            pub.publish(twist)
        except Exception as e:
            rospy.logwarn("错误提示：%s",e)

        rate.sleep()

五、运行

使用 launch 文件组织需要运行的节点，代码如下：

<launch>

    
    
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen"/>

    
    
    <node pkg="tf04_test" type="test01_new_turtle_p.py" name="turtle2" output="screen"/>

    
    <node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub1" args="turtle1" output="screen"/>
    <node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub2" args="turtle2" output="screen"/>

    
    <node pkg="tf04_test" type="test03_control_turtle2_p.py" name="control" output="screen"/>
    
launch>

打开一个命令行：

cd workspace
roslaunch tf04_test test.launch 

六、小结

坐标变换在机器人系统中是一个极其重要的组成模块，在 ROS 中 TF2 组件是专门用于实现坐标变换的，TF2 实现具体内容又主要介绍了如下几部分:

1.静态坐标变换广播器，可以编码方式或调用内置功能包来实现(建议后者)，适用于相对固定的坐标系关系
2.动态坐标变换广播器，以编码的方式广播坐标系之间的相对关系，适用于易变的坐标系关系
3.坐标变换监听器，用于监听广播器广播的坐标系消息，可以实现不同坐标系之间或同一点在不同坐标系之间的变换
4.机器人系统中的坐标系关系是较为复杂的，还可以通过 tf2_tools 工具包来生成 ros 中的坐标系关系图
5.当前 TF2 已经替换了 TF，官网建议直接学习 TF2，并且 TF 与 TF2 的使用流程与实现 API 比较类似，只要有任意一方的使用经验，另一方也可以做到触类旁通

总结

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