多坐标系变换

功能包：
tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

1. 发布方：

为了方便，使用静态坐标变换发布：

<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>

2. 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.创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    // 4.编写解析逻辑

    //创建坐标点
    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 的相对关系
            /*
                参数1：目标坐标系   B
                参数2：源坐标系     A
                参数3：ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系
                返回值：geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
            */
            geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(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());
        }
    // 5.spinOnce()
    ros::spinOnce();
    }
    return 0;
}

输出：

3. python实现：

#! /usr/bin/env python
from email import header
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("static_sub_p")
    # 3.创建订阅对象
    # 3-1.创建缓存对象
    buffer = tf2_ros.Buffer()
    # 3-2.创建订阅对象的坐标点
    sub = tf2_ros.TransformListener(buffer)
    # 4.组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()
    # 时间戳 -- 0
    # ps.header.stamp =rospy.Time()
    ps.header.stamp = rospy.Time.now()
    # 坐标系
    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(10)
    while not rospy.is_shutdown():
        try:
            #----------计算 son1 相对于 son2 的坐标关系
            """
                参数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()