二十四、TF坐标变换（四）：多坐标变换及坐标系关系查看

一、引言

在父级坐标系world下，有两个子级坐标系son1,son2,已知两个子级坐标系相对于父级坐标系的关系，求出son1相对于son2的坐标原点的关系，以及son1中的一点相对于son2中的关系。
这时，我们就可以先发布son1，son2相对于world的坐标关系，然后创建TF订阅对象，通过tf2功能包完成坐标转换

二、C++实现

• 功能包依赖：tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

1.发布方实现

<launch>
    <!-- 通过命令参数发布 -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.3 0.4 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0.7 0.7 0 0 0 /world /son2" output="screen" />
</launch>

2.订阅方实现

// 1.包含头文件
#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/TransformStamped.h"
#include"geometry_msgs/PointStamped.h"
int main(int argc, char *argv[]){
    setlocale(LC_ALL,"");
    // 2.初始化ros节点
    ros::init(argc,argv,"sub_frames");
    // 3.创建ros句柄
    ros::NodeHandle nh;
    // 4.创建TF订阅对象
    tf2_ros::Buffer buffer;//存储订阅消息
    tf2_ros::TransformListener listener(buffer);
    // 5.解析订阅信息中获取son1坐标原点在son2中的坐标
    ros::Rate r(1);
    while (ros::ok())
    {
        try
        {
            // ros::Time(0)：表示取son1，son2最近的关系作为参考量  son2:父级坐标系，目标坐标系  son1：子级坐标系
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("父级坐标系:%s\n,子级坐标系：%s\n,偏移量：x=%.2f,y=%.2f,z=%.2f\n",
                tfs.header.frame_id.c_str(),
                tfs.child_frame_id.c_str(),
                tfs.transform.translation.x,
                tfs.transform.translation.y,
                tfs.transform.translation.z
            );
            // 坐标点解析
            geometry_msgs::PointStamped ps;
            ps.header.frame_id = "son1";
            ps.header.stamp = ros::Time::now();
            ps.point.x = 1.0;
            ps.point.y = 2.0;
            ps.point.z = 3.0;

            geometry_msgs::PointStamped ps_at_son2;
            ps_at_son2 = buffer.transform(ps,"son2");
            ROS_INFO("在son2中的坐标：x=%.2f,y=%.2f,z=%.2f\n",
            ps_at_son2.point.x,
            ps_at_son2.point.y,
            ps_at_son2.point.z
            );
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("异常信息：%s",e.what());
        }
        
        r.sleep();
        // 6.spin
        ros::spinOnce();
    }
    return 0;
}

• 运行结果

  [ INFO] [1658377161.418557965]: 父级坐标系:son2
  ,子级坐标系：son1
  ,偏移量：x=-0.30,y=-0.40,z=-0.30
  
  [ INFO] [1658377161.418658173]: 在son2中的坐标：x=0.70,y=1.60,z=2.70
  

三、Python实现

1.发布方实现

<launch>
    <!-- 通过命令参数发布 -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.3 0.4 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0.7 0.7 0 0 0 /world /son2" output="screen" />
</launch>

2.订阅方实现

#!/usr/bin/env python
# 1.导包
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import PointStamped

if __name__ == "__main__":
    # 2.初始化ROS节点
    rospy.init_node("frames_sub_p")
    # 3.创建TF订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)
    rate = rospy.Rate(1)

    while not rospy.is_shutdown():
        try:
            # 4.调用API求出son1相对于son2的相对坐标关系
            # lookup_transform(target_frame,source_frame,time)
            # rospy.Time(0)指son1，son2相对于world坐标系的最新关系
            tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("son1与son2的相对关系：")
            rospy.loginfo("父级坐标系：%s，子级坐标系：%s",tfs.header.frame_id,tfs.child_frame_id)
            rospy.loginfo("相对坐标：x=%.2f,y=%.2f,z=%.2f",
                            tfs.transform.translation.x,
                            tfs.transform.translation.y,
                            tfs.transform.translation.z
                            )
            # 5.创建一个son1中的坐标点，求出该点在son2中的坐标
            ps = PointStamped()
            ps.header.frame_id = "son1"
            ps.header.stamp = rospy.Time.now()
            ps.point.x = 1
            ps.point.y = 1
            ps.point.z = 1
            ps_target = buffer.transform(ps,"son2",rospy.Duration(0.5))
            rospy.loginfo("ps_target所属坐标系：%s",ps_target.header.frame_id)
            rospy.loginfo("相对于son2的坐标点:(%.2f,%.2f,%.2f)",
            ps_target.point.x,
            ps_target.point.y,
            ps_target.point.z
            )
        except Exception as e:
            rospy.logerr("错误提示：%s",e)
        rate.sleep()
    # 6.spin
    # rospy.spin()

• 运行结果

  [INFO] [1658395990.515741]: son1与son2的相对关系：
  [INFO] [1658395990.517491]: 父级坐标系：son2，子级坐标系：son1
  [INFO] [1658395990.519920]: 相对坐标：x=-0.30,y=-0.40,z=-0.30
  [INFO] [1658395990.522142]: ps_target所属坐标系：son2
  [INFO] [1658395990.524224]: 相对于son2的坐标点:(0.70,0.60,0.70)
  

四、坐标系关系查看

在ros系统中，可以通过相应的功能包生成树形结构的坐标系图谱

1. 准备
   查看是否安装对应功能包：rospack find tf2_tools
   若没有安装：sudo apt install ros-noetic-tf2-tools
2. 使用
   rosrun tf2_tools view_frames.py
   会在命令运行目录下生成一个PDF文件，并输出如下信息：

   rosrun tf2_tools view_fram
   es.py 
   Unable to register with master node [http://localhost:11311]: master may not be running yet. Will keep trying.
   [INFO] [1658400722.910123]: Listening to tf data during 5 seconds...
   [INFO] [1658400727.919585]: Generating graph in frames.pdf file...