6.TF坐标变换实操

文章发布时间:

最后更新时间:

文章总字数:
1.5k

预计阅读时间:
6 分钟

1、结果演示

img

实现分析：

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

  1. 启动乌龟GUI和键盘控制节点
  2. 调用服务生成一只新乌龟
  3. 编写两只乌龟发布坐标信息的节点
  4. 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

2、package

rospy

roscpp

std_msgs

geometry_msgs

tf2

tf2_ros

tf2_geometry_msgs

turtlesim


3、Launch文件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
<launch>
    <!--
        流程详解：
            1.准备工作：启动乌龟GUI节点和键盘控制节点
            2.调用服务生成一只新乌龟
            3.发布两只乌龟的坐标信息
            4.订阅坐标信息，并且转换成乌龟A相对于乌龟B的坐标信息，最后生成控制乌龟B的速度信息
    -->>
    <!-- 1.启动乌龟GUI节点和键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

    <!-- 2.调用服务生成一只新乌龟 -->
    <node pkg="tftf" type="new_turtle.py" name="turtle2" output="screen" />

    <!-- 3.发布两只乌龟的坐标信息
            1.复用之前的乌龟坐标发布功能
            2.调用节点时，以参数的形式传递乌龟名称，解析参数置换：订阅的话题消息 和 子级坐标系的名称
     -->
    <node pkg="tftf" type="pub_turtle.py" name="pub1" args="turtle1" output="screen" />
    <node pkg="tftf" type="pub_turtle.py" name="pub2" args="turtle2" output="screen" />

    <!-- 4.订阅坐标信息，并且转换成 乌龟A 相对于 乌龟B 的坐标信息，最后生成控制乌龟B的速度信息 -->
    <node pkg="tftf" type="control_turtle.py" name="control" output="screen" />

</launch>

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
#! /usr/bin/env python

# 1.导包
import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

'''
    需求：向服务器发送请求生成一只乌龟
        话题：/spwan
        消息：turtlesim/Spawn
    步骤：
        1.导包
        2.初始化ros节点
        3.创建客户端对象
        4.组织数据并发送请求
        5.处理响应结果
'''

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

    # 3.创建客户端对象
    client = rospy.ServiceProxy("/spawn",Spawn)

    # 4.组织数据并发送请求
    #4-1组织数据
    request = SpawnRequest()
    request.x = 4.5
    request.y = 2.0
    request.theta = -3
    request.name = "turtle2"
    # 4-2判断服务器状态并发送
    client.wait_for_service()
    try:
        response = client.call(request)

        # 5.处理响应
        rospy.loginfo("生成的乌龟名字：%s",response.name)

    except Exception as e:
        rospy.logerr("请求处理异常！")


5、pub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
#! /usr/bin/env python

# 1.导包
import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import sys

'''
    发布方：订阅乌龟的位姿信息，转换成坐标系的相对关系，再发布
    准备：
        话题：/turtle1/pose
        类型：/turtlesim/Pose
    流程：
        1.导包
        2.初始化ros节点
        3.创建订阅对象
        4.回调函数处理
        5.spin
'''

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

# 4.
def dopose(pose):
    # 4-1 创建发布坐标系相对关系的对象
    pub = tf2_ros.StaticTransformBroadcaster()
    # 4-2 将 pose 转换成 坐标相对关系信息
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()

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

    # 子级坐标系相对于父级坐标系的偏移量
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0

    # 四元数
    # 从 欧拉角 转换 四元数
    '''
        乌龟是2D的，不存在 X 上的翻滚，Y 上的俯仰，只有 Z 上的偏航
        0 0 pose.theta
    '''
    qtn = tf.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.发布
    pub.sendTransform(tfs)


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

    # 解析传入的参数（执行launch文件传入的参数：文件全路径 + 传入的参数 + 节点名称 + 日至文件路径）
    if len(sys.argv) != 4:
        rospy.logerr("参数个数错误！")
        sys.exit(1)
    else:
        turtle_name = sys.argv[1]

    # 3.创建订阅对象
    # 修改1：话题名称------------------------------------
    sub = rospy.Subscriber(turtle_name + "/pose",Pose,dopose,queue_size=100)

    # 4.回调函数处理订阅到的消息（核心）

    # 5.spin()
    rospy.spin()


6、sub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
#! /usr/bin/env python

# 1.导包
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped,Twist
import math

if __name__ == "__main__":

    # 2.初始化
    rospy.init_node("dsub")

    # 3.创建订阅对象
    # 3-1 创建缓存对象
    buffer = tf2_ros.Buffer()
    # 3-2 创建订阅对象（将缓存传入）
    sub = tf2_ros.TransformListener(buffer)

    # 4.创建速度消息发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=100)

    # 5.转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            # --- 计算 turtle1 相对于 turtle2 的坐标关系
            '''
                参数1：目标坐标系
                参数2：源坐标系
                参数3：rospy.Time(0)---取时间间隔最近的两个坐标帧（son1 相对 world 与 son2 相对 world）计算结果
                返回值：son1 与 son2 的坐标关系
            '''
            ts = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0))
            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)再开方
            twist.linear.x = 0.5 * math.sqrt(math.pow(ts.transform.translation.x,2) + math.pow(ts.transform.translation.y,2))
            # 角速度 = 系数 * 夹角  ， 夹角 = arctan(y的偏移量，x的偏移量)
            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()

    # 6.回旋
    # rospy.spin()
≡