ROS 多级tf坐标转换

题目

现有一移动机器人，该机器人的基坐标系为“base_link”，机器人包含3个子坐标系分别为“joint1”，“joint2”，“joint3”。

要求：利用多坐标转换，实现joint1下的坐标向joint2下的坐标转换，joint2下的坐标向joint3下的坐标转换。静态坐标的位置发布和最终的程序启动采用launch文件形式，基于已有功能包进行发布，编写多坐标变换的订阅方实现，打印输出两个转换后的坐标信息

已知坐标关系：

joint1相对于base_link的坐标为：x = 0.2 y = 0.0 z = 0.3

joint2相对于base_link的坐标为：x = 0.5 y = 0.1 z = 0.4

joint3相对于base_link的坐标为：x = 0.3 y = 0.3 z = 0.0

已知坐标点：

joint1坐标系下的坐标为：（1.0，1.0，1.0）

joint2坐标系下的坐标为：（2.0，2.0，2.0）

方案

一.订阅方（text_tf_sub.py）

在my_tf功能包下的scripts文件夹下创建python文件text_tf_sub.py
撰写代码

#! /usr/bin/env python

"""
订阅方
"""

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped


def one_two():
    # tfs = sub.lookup_transform("joint2","joint1",rospy.Time(0))
    # 创建消息对象
    source_point = PointStamped()  # 使用正确的消息类型    
    source_point.header.stamp = rospy.Time.now()
    source_point.header.frame_id = "joint1"    
    source_point.point.x = 1.0     # x偏移量
    source_point.point.y = 1.0 
    source_point.point.z = 1.0
    target_point_1 = sub.transform(source_point, "joint2",rospy.Duration(1))  
    
    return target_point_1


def two_three():
    # tfs = sub.lookup_transform("joint2","joint1",rospy.Time(0))
    # 创建消息对象
    source_point = PointStamped()  # 使用正确的消息类型    
    source_point.header.stamp = rospy.Time.now()
    source_point.header.frame_id = "joint2"
    source_point.point.x = 2.0
    source_point.point.y = 2.0
    source_point.point.z = 2.0
    target_point_2 = sub.transform(source_point, "joint3",rospy.Duration(1))  
    
    return target_point_2


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

    # 创建缓冲区对象
    sub = tf2_ros.Buffer()

    # 创建TF订阅对象
    listener = tf2_ros.TransformListener(sub)

    # 设置发布频率
    rate = rospy.Rate(1)

    while not rospy.is_shutdown():

        try:
            target_point_1 = one_two()
            target_point_2 = two_three()
            rospy.loginfo("joint1向joint2的坐标转换: x = %.2f, y = %.2f, z = %.2f ",target_point_1.point.x,target_point_1.point.y,target_point_1.point.z)
            rospy.loginfo("joint2向joint3的坐标转换: x = %.2f, y = %.2f, z = %.2f \n",target_point_2.point.x,target_point_2.point.y,target_point_2.point.z)
        except Exception as e:
            rospy.logwarn("不可用")

        rate.sleep()
    rospy.spin()

在CMakeLists.txt中添加scripts/text_tf_sub.py

从终端进入工作空间下的src下的功能包的scripts ~/ros_ws/src/my_tf/scripts为python文件添加可执行权限

chmod +x *.py

二.发布方（text_tf_pub.launch）

在my_tf下的launch文件夹下创建 text_tf_pub.launch
撰写launch文件

<?xml version="1.0"?>
<launch>

    <node name="joint1" pkg="tf2_ros" type="static_transform_publisher" args="0.2 0.0 0.3 0 0 0 /base_link /joint1" output="screen"/>
    <node name="joint2" pkg="tf2_ros" type="static_transform_publisher" args="0.5 0.1 0.4 0 0 0 /base_link /joint2" output="screen"/>
    <node name="joint3" pkg="tf2_ros" type="static_transform_publisher" args="0.3 0.3 0 0 0 0 /base_link /joint3" output="screen"/>
    <node name="text_tf_sub" pkg="my_tf" type="text_tf_sub.py" output="screen"/>
   

</launch>