TF_静态坐标系之间的坐标转化——求解一个点在另一个坐标系下的位置

最新推荐文章于 2024-09-13 19:41:31 发布

学习心得

最新推荐文章于 2024-09-13 19:41:31 发布

阅读量959

点赞数 29

分类专栏： ROS_入门项目文章标签： python

本文链接：https://blog.csdn.net/ccy1522929997/article/details/141085750

版权

ROS_入门项目专栏收录该内容

11 篇文章 0 订阅

订阅专栏

需求

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

示例

在这里插入图片描述

实现逻辑

即：当两个坐标系的相对位置不发生改变时，可以用该消息结合TF功能，将父坐标系下的物体坐标，直接转移到子坐标系下的物体坐标。
具体流程：

首先要知道坐标系的相对关系，因此编写一个发布方，传递坐标系的相对关系信息
其次，订阅方接收到相对坐标关系后再进行处理。

代码实现

相对坐标关系的发布方：

#! usr/bin/env python

import rospy
import tf.transformations
import tf2_ros
from geometry_msgs.msg import TransformStamped  #坐标系变换
import tf #实现旋转角到四元数的转换

'''
需求描述:现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，
        已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。
        需要发布相对坐标关系的信息
实现流程：
    1. 导入包
    2. 初始化节点
    3. 创建tf静态发布者_广播器
    4. 组织tf数据
    5. 发布tf变换
    6. 循环发布

'''

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


    #创建静态发布，广播器
    static_tf_pub = tf2_ros.StaticTransformBroadcaster()

    #组织发布的tf数据
    static_tf_msg = TransformStamped()
    '''
    std_msgs/Header header                     #头信息
    uint32 seq                                #|-- 序列号
    time stamp                                #|-- 时间戳
    string frame_id                            #|-- 坐标 ID
    string child_frame_id                    #子坐标系的 id
    geometry_msgs/Transform transform        #坐标信息
    geometry_msgs/Vector3 translation        #偏移量
        float64 x                                #|-- X 方向的偏移量
        float64 y                                #|-- Y 方向的偏移量
        float64 z                                #|-- Z 方向上的偏移量
    geometry_msgs/Quaternion rotation        #四元数
        float64 x                                
        float64 y                                
        float64 z                                
        float64 w
    '''
    # header数据-----------------------------------------------------
    static_tf_msg.header.frame_id = "base_link"
    static_tf_msg.header.stamp = rospy.Time.now()
    # 子坐标系数据-----------------------------------------------------
    static_tf_msg.child_frame_id = "child_link"
    # 坐标数据-----------------------------------------------------
    # 偏移量数据-----------------------------------------------------
    static_tf_msg.transform.translation.x = 0.2
    static_tf_msg.transform.translation.y = 0.0
    static_tf_msg.transform.translation.z = 0.5

    # 四元数数据-----------------------------------------------------
    #需要从旋转角进行相应的zhuan换
    qtn = tf.transformations.quaternion_from_euler(0,0,0)  #返回四元数列表
    static_tf_msg.transform.rotation.x = qtn[0]
    static_tf_msg.transform.rotation.y = qtn[1]
    static_tf_msg.transform.rotation.z = qtn[2]
    static_tf_msg.transform.rotation.w = qtn[3]

    #使用广播器发送消息
    static_tf_pub.sendTransform(static_tf_msg)

    #rospy.spin() :将进程挂起
    rospy.loginfo("静态坐标系发布成功")
    rospy.spin()

相对坐标关系订阅方并处理数据


#! usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped
'''
需求描述:接收相对坐标关系，再根据坐标系关系(当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，
        该障碍物相对于主体的坐标是多少？
实现步骤：
    1.导包
    2.初始化ros节点
    3.创建订阅者对象
    4.解析订阅者收到的数据
    5.创建发布者对象
    6.组织要发布的数据
    7.发布数据
    8.spin()

'''

if __name__ == "__main__":

    # 2.初始化ros节点
    rospy.init_node('sub_tf_static')

    # 3.创建tf订阅者对象

    # 3.1创建缓存对象
    buffer = tf2_ros.Buffer()
    # 3.2创建tf订阅者对象：订阅者对象需要缓存对象作为参数，当tf广播后，缓存对象会自动更新
    listener = tf2_ros.TransformListener(buffer)
    # 3.3休眠2秒，等待tf广播
    rospy.sleep(2.0)
    rospy.loginfo("相对坐标位置已经收")

    # 4.解析订阅者收到的数据
    rate = rospy.Rate(1)

    #循环的目的：循环解析订阅者收到的数据，可以不循环
    # 
    if True:
        # 已经有了相对坐标关系，明确子坐标系下的物体坐标
        child_link_pose = PointStamped()
        '''
        std_msgs/Header header                    #头
            uint32 seq                                #|-- 序号
            time stamp                                #|-- 时间戳
            string frame_id                            #|-- 所属坐标系的 id
        geometry_msgs/Point point                #点坐标
            float64 x                                    #|-- x y z 坐标
            float64 y
            float64 z
        
        '''
        #需要注意，这里的id要与相对坐标系下的坐标系id一致
        child_link_pose.header.frame_id = "child_link"
        #时间戳需要指定，为最近发布的时间戳
        child_link_pose.header.stamp = rospy.Time.now()
        child_link_pose.header.seq = 101

        # 物体位置坐标信息-----------------------------------
        child_link_pose.point.x = 2.0
        child_link_pose.point.y = 3.0
        child_link_pose.point.z = 5.0
        rospy.loginfo('buffer: %s')

        #---------------------将物体坐标位置与两个坐标系相对位置关系结合，实现转换---------------------------
        target_link_pose = buffer.transform(child_link_pose, "world")

        rospy.loginfo("在child_link坐标系下的物体坐标为：%f,%f,%f",child_link_pose.point.x,child_link_pose.point.y,child_link_pose.point.z)
        rospy.loginfo("在world坐标系下的物体坐标为：%f,%f,%f",target_link_pose.point.x,target_link_pose.point.y,target_link_pose.point.z)
    rate.sleep()

结果展示：
在这里插入图片描述