ROS笔记之TF坐标变换

code review!

一些相关函数的用法

tf::TransFormBroadcaster tf1; tf1.sendTransform()

tf::TransformBroadcaster是ROS（机器人操作系统）库中的一个类，允许在ROS系统中的不同坐标系之间广播变换信息。sendTransform()方法是tf::TransformBroadcaster类的成员函数，用于向ROS系统发送变换消息。

以下是对br.sendTransform()函数的详细解释：

1. br：这是tf::TransformBroadcaster类的实例。通常在需要广播变换信息时创建该变量。

2. sendTransform()：该方法在tf::TransformBroadcaster的实例上调用，用于发送变换消息。它向ROS系统广播两个坐标系之间的变换。

   ``sendTransform()`方法需要多个参数来指定变换信息：

   • const geometry_msgs::TransformStamped& transform：此参数指定要广播的变换数据。它的类型为geometry_msgs::TransformStamped，其中包含平移、旋转和变换的时间戳等信息。

   • const std::string& parent_frame_id：此参数指定变换的父坐标系。它表示变换所定义的参考坐标系。

   • const std::string& child_frame_id：此参数指定变换的子坐标系。它表示相对于父坐标系进行变换的参考坐标系。

   • const ros::Time& time：此参数指定与变换关联的时间戳。它表示变换的有效时间。

   ``sendTransform()`方法将变换消息发布到ROS系统，使其他节点可以订阅并使用此信息进行各种用途，例如传感器融合、定位或机器人控制。

总而言之，br.sendTransform()函数用于使用tf::TransformBroadcaster类在ROS系统中从一个坐标系广播到另一个坐标系的变换消息。

下面是一个简单的示例，演示如何使用tf::TransformBroadcaster类广播一个变换消息：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "transform_broadcaster_example");
  ros::NodeHandle nh;

  // 创建一个tf::TransformBroadcaster对象
  tf::TransformBroadcaster br;

  // 循环发布变换消息
  ros::Rate rate(1.0);  // 发布频率为1Hz
  while (ros::ok())
  {
    // 创建一个tf::Transform对象，表示变换关系
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(1.0, 2.0, 0.0));  // 平移部分
    tf::Quaternion q;
    q.setRPY(0, 0, 1.57);  // 旋转部分（绕Z轴旋转90度）
    transform.setRotation(q);

    // 发布变换消息
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "parent_frame", "child_frame"));

    // 等待一段时间
    rate.sleep();
  }

  return 0;
}

在上述示例中，我们首先创建一个tf::TransformBroadcaster对象，然后在一个循环中发布变换消息。在每次循环中，我们创建一个tf::Transform对象来表示变换关系，设置其平移和旋转部分。然后，使用br.sendTransform()方法将变换消息发布到ROS系统中，指定父坐标系为"parent_frame"，子坐标系为"child_frame"。变换消息的时间戳使用ros::Time::now()获取当前时间。最后，通过rate.sleep()等待一段时间，以控制变换消息的发布频率。

这个示例仅用于演示如何使用tf::TransformBroadcaster类广播变换消息。在实际应用中，您需要根据自己的坐标系变换需求进行适当的修改。

tf::StampedTransform()

tf::StampedTransform()是ROS中tf库中的一个类，用于表示具有时间戳的变换信息。它是tf::Transform类的子类，可以包含变换矩阵、平移、旋转以及关联的时间戳。

以下是对tf::StampedTransform()类的详细解释：

1. tf::StampedTransform类：它表示一个具有时间戳的变换信息，用于描述坐标系之间的变换关系。它继承自tf::Transform类，因此拥有tf::Transform类的所有成员函数和属性。

2. 构造函数：tf::StampedTransform类提供了多个构造函数，用于创建具有时间戳的变换信息。构造函数的参数包括：

   • const tf::Transform& transform：一个tf::Transform对象，表示变换的平移和旋转部分。
   • const ros::Time& timestamp：一个ROS时间戳，表示与变换关联的时间。

3. 成员函数：tf::StampedTransform类还提供了一些成员函数，用于获取和设置变换信息的各个部分，如平移、旋转和时间戳。

   • getOrigin()：获取变换的平移部分，返回一个tf::Vector3对象。
   • getRotation()：获取变换的旋转部分，返回一个tf::Quaternion对象。
   • stamp_：表示与变换关联的时间戳的成员变量。

tf::StampedTransform()类的主要作用是在ROS系统中描述具有时间戳的坐标系之间的变换关系。它可以用于发布和接收具有时间戳的变换消息，并与其他ROS节点共享坐标系变换信息，从而实现传感器融合、定位和控制等应用。

tf::Transform()

tf::Transform是ROS中tf库中的一个类，用于表示坐标系之间的变换关系。它包含了平移和旋转的信息，可以用来描述一个坐标系相对于另一个坐标系的变换。

以下是对tf::Transform类的详细解释：

1. 构造函数：tf::Transform类提供了多个构造函数，用于创建变换对象。构造函数的参数包括：

   • const tf::Quaternion& rotation：一个tf::Quaternion对象，表示变换的旋转部分。
   • const tf::Vector3& translation：一个tf::Vector3对象，表示变换的平移部分。

2. 成员函数：tf::Transform类提供了一些成员函数，用于获取和设置变换的不同部分，如平移和旋转。

   • setOrigin(const tf::Vector3& translation)：设置变换的平移部分。
   • getOrigin()：获取变换的平移部分，返回一个tf::Vector3对象。
   • setRotation(const tf::Quaternion& rotation)：设置变换的旋转部分。
   • getRotation()：获取变换的旋转部分，返回一个tf::Quaternion对象。

3. 变换操作：tf::Transform类支持一些常见的变换操作，如乘法和逆变换。

   • operator*(const tf::Transform& other)：将当前变换与另一个变换相乘，返回一个新的变换结果。
   • inverse()：获取当前变换的逆变换，返回一个新的变换对象。

tf::Transform类的主要作用是描述一个坐标系相对于另一个坐标系的变换关系。通过设置平移和旋转部分，可以定义一个坐标系相对于另一个坐标系的位置和姿态关系。在ROS系统中，tf::Transform类经常与tf::TransformBroadcaster类一起使用，用于发布和接收坐标系变换信息。

例如，可以使用tf::Transform来表示一个机器人在全局坐标系中的位置和姿态，或者表示一个传感器相对于机器人坐标系的位置和姿态。通过将不同的tf::Transform对象相乘，可以组合多个坐标系之间的变换关系，实现坐标系的链式变换。

下面是一个简单的示例，演示如何使用tf::Transform类来表示坐标系之间的变换关系：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "transform_example");
  ros::NodeHandle nh;

  // 创建一个tf::Transform对象，表示变换关系
  tf::Transform transform;
  transform.setOrigin(tf::Vector3(1.0, 2.0, 0.0));  // 平移部分
  tf::Quaternion q;
  q.setRPY(0, 0, 1.57);  // 旋转部分（绕Z轴旋转90度）
  transform.setRotation(q);

  // 输出变换关系的平移和旋转部分
  tf::Vector3 translation = transform.getOrigin();
  tf::Quaternion rotation = transform.getRotation();
  ROS_INFO("Translation: %f, %f, %f", translation.x(), translation.y(), translation.z());
  ROS_INFO("Rotation: %f, %f, %f, %f", rotation.x(), rotation.y(), rotation.z(), rotation.w());

  return 0;
}

在上述示例中，我们创建了一个tf::Transform对象，表示一个坐标系相对于另一个坐标系的变换关系。我们设置了平移部分为(1.0, 2.0, 0.0)，表示相对于父坐标系的平移向量。旋转部分使用欧拉角表示，我们设置了绕Z轴旋转90度。

然后，我们使用getOrigin()和getRotation()成员函数获取变换关系的平移和旋转部分，并通过ROS_INFO输出到控制台。

这个示例仅用于演示如何使用tf::Transform类来表示坐标系之间的变换关系，并获取其平移和旋转信息。在实际应用中，您需要根据自己的需求进行适当的修改和使用。

tf::Vector3()详解

tf::Vector3是ROS中tf库中的一个类，用于表示三维空间中的向量。它包含了三个分量（x、y、z），可以用来表示位置、位移、速度等。

以下是对tf::Vector3类的详细解释：

1. 构造函数：tf::Vector3类提供了多个构造函数，用于创建向量对象。构造函数的参数包括：

   • double x：向量的x分量。
   • double y：向量的y分量。
   • double z：向量的z分量。

2. 成员函数：tf::Vector3类提供了一些成员函数，用于获取和设置向量的各个分量。

   • setX(double x)：设置向量的x分量。
   • setY(double y)：设置向量的y分量。
   • setZ(double z)：设置向量的z分量。
   • getX()：获取向量的x分量。
   • getY()：获取向量的y分量。
   • getZ()：获取向量的z分量。

3. 向量操作：tf::Vector3类支持一些常见的向量操作，如加法、减法、数乘和点积等。

   • operator+(const tf::Vector3& other)：将当前向量与另一个向量相加，返回一个新的向量结果。
   • operator-(const tf::Vector3& other)：将当前向量与另一个向量相减，返回一个新的向量结果。
   • operator*(double scalar)：将当前向量与一个标量相乘，返回一个新的向量结果。
   • dot(const tf::Vector3& other)：计算当前向量与另一个向量的点积。

tf::Vector3类的主要作用是表示三维空间中的向量。在ROS系统中，它经常与tf::Transform类一起使用，用于表示坐标系的平移部分。它还可以用于表示位移向量、速度向量等。

例如，可以使用tf::Vector3来表示一个机器人在三维空间中的位置，或者表示一个物体的位移向量。通过对向量进行加法、减法、数乘等操作，可以进行向量运算，实现位置偏移、速度调整等功能。

br.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion::getIdentity(),tf::Vector3),),)详解

在您提供的代码片段中，br.sendTransform()函数用于发布一个带有时间戳的变换消息。下面是对代码片段中的参数的详细解释：

br.sendTransform(
    tf::StampedTransform(
        tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3),
        ros::Time::now(),
        parent_frame,
        child_frame
    )
);

• tf::StampedTransform：这是一个带有时间戳的变换消息类，用于在发布坐标系变换时指定时间戳。它是tf::Transform的子类。构造函数的参数包括：

  • tf::Transform transform：一个tf::Transform对象，表示坐标系之间的变换关系。
  • ros::Time stamp：一个ros::Time对象，表示变换消息的时间戳。
  • std::string parent_frame：一个std::string对象，表示父坐标系的名称。
  • std::string child_frame：一个std::string对象，表示子坐标系的名称。

• tf::Transform(tf::Quaternion::getIdentity(), tf::Vector3)：这是一个tf::Transform对象的构造函数调用，用于创建一个表示单位变换的对象。参数包括：

  • tf::Quaternion::getIdentity()：一个静态函数调用，返回一个代表恒等旋转的tf::Quaternion对象。
  • tf::Vector3：一个空的tf::Vector3对象，表示零平移。

• ros::Time::now()：这是一个静态函数调用，返回当前系统时间的ros::Time对象。它用作变换消息的时间戳。

• parent_frame：一个std::string对象，表示父坐标系的名称。

• child_frame：一个std::string对象，表示子坐标系的名称。

br.sendTransform()函数通过tf::TransformBroadcaster对象发布一个带有时间戳的坐标系变换消息。该消息描述了父坐标系到子坐标系的变换关系，并指定了变换消息的时间戳。

CMakeLists.txt和package.xml中添加对tf库的支持

在CMakeLists.txt文件中，您可以使用以下方法来添加对tf库的依赖：

find_package(catkin REQUIRED COMPONENTS
  ...
  tf
  ...
)

catkin_package(
  ...
  CATKIN_DEPENDS
    ...
    tf
    ...
)

...

target_link_libraries(your_executable_name
  ${catkin_LIBRARIES}
)

在上述示例中，假设您的项目是一个ROS项目。通过find_package(catkin REQUIRED COMPONENTS ... tf ...)，您告诉CMake在ROS环境中查找和导入tf库。然后，通过在catkin_package()的CATKIN_DEPENDS部分添加tf，您指定了您的软件包依赖于tf库。

最后，通过在target_link_libraries()中添加${catkin_LIBRARIES}，您将tf库添加到您的可执行文件的链接器指令中。

请确保您的CMakeLists.txt文件中包含了正确的ROS构建指令和其他必要的依赖项，具体根据您的项目进行相应的修改。

在package.xml文件中，您可以使用以下方式将tf库添加为依赖项：

<build_depend>tf</build_depend>
<exec_depend>tf</exec_depend>

在package.xml文件中，build_depend元素用于指定构建时的依赖项，而exec_depend元素用于指定运行时的依赖项。通过在这两个元素中添加tf，您将tf库添加为构建和运行时的依赖项。

确保在package.xml文件中的其他部分包含了正确的ROS包描述信息和其他依赖项，具体根据您的项目进行相应的修改。

tf::Quaternion::normalize()

以下是一个示例代码片段，演示如何归一化四元数：

#include <iostream>
#include <cmath>
#include <tf/tf.h>

int main() {
    // 创建一个未归一化的四元数
    tf::Quaternion quaternion(1.0, 2.0, 3.0, 4.0);

    // 输出未归一化的四元数
    std::cout << "未归一化的四元数：" << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << ", " << quaternion.w() << std::endl;

    // 归一化四元数
    quaternion.normalize();

    // 输出归一化后的四元数
    std::cout << "归一化的四元数：" << quaternion.x() << ", " << quaternion.y() << ", " << quaternion.z() << ", " << quaternion.w() << std::endl;

    return 0;
}

在这个示例中，我们使用了ROS的tf库来处理四元数。首先，我们创建了一个未归一化的四元数对象quaternion，其中包含了一些任意的值。然后，我们调用normalize()函数对该四元数进行归一化操作。最后，我们输出未归一化和归一化后的四元数值。

请注意，在实际使用中，您需要根据您的具体应用场景和变换消息的来源，将归一化操作应用到适当的地方。这个示例提供了一个基本的框架，供您参考如何进行四元数的归一化操作。

tf::quaternionMsgToTF

tf::quaternionMsgToTF是ROS tf库中的一个函数，用于将ROS消息中的Quaternion消息类型转换为tf库中的tf::Quaternion类型。

函数签名如下：

void quaternionMsgToTF(const geometry_msgs::Quaternion& msg, tf::Quaternion& bt);

参数说明：

• msg：geometry_msgs包中的Quaternion消息类型的引用，表示要转换的ROS消息中的四元数。
• bt：tf库中的tf::Quaternion类型的引用，表示转换后的四元数结果。

此函数用于将ROS中定义的geometry_msgs/Quaternion消息类型转换为tf库中的tf::Quaternion类型，以便在tf坐标转换系统中使用。

使用示例：

#include <ros/ros.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "quaternion_conversion_example");
    ros::NodeHandle nh;

    // 创建一个ROS的Quaternion消息
    geometry_msgs::Quaternion rosQuaternion;
    rosQuaternion.x = 1.0;
    rosQuaternion.y = 2.0;
    rosQuaternion.z = 3.0;
    rosQuaternion.w = 4.0;

    // 将ROS Quaternion消息转换为tf::Quaternion类型
    tf::Quaternion tfQuaternion;
    tf::quaternionMsgToTF(rosQuaternion, tfQuaternion);

    // 输出转换后的tf::Quaternion值
    ROS_INFO("tf::Quaternion: x=%f, y=%f, z=%f, w=%f",
             tfQuaternion.x(), tfQuaternion.y(), tfQuaternion.z(), tfQuaternion.w());

    return 0;
}

在示例中，我们首先创建一个ROS的Quaternion消息类型，并为其赋予一些任意的值。然后，我们使用quaternionMsgToTF函数将ROS消息转换为tf::Quaternion类型。最后，我们输出转换后的tf::Quaternion的值。

通过quaternionMsgToTF函数，您可以方便地在ROS和tf库之间进行四元数的转换，以满足不同模块之间的坐标变换和旋转表示需求。

[WARN]:MSG to TF:Quaternion Not Properly Normalized

"[WARN]: MSG to TF: Quaternion Not Properly Normalized"警告消息表明将ROS消息中的四元数转换为tf库中的tf::Quaternion类型时，发现四元数没有被正确归一化。

为了解决这个警告，您可以在转换之前对ROS消息中的四元数进行归一化操作，以确保其长度为1。以下是一个示例代码片段，演示如何归一化ROS消息中的四元数：

#include <ros/ros.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "quaternion_conversion_example");
    ros::NodeHandle nh;

    // 创建一个ROS的Quaternion消息
    geometry_msgs::Quaternion rosQuaternion;
    rosQuaternion.x = 1.0;
    rosQuaternion.y = 2.0;
    rosQuaternion.z = 3.0;
    rosQuaternion.w = 4.0;

    // 归一化ROS Quaternion消息
    tf::Quaternion tfQuaternion;
    tf::quaternionMsgToTF(rosQuaternion, tfQuaternion);
    tfQuaternion.normalize(); // 归一化操作

    // 输出归一化后的tf::Quaternion值
    ROS_INFO("Normalized tf::Quaternion: x=%f, y=%f, z=%f, w=%f",
             tfQuaternion.x(), tfQuaternion.y(), tfQuaternion.z(), tfQuaternion.w());

    return 0;
}

在示例中，我们使用quaternionMsgToTF函数将ROS Quaternion消息转换为tf::Quaternion类型。然后，我们对tf::Quaternion进行归一化操作，确保其长度为1。最后，我们输出归一化后的tf::Quaternion值。

通过在转换之前进行归一化操作，您可以避免警告消息并确保准确的旋转表示。请注意，具体的归一化操作可能因您的应用场景而有所不同，您可以根据需要进行适当的调整。

ROS笔记之TF坐标变换原文地址：http://www.autolabor.com.cn/book/ROSTutorials

一.坐标msg消息

二.静态坐标变换

方案A:C++实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.发布方

/* 
    静态坐标变换发布方:
        发布关于 laser 坐标系的位置信息 

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建静态坐标转换广播器
        4.创建坐标系信息
        5.广播器发布坐标系信息
        6.spin()
*/


// 1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"static_brocast");
    // 3.创建静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster broadcaster;
    // 4.创建坐标系信息
    geometry_msgs::TransformStamped ts;
    //----设置头信息
    ts.header.seq = 100;
    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();
    // 5.广播器发布坐标系信息
    broadcaster.sendTransform(ts);
    ros::spin();
    return 0;
}

配置文件此处略。

3.订阅方

/*  
    订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 TF 订阅节点
        4.生成一个坐标点(相对于子级坐标系)
        5.转换坐标点(相对于父级坐标系)
        6.spin()
*/
//1.包含头文件
#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" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "laser";
        point_laser.header.stamp = ros::Time::now();
        point_laser.point.x = 1;
        point_laser.point.y = 2;
        point_laser.point.z = 7.3;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"base_link");
            ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常.....");
        }


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


    return 0;
}

配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，控制台将输出，坐标转换后的结果。

三.动态坐标变换

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述:

启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

实现分析:

乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点

订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度

将 pose 信息转换成 坐标系相对信息并发布

实现流程:C++ 与 Python 实现流程一致

新建功能包，添加依赖

创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)

创建坐标相对关系订阅方

执行

方案A:C++实现

1.创建功能包

创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

/*  
    动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)

    需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
    控制乌龟运动，将两个坐标系的相对位置动态发布

    实现分析:
        1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
        2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
        3.将 pose 信息转换成 坐标系相对信息并发布

    实现流程:
        1.包含头文件
        2.初始化 ROS 节点
        3.创建 ROS 句柄
        4.创建订阅对象
        5.回调函数处理订阅到的数据(实现TF广播)
            5-1.创建 TF 广播器
            5-2.创建 广播的数据(通过 pose 设置)
            5-3.广播器发布数据
        6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  5-1.创建 TF 广播器
    static tf2_ros::TransformBroadcaster broadcaster;
    //  5-2.创建 广播的数据(通过 pose 设置)
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
    //  |--------- 四元数设置
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();


    //  5-3.广播器发布数据
    broadcaster.sendTransform(tfs);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_pub");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
    //     5.回调函数处理订阅到的数据(实现TF广播)
    //        
    // 6.spin
    ros::spin();
    return 0;
}

配置文件此处略。

3.订阅方

//1.包含头文件
#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" //注意: 调用 transform 必须包含该头文件

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"dynamic_tf_sub");
    ros::NodeHandle nh;
    // 3.创建 TF 订阅节点
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    ros::Rate r(1);
    while (ros::ok())
    {
    // 4.生成一个坐标点(相对于子级坐标系)
        geometry_msgs::PointStamped point_laser;
        point_laser.header.frame_id = "turtle1";
        point_laser.header.stamp = ros::Time();
        point_laser.point.x = 1;
        point_laser.point.y = 1;
        point_laser.point.z = 0;
    // 5.转换坐标点(相对于父级坐标系)
        //新建一个坐标点，用于接收转换结果  
        //--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
        try
        {
            geometry_msgs::PointStamped point_base;
            point_base = buffer.transform(point_laser,"world");
            ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);

        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("程序异常:%s",e.what());
        }


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


    return 0;
}

配置文件此处略。

4.执行

可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，与演示结果类似。

可以使用 rviz 查看坐标系相对关系。

四.多坐标变换

1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.发布方

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

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>

3.订阅方

/*

需求:
    现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
    son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
    求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
    1.包含头文件
    2.初始化 ros 节点
    3.创建 ros 句柄
    4.创建 TF 订阅对象
    5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
      解析 son1 中的点相对于 son2 的坐标
    6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.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
        {
        //   解析 son1 中的点相对于 son2 的坐标
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
            ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
            ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
                    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 psAtSon2;
            psAtSon2 = buffer.transform(ps,"son2");
            ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.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;
}

配置文件此处略。

4.执行
可以使用命令行或launch文件的方式分别启动发布节点与订阅节点，如果程序无异常，将输出换算后的结果。

五.坐标关系查看

六.TF坐标变换实操

需求描述:

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

结果演示:

实现分析:

乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

• 启动乌龟显示节点
• 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
• 编写两只乌龟发布坐标信息的节点
• 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

实现流程:C++ 与 Python 实现流程一致

• 新建功能包，添加依赖

• 编写服务客户端，用于生成一只新的乌龟

• 编写发布方，发布两只乌龟的坐标信息

• 编写订阅方，订阅两只乌龟信息，生成速度信息并发布

• 运行

准备工作:

1.了解如何创建第二只乌龟，且不受键盘控制

创建第二只乌龟需要使用rosservice,话题使用的是 spawn

rosservice call /spawn “x: 1.0
y: 1.0
theta: 1.0
name: ‘turtle_flow’”
name: “turtle_flow”
Copy
键盘是无法控制第二只乌龟运动的，因为使用的话题: /第二只乌龟名称/cmd_vel,对应的要控制乌龟运动必须发布对应的话题消息

2.了解如何获取两只乌龟的坐标

是通过话题 /乌龟名称/pose 来获取的

x: 1.0 //x坐标
y: 1.0 //y坐标
theta: -1.21437060833 //角度
linear_velocity: 0.0 //线速度
angular_velocity: 1.0 //角速度
Copy
方案A:C++实现

1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.服务客户端(生成乌龟)

/* 
    创建第二只小乌龟
 */
#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{

    setlocale(LC_ALL,"");

    //执行初始化
    ros::init(argc,argv,"create_turtle");
    //创建节点
    ros::NodeHandle nh;
    //创建服务客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");

    ros::service::waitForService("/spawn");
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.x = 1.0;
    spawn.request.y = 2.0;
    spawn.request.theta = 3.12415926;
    bool flag = client.call(spawn);
    if (flag)
    {
        ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("乌龟2创建失败!");
    }

    ros::spin();

    return 0;
}

配置文件此处略。

3.发布方(发布两只乌龟的坐标信息)
可以订阅乌龟的位姿信息，然后再转换成坐标信息，两只乌龟的实现逻辑相同，只是订阅的话题名称，生成的坐标信息等稍有差异，可以将差异部分通过参数传入:

该节点需要启动两次
每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)

/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建 ros 句柄
        5.创建订阅对象
        6.回调函数处理订阅的 pose 信息
            6-1.创建 TF 广播器
            6-2.将 pose 信息转换成 TransFormStamped
            6-3.发布
        7.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;


void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  6-1.创建 TF 广播器 ---------------------------------------- 注意 static
    static tf2_ros::TransformBroadcaster broadcaster;
    //  6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    tfs.child_frame_id = turtle_name;
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //  6-3.发布
    broadcaster.sendTransform(tfs);

} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"pub_tf");
    // 3.解析传入的命名空间
    if (argc != 2)
    {
        ROS_ERROR("请传入正确的参数");
    } else {
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    // 4.创建 ros 句柄
    ros::NodeHandle nh;
    // 5.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
    //     6.回调函数处理订阅的 pose 信息
    //         6-1.创建 TF 广播器
    //         6-2.将 pose 信息转换成 TransFormStamped
    //         6-3.发布
    // 7.spin
    ros::spin();
    return 0;
}

配置文件此处略。

4.订阅方(解析坐标信息并生成速度信息)

/*  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 TF 订阅对象
        5.处理订阅到的 TF
        6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_TF");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    // 5.处理订阅到的 TF

    // 需要创建发布 /turtle2/cmd_vel 的 publisher 对象

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示:%s",e.what());
        }



        rate.sleep();
        // 6.spin
        ros::spinOnce();
    }

    return 0;
}

配置文件此处略。

5.运行
使用 launch 文件组织需要运行的节点，内容示例如下:

<!--
    tf2 实现小乌龟跟随案例
-->
<launch>
    <!-- 启动乌龟节点与键盘控制节点 -->
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key_control" output="screen"/>
    <!-- 启动创建第二只乌龟的节点 -->
    <node pkg="demo_tf2_test" type="Test01_Create_Turtle2" name="turtle2" output="screen" />
    <!-- 启动两个坐标发布节点 -->
    <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster1" output="screen" args="turtle1" />
    <node pkg="demo_tf2_test" type="Test02_TF2_Caster" name="caster2" output="screen" args="turtle2" />
    <!-- 启动坐标转换节点 -->
    <node pkg="demo_tf2_test" type="Test03_TF2_Listener" name="listener" output="screen" />
</launch>

七.TF2与TF

八.小结