2.静态坐标变换

文章发布时间:

最后更新时间:

文章总字数:
834

预计阅读时间:
3 分钟

1、package

rospy

roscpp

std_msgs

geometry_msgs

tf2

tf2_ros

tf2_geometry_msgs


2、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
#! /usr/bin/env python

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

'''
    发布方：发布两个坐标系的相对关系（车辆底盘：base_link ， 雷达：laser）
    流程：
        1.导包
        2.初始化ros节点
        3.创建发布者对象
        4.组织被发布的数据
        5.发布数据
        6.spin()
'''

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

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

    # 4.组织数据消息
    ts = TransformStamped()
    # 4-1 header
    ts.header.stamp = rospy.Time.now()
    ts.header.frame_id = "base_link"
    # 4-2 child_frame
    ts.child_frame_id = "laser"
    # 4-3 偏移量
    ts.transform.translation.x = 0.2
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5
    # 4-4 四元数：子坐标相对于父坐标
    # 先从欧拉角转换成四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    '''
        第1个参数：围绕 X轴 的 翻滚度
        第2个参数：围绕 Y轴 的 俯仰度
        第3个参数：围绕 Z轴 的 偏航度
        单位：rad
        返回值：四元数的列表
    '''
    # 设置四元数
    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()


rviz：

image-20230827145214196


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
66
#! /usr/bin/env python

# 1.导包
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
# from tf2_geometry_msgs import PointStamped

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

if __name__ == "__main__":

    # 2.初始化ros节点
    rospy.init_node("static_sub")

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

    # 4.组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser"
    ps.point.x = 2.0
    ps.point.y = 5.0
    ps.point.z = 3.0

    # 5.转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            # 转换实现
            '''
            param 1：被转换的座标点
            param 2：目标坐标系
            返回值：输出的目标点

            ps:
                问题：抛出异常 base_link 不存在
                原因：转换函数调用时，可能还没有订阅到坐标系的相对关系
                解决：try 捕获异常，并处理
            '''
            ps_out = buffer.transform(ps,"base_link")

            # 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()


4、补充

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

1
2
3
4
5
# rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

# 欧拉角：z偏航角度 y俯仰角度 x翻滚角度（单位：rad）

rosrun tf2_ros static_transform_publisher 0.2 0.0 0.5 0 0 0 base_link laser
≡