ROS 多坐标变换(已整理)

多坐标变换

发布方

建立launch文件

<launch>
    <!-- 发布 son1 相对于 world 以及 son2 相对于 world 的关系 -->
    <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> 

roscore
cd demo02_ws/
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch(先启动)
rosrun tf03_tfs demo01_tfs_sub(开新的窗口)

son1 相对于 son2 的信息: 父级: son2,子级: son1 偏移量(2.00,0.00,0.00)
son1 相对于 son2 的信息: 父级: son2,子级: son1

新开一个窗口：rviz

订阅方

C++实现

#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"geometry_msgs/PointStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/TransformStamped.h"

/*
    订阅方实现: 1. 计算 son1 与 son2 的相对关系 2. 计算 son1 的某个坐标点在 son2 中的坐标值

    流程:
        1. 包含头文件
        2. 编码，初始化 NodeHandle
        3. 创建订阅对象
        4. 编写解析逻辑
        5. spinOnce()
*/

int main(int argc, char *argv[])
{
    // 2. 编码，初始化 NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;

    // 3. 创建订阅对象 头文件2/3
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    // 4. 编写解析逻辑

    // 创建坐标点 头文件4/5
    geometry_msgs::PointStamped psAtSon1;

    psAtSon1.header.stamp = ros::Time::now();
    psAtSon1.header.frame_id = "son1";
    psAtSon1.point.x = 1.0;
    psAtSon1.point.y = 2.0;
    psAtSon1.point.z = 3.0;

    ros::Rate rate(10);
    while(ros::ok())
    {
        // 核心
        try
        {
            // 1. 计算 son1 与 son2 的相对关系
            /*

                A相对于B的坐标系关系
                参数1: 目标坐标系 B
                参数2: 源坐标系 A
                参数3: ros::Time(0)取间隔最短的两个坐标关系帧来计算相对关系
                返回值: geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
            */
            geometry_msgs::TransformStamped son1ToSon2 =  buffer.lookupTransform("son2","son1",ros::Time(0)); 
            // 目标坐标系，源坐标系，时间0表示查找最近两个坐标系消息
            ROS_INFO("son1 相对于 son2 的信息: 父级: %s,子级: %s 偏移量(%.2f,%.2f,%.2f)",
                    son1ToSon2.header.frame_id.c_str(), // 父节点 son2
                    son1ToSon2.child_frame_id.c_str(), // 子节点 son1
                    son1ToSon2.transform.translation.x,
                    son1ToSon2.transform.translation.y,
                    son1ToSon2.transform.translation.z
                    ); 
            // 2. 计算 son1 的某个坐标点在 son2 中的坐标值
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2"); 
            // 前者是被转换的坐标点,后者是转换的坐标系
            ROS_INFO("坐标点在 Son2 中的值(%.2f,%.2f,%.2f)",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z
                    );

        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示: %s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    // 5. spinOnce()

    return 0;
}

Python实现

转换后的坐标:(3.00,2.00,3.00),参考的坐标系: son2 父级坐标系: son2,子级坐标系: son1, 偏移量(2.00,0.00,0.00)

#! /usr/bin/env python


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("tfs_sub_p")

    # 3. 创建订阅对象
    # 3.1 创建一个缓存对象 头文件2
    buffer = tf2_ros.Buffer()
    # 3.2 创建订阅对象(将缓存传入)
    sub = tf2_ros.TransformListener(buffer)

    # 4. 组织被转换的坐标点 头文件3
    ps = tf2_geometry_msgs.PointStamped()
    # 时间戳--0
    # ps.header.stamp = rospy.Time() 
    ps.header.stamp = rospy.Time.now() # launch使用的是静态坐标变换

    # 坐标系
    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(1)
    while not rospy.is_shutdown():
        # 使用try防止
        try:
            # ------ 需要计算 son1 相对于 son2 的坐标关系  头文件4
            """
                参数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()

坐标系关系

在vscode终端里打开：ctrl+`
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch
rosrun tf2_tools view_frames.py
evince frames.pdf（查看pdf）