4.多坐标变换

文章发布时间:

最后更新时间:

文章总字数:
412

预计阅读时间:
2 分钟

1、package

rospy

roscpp

std_msgs

geometry_msgs

tf2

tf2_ros

tf2_geometry_msgs


2、Pub

1
2
3
4
<launch>
    <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>

3、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
60
61
62
63
64
65
#! /usr/bin/env python

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

if __name__ == "__main__":

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

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

    # 4.组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()
    # 时间戳 --- 0
    # ps.header.stamp = rospy.Time.now()
    ps.header.stamp = rospy.Time.now()
    # 坐标系
    ps.header.frame_id = "son1"
    ps.point.x = 1.0
    ps.point.y = 2.0
    ps.point.z = 3.0

    # 5.转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            # --- 计算 son1 相对于 son2 的坐标关系
            '''
                参数1：目标坐标系
                参数2：源坐标系
                参数3：rospy.Time(0)---取时间间隔最近的两个坐标帧（son1 相对 world 与 son2 相对 world）计算结果
                返回值：son1 与 son2 的坐标关系
            '''
            ts = buffer.lookup_transform("son2","son1",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
                        )
            # 转换实现
            ps_out = buffer.transform(ps,"son2")

            # 6.输出结果
            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()

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