p187静态坐标变换_python订阅

1.初始代码，有问题，未改进

报错原因：sub还未监听到数据，坐标转化transform就先开始运行了,此时base_link找不到，开始报错

#! /usr/bin/env python
"""  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，
    转换成父级坐标系中的坐标点

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建 TF 订阅对象
        4.组织被转换的坐标点
        5.转化逻辑实现,调用tf封装的算法
        （调用订阅对象的 API 将 4 中的点坐标转换成相对于 base_link 的坐标）
        6.spin() | spinOnce()   #| (逻辑或);python中无spinOnce(),c++使用

"""
# 1.导包
import rospy
import tf2_ros
#import tf2_geometry_msgs
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_p")
    # 3.创建 TF 订阅对象
    #需调用tf2_ros
    #3.1创建缓存对象
    #订阅对象负责订阅数据，订阅后的数据需要保存一下，需要缓存至buffer
    buffer = tf2_ros.Buffer()
    #3.2创建订阅对象（将缓存传入）
    sub = tf2_ros.TransformListener(buffer)

    #4.组织被转换的坐标点

    #以前使用geometry_msgs/PointStamped，
    #但在 tf2 的 python 实现中，tf2 已经封装了一些消息类型，不可以使用 geometry_msgs.msg 中的类型
    #而是使用tf2内置消息

    #ps = tf2_geometry_msgs.PointStamped()#报错：未定义tf2_geometry_msgs，需要import tf2_geometry_msgs
    ps = PointStamped()
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser"  #该坐标点的坐标系参考laser
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0
    
    
    #若运行此代码，出现问题：
    #报错，base_link找不到，原因：监听对象sub创建完毕后，就开始监听发布的坐标关系数据，然后执行坐标转化，
    #sub还未监听到数据，坐标转化transform就先开始运行了,此时base_link找不到，开始报错

    #方案1：转换前休眠1-2s，此后订阅对象已经订阅到数据，之后再执行坐标转化（此时base_link已经有了）
    #本次实验推荐方案2
    #方案2：try  except进行异常处理 #base_link找不到，有异常，执行except提示警告，然后继续循环，
    # 直至找到base_link（订阅到数据），开始执行try (transform坐标变换)，正常输出坐标信息

    #5.转化逻辑实现，调用tf封装的算法
    rate = rospy.Rate(1)#；频率为1 ，模拟雷达循环获取坐标点数据并进行转化
    #实际中坐标点是动态生成的
    while not rospy.is_shutdown():
        #转换实现
        ps_out=buffer.transform(ps,"base_link")#两个输入参数：被转化的坐标点（还未转化），目标坐标系；
        #输出：坐标点被转化后的结果（参考目标坐标系）（将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
                      )

        rate.sleep()

图1 报错

2.改进的代码，使用try-except进行异常处理 

#! /usr/bin/env python
"""  

    订阅方：订阅坐标变换消息，传入被转换的坐标点，调用转换算法
    （订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点）

    实现流程:
        1.导包
        2.初始化 ROS 节点
        3.创建订阅对象
        4.组织被转换的坐标点
        5.转化逻辑实现,调用tf封装的算法
        （调用订阅对象的 API 将 4 中的点坐标转换成相对于 base_link 的坐标）
        6.输出结果
        
        spin() | spinOnce()   #| (逻辑或);python中无spinOnce(),c++使用两个均可使用

"""
# 1.导包
import rospy
import tf2_ros
#import tf2_geometry_msgs
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_p")
    # 3.创建订阅对象
    #需调用tf2_ros
    #3.1创建缓存对象
    #订阅对象负责订阅数据，订阅后的数据需要保存一下，需要缓存至buffer
    buffer = tf2_ros.Buffer()
    #3.2创建订阅对象（将缓存传入）
    sub = tf2_ros.TransformListener(buffer)

    #4.组织被转换的坐标点

    #以前使用geometry_msgs/PointStamped，
    #但在 tf2 的 python 实现中，tf2 已经封装了一些消息类型，不可以使用 geometry_msgs.msg 中的类型
    #而是使用tf2内置消息

    #ps = tf2_geometry_msgs.PointStamped()#报错：未定义tf2_geometry_msgs，需要import tf2_geometry_msgs
    ps = PointStamped()
    ps.header.stamp = rospy.Time.now()#为点ps创建时间戳，id,坐标
    ps.header.frame_id = "laser"  #该坐标点的坐标系参考laser
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0
    
    
    #若运行此代码，出现问题：
    #报错，base_link找不到，原因：监听对象sub创建完毕后，就开始监听发布的坐标关系数据，然后执行坐标转化，
    #sub还未监听到数据，坐标转化transform就先开始运行了,此时base_link找不到，开始报错
    #方案1：转换前休眠1-2s，此后订阅对象已经订阅到数据，之后再执行坐标转化（此时base_link已经有了）
    #本次实验推荐方案2
    #方案2：try  except进行异常处理 #base_link找不到，有异常，执行except提示警告，然后继续循环，
    # 直至找到base_link（订阅到数据），开始执行try (transform坐标变换)，正常输出坐标信息

    #5.转化逻辑实现，调用tf封装的算法
    rate = rospy.Rate(1)#；频率为1 ，模拟雷达循环获取坐标点数据并进行转化
    #实际中坐标点是动态生成的
    while not rospy.is_shutdown():
        try:
            #转换实现
            """ 
                参数1:被转化的坐标点
                参数2:目标坐标系
                返回值:转换后的坐标点

                PS: 
                问题：抛出异常 base_link不存在
                原因：转换函数调用时，可能还没有订阅到坐标系的相对信息
                解决:try 捕获异常，并处理
            """
            ps_out=buffer.transform(ps,"base_link")#两个输入参数：被转化的坐标点（还未转化），目标坐标系；
            #输出：坐标点被转化后的结果（参考目标坐标系）（将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()
        
    

 

图2 实验结果 

第一次报异常，正常情况，表面在进行坐标转换时还未接受到base_link数据，此后接收到base_link数据，开始执行坐标转换。

 图3 节点，话题，计算图

3.应用背景

已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，障碍物原点坐标（2.00,3.00,5.00）(相对于雷达坐标系)，转换至参考坐标（小车），此时障碍物原点坐标为（2.20,3.00,5.50）。

参考资料

1.《ROS理论与实践》

5.1.2 静态坐标变换 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程