【ROS】学习笔记——TF坐标变换

一、两个消息类型

通过tf2库来实现不同坐标系之间的变换，需要用到两个重要的消息类型。

• geometry_msgs::TransformStamped
• geometry_msgs::PointStamped

geometry_msgs::TransformStamped 是 ROS中的一个消息类型，定义在 geometry_msgs 包内，主要用于存储变换信息。它通常用于 tf2 库，用来表示坐标系之间的变换关系，包括平移、旋转以及时间戳。

geometry_msgs::PointStamped 是 ROS中的一个消息类型，定义在 geometry_msgs 包内，主要用于表示带有时间戳和坐标系信息的 3D 点。它通常用于存储某个坐标系下的点的位置，并在 tf2 变换中转换到其他坐标系。

下面展示一下两个消息类型的结构定义。

# geometry_msgs::TransformStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/Transform transform
  geometry_msgs/Vector3 translation
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation
    float64 x
    float64 y
    float64 z
    float64 w

# geometry_msgs::PointStamped

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

---

二、问题例子描述

参考资料：AutoLabor ROS学习

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

---

三、C++编码实现

// tf_pub.cpp

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "tf_pub");
    tf2_ros::StaticTransformBroadcaster broadcaster;
    geometry_msgs::TransformStamped ts;

    // 定义消息类型信息
    ts.header.stamp = ros::Time::now();
    ts.header.frame_id = "base_link";
    ts.child_frame_id = "laser";
    ts.transform.translation.x = 0.2;
    ts.transform.translation.y = 0.0;
    ts.transform.translation.z = 0.5;

    // 定义四元数并赋值
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, 0);

    // 从四元数中获取欧拉角并赋值
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    // 广播器发布坐标变换信息
    broadcaster.sendTransform(ts);
  
    ros::spin();
    return 0;
}

在广播器广播了坐标变换信息后，我们可以在rViz中查看到两个坐标点之间的变换情况。可以理解为让ROS服务器知道了有这两个坐标系的存在，并且告诉了它两个坐标系之间变换的关系。

// tf_sub.cpp

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

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "tf_sub");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate rate(1);
    while(ros::ok())
    {   
        // 生成一个相对于激光坐标系的坐标点
        geometry_msgs::PointStamped ps_laser;
        ps_laser.header.frame_id = "laser";
        ps_laser.header.stamp = ros::Time::now();
        ps_laser.point.x = 2.0;
        ps_laser.point.y = 3.0;
        ps_laser.point.z = 5.0;
        ROS_INFO("转换前的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",
                        ps_laser.point.x,
                        ps_laser.point.y,
                        ps_laser.point.z,
                        ps_laser.header.frame_id.c_str());

        try
        {
            geometry_msgs::PointStamped ps_base;
            ps_base = buffer.transform(ps_laser, "base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",
                        ps_base.point.x,
                        ps_base.point.y,
                        ps_base.point.z,
                        ps_base.header.frame_id.c_str());
        }
        catch(const std::exception& e)
        {  
            ROS_INFO("ERROR: %s", e.what());
        }

        rate.sleep();
        ros::spinOnce();
    }
}

运行结果如下。