ROS学习笔记9：TF坐标变换实操

前言

本人ROS小白，利用寒假时间学习ROS，在此以笔记的方式记录自己每天的学习过程。争取写满20篇(9/20)。
环境：Ubuntu20.04、ROS1：noetic
环境配置：严格按照下方学习链接的教程配置，基本一次成功。
学习链接：【Autolabor初级教程】ROS机器人入门
对应链接文档：ROS机器人入门课程《ROS理论与实践》
笔记绝大部分代码使用Python语言编写。
本期关键词：乌龟跟随

要求

产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行。

思路：

1. 生成乌龟
2. 发布方：发布两只乌龟的坐标信息
3. 订阅方：解析坐标信息并生成速度信息

一、生成乌龟

新乌龟的生成与服务调用有关。这里新乌龟的生成方法和以前我们提到过的基本一致，这里直接展示示例代码：

#! usr/bin/env python

import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

if __name__=="__main__":
    rospy.init_node("turtle_create1")
    client=rospy.ServiceProxy("/spawn",Spawn)
    
    # wait for service
    client.wait_for_service()
    
    # send request
    req=SpawnRequest()
    req.x=3
    req.y=3
    req.theta=0
    req.name="my_turtle"
    
    try:
        response = client.call(req)
        rospy.loginfo("turtle create successfully! Named: %s",response.name)
        rospy.loginfo("turtle's location: x=%.2f,y=%.2f",req.x,req.y)
    except Exception as e:
        rospy.logerr("create false!")

配置launch文件，如下：

<launch>

    <node pkg="turtlesim" type="turtlesim_node" name="t1" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="t2" />
    <node pkg="helloworld" type="double_turtle1.py" name="t3" />

launch>

注意：如果launch文件的"type"参数包含的是Python文件，那么一定要加上".py"的后缀，不然会报错！

二、发布方实现

1. 难点：发布两只乌龟的坐标信息
2. 在之前的学习过程中，我们发布过一只乌龟的坐标信息，而本次实操我们需要发布两只乌龟的坐标信息，代码要如何实现呢？
3. 利用Python的系统模块 sys来获取输入参数，从而实现对两只乌龟坐标信息的发布，示例代码如下：

#! /usr/bin/env python

import rospy
import sys
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import tf2_ros
import tf_conversions

turtle_name = ""

def doPose(pose):
    #1.创建坐标系广播器
    broadcaster = tf2_ros.TransformBroadcaster()
    #2.将 pose 信息转换成 TransFormStamped
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()

    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0

    qtn = tf_conversions.transformations.quaternion_from_euler(0, 0, pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    #3.广播器发布 tfs
    broadcaster.sendTransform(tfs)


if __name__ == "__main__":
    rospy.init_node("sub_tfs_p")
    # 需要和launch文件的args（将参数传递给节点）搭配使用
    if len(sys.argv) < 2:
        rospy.loginfo("请传入参数:乌龟的命名空间")
    else:
        # 第一个参数默认是脚本名称
        turtle_name = sys.argv[1]

    # 位姿话题：/turtle1/pose /turtle2/pose
    rospy.Subscriber(turtle_name + "/pose",Pose,doPose)

    rospy.spin()

4. 对应的launch文件代码：

<launch>

    <node pkg="turtlesim" type="turtlesim_node" name="t1" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="t2" />

    <node pkg="helloworld" type="double_turtle1.py" name="t3" output="screen"/>

    <node pkg="helloworld" type="double_turtle2.py" name="t4" args="turtle1" output="screen" />
    <node pkg="helloworld" type="double_turtle2.py" name="t5" args="turtle2" output="screen" />

launch>

5. 还有另一种方法，就是创建两个订阅者，分别监听两个乌龟的位姿话题，示例代码如下：

#! /usr/bin/env python

import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
import tf.transformations
from geometry_msgs.msg import TransformStamped

def doPose1(pose):
    # 创建一个广播器
    broadcaster=tf2_ros.TransformBroadcaster()
    # 创建要被广播的数据
    d_pose=TransformStamped()
    d_pose.header.frame_id="world"
    d_pose.header.stamp=rospy.Time.now()
    d_pose.child_frame_id="turtle1"
    d_pose.transform.translation.x=pose.x
    d_pose.transform.translation.y=pose.y
    d_pose.transform.translation.z=0.0
    qua=tf.transformations.quaternion_from_euler(0,0,pose.theta)
    d_pose.transform.rotation.x=qua[0]
    d_pose.transform.rotation.y=qua[1]
    d_pose.transform.rotation.z=qua[2]
    d_pose.transform.rotation.w=qua[3]
    
    broadcaster.sendTransform(d_pose)
    
def doPose2(pose):
    # 创建一个广播器
    broadcaster=tf2_ros.TransformBroadcaster()
    # 创建要被广播的数据
    d_pose=TransformStamped()
    d_pose.header.frame_id="world"
    d_pose.header.stamp=rospy.Time.now()
    d_pose.child_frame_id="turtle2"
    d_pose.transform.translation.x=pose.x
    d_pose.transform.translation.y=pose.y
    d_pose.transform.translation.z=0.0
    qua=tf.transformations.quaternion_from_euler(0,0,pose.theta)
    d_pose.transform.rotation.x=qua[0]
    d_pose.transform.rotation.y=qua[1]
    d_pose.transform.rotation.z=qua[2]
    d_pose.transform.rotation.w=qua[3]
    
    broadcaster.sendTransform(d_pose)
    
    
if __name__=="__main__":
    rospy.init_node("dynamic_pub")
    # 我们要根据/turtle1/pose的数据来设置tf坐标系的数据，所以先订阅/turtle1/pose
    sub1=rospy.Subscriber("/turtle1/pose",Pose,doPose1)
    sub2=rospy.Subscriber("/turtle2/pose",Pose,doPose2)
    
    rospy.spin()

6. 对应的launch文件为：

<launch>

    <node pkg="turtlesim" type="turtlesim_node" name="t1" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="t2" />

    <node pkg="helloworld" type="double_turtle1.py" name="t3" output="screen"/>
    <node pkg="helloworld" type="double_turtle2_test.py" name="t7" output="screen" />
    <node pkg="helloworld" type="double_turtle3.py" name="t6" output="screen" />

launch>

7. 经过测试，以上方法也是可行的，不过对比之下，个人认为第一种方法更好。

三、订阅方实现

1. 订阅方实现只需要将两只乌龟的坐标关系进行比较，然后通过距离设置第二只乌龟的运动参数即可，示例代码如下：

#! /usr/bin/env python

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped, Twist
import math

if __name__ == "__main__":
    rospy.init_node("sub_tfs_p")

    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(10)
    # 创建速度发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=1000)
    
    while not rospy.is_shutdown():
        try:
            # turtle1-turtle2
            trans = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
            twist = Twist()
            # 间距 = x^2 + y^2  然后开方
            # 通过距离（角度）推理速度，简单的公式转换，参数可根据需求自行设置
            twist.linear.x = 2 * math.sqrt(math.pow(trans.transform.translation.x,2) + math.pow(trans.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)

            pub.publish(twist)

        except Exception as e:
            rospy.logwarn("警告:%s",e)
        
        rate.sleep()

2. 对应的launch文件：

<launch>

    <node pkg="turtlesim" type="turtlesim_node" name="t1" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="t2" />
    
    <node pkg="helloworld" type="double_turtle1.py" name="t3" output="screen"/>
    <node pkg="helloworld" type="double_turtle2.py" name="t4" args="turtle1" output="screen" />
    <node pkg="helloworld" type="double_turtle2.py" name="t5" args="turtle2" output="screen" />
    <node pkg="helloworld" type="double_turtle3.py" name="t6" output="screen" />

launch>

往期内容

1. ROS学习笔记1：基础知识
2. ROS学习笔记2：话题通信
3. ROS学习笔记3：服务通信与参数服务器
4. ROS常用命令记录
5. ROS学习笔记4：通信机制实操
6. ROS学习笔记5：常用API和模块导入
7. ROS学习笔记6：launch文件
8. ROS学习笔记7：重名解决与名称设置
9. ROS学习笔记8：TF坐标变换