ROS 学习笔记4.TF变换

（自学笔记，大部分是抄的，长期更新）

TF坐标变换

一、坐标变换的msg信息
- 1.geometry_msgs/TransformStamped
- 2.geometry_msgs/PointStamped
二、静态坐标变换
- 1.需求描述:
- 2.发布方C++代码实现
- 3.订阅方C++实现
三、动态坐标变换
- 1.需求描述
- 2.发布方C++实现
- 3.订阅方C++实现
四、多坐标变换
- 1.需求描述
- 2.发布方launch实现
- 3.订阅方C++实现
五、tf坐标变换实操
- 1.需求描述
- 2.生成两只小海龟
- - 2.1 创建生成海龟的服务
  - 2.2生成小海龟launch文件
- 3.两只乌龟坐标发布
- - 3.1创建CPP文件
  - 3.2创建launch文件
- 4.订阅两只海龟的位姿消息
- - 4.1创建CPP文件
  - 4.2 运行所有节点的launch文件

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用以下两种消息格式：
msg:geometry_msgs/TransformStamped
geometry_msgs/PointStamped

前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。

一、坐标变换的msg信息 1.geometry_msgs/TransformStamped

查看消息类型：rosmsg info geometry_msgs/TransformStamped，描述两个坐标系之间的关系：旋转矩阵R和平移矩阵T。

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

这里使用了四元数进行位姿的表示。

2.geometry_msgs/PointStamped

查看消息类型：rosmsg info geometry_msgs/PointStamped，描述某坐标系下的某个点的坐标。

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

二、静态坐标变换

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

1.需求描述:

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

（答：2.2，3.0，5.5）

构建功能包：tf01_static
添加依赖：rospy roscpp std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs
建立CPP文件:demo01_static_pub.cpp
配置CMakeLists.txt:

add_executable(demo01_static_pub src/demo01_static_pub.cpp)
add_dependencies(demo01_static_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} 
target_link_libraries(demo01_static_pub
  ${catkin_LIBRARIES}
)

2.发布方C++代码实现

// 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_pub");
    // 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;
}

rosrun tf01_static demo01_static_pub
rostopic echo /tf_static

transforms: 
  - 
    header: 
      seq: 100
      stamp: 
        secs: 1653408056
        nsecs: 773908374
      frame_id: "base_link"
    child_frame_id: "laser"
    transform: 
      translation: 
        x: 0.2
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

rviz查看

3.订阅方C++实现

//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),参考的坐标系是:%s",
                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;
}

三、动态坐标变换

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

1.需求描述

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

实现分析:

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

tf02_dynamic

rospy roscpp std_msgs geometry_msgs turtlesim tf2 tf2_ros tf2_geometry_msgs

2.发布方C++实现

// 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("/turtle1/pose",1000,doPose);
    //     5.回调函数处理订阅到的数据(实现TF广播)
    //        
    // 6.spin
    ros::spin();
    return 0;
}

配置文件：

add_executable(demo01_dynamic_pub src/demo01_dynamic_pub.cpp)
add_dependencies(demo01_dynamic_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(demo01_dynamic_pub
  ${catkin_LIBRARIES}
)

运行tf动态坐标发布方：

rosrun tf02_dynamic demo01_dynamic_pub
rostopic echo /tf

3.订阅方C++实现

//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;
}

add_executable(demo02_dynamic_sub src/demo02_dynamic_sub.cpp
add_dependencies(demo02_dynamic_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(demo02_dynamic_sub
  ${catkin_LIBRARIES}
)

rosrun tf02_dynamic demo01_dynamic_pub
rosrun tf02_dynamic demo02_dynamic_sub

四、多坐标变换 1.需求描述

现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标

实现分析:

首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息；
然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换；
最后，还要实现坐标点的转换；

rospy roscpp std_msgs geometry_msgs turtlesim tf2 tf2_ros tf2_geometry_msgs

2.发布方launch实现

tfs_c.launch

roslaunch tf03_tfs tfs_c.launch

3.订阅方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"

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

    //创建坐标点
    geometry_msgs::PointStamped psAtSon1;

    psAtSon1.header.stamp=ros::Time::now();
    psAtSon1.header.frame_id="son1";
    psAtSon1.point.x=1;
    psAtSon1.point.y=2;
    psAtSon1.point.z=3;



    ros::Rate rate(10);
    while (ros::ok())
    {
        
        try
        {
            //1.计算son1和son2的关系
            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(),
                        son1ToSon2.child_frame_id.c_str(),
                        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)
        {
            std::cerr << e.what() << 'n';
        }
        

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

rosrun tf03_tfs demo01_tfs

五、tf坐标变换实操 1.需求描述

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

tf04_test
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

2.生成两只小海龟 2.1 创建生成海龟的服务

创建生成小海龟的服务文件：test01_new_turtle.cpp。

#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("/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.1415926;
    bool flag=client.call(spawn);
    if(flag)
    {
        ROS_INFO("乌龟%s创建成功！",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("乌龟2创建失败!");
    }
    ros::spin();
    
    return 0;
}

2.2生成小海龟launch文件

创建test.launch文件,生成两只小海龟，并启动键盘控制节点。

3.两只乌龟坐标发布 3.1创建CPP文件

创建发布小海龟位姿的代码文件test02_pub_turtle.cpp,主要是读取Pose话题内容，创建TransformStamped并发布。

//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(turtle_name + "/pose",1000,doPose);
    //     6.回调函数处理订阅的 pose 信息
    //         6-1.创建 TF 广播器
    //         6-2.将 pose 信息转换成 TransFormStamped
    //         6-3.发布
    // 7.spin
    ros::spin();
    return 0;
}

3.2创建launch文件

4.订阅两只海龟的位姿消息 4.1创建CPP文件

订阅两只小海龟的位姿消息，并转换成速度信息。

test03_control_turtle2.cpp

#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"
#include "geometry_msgs/Twist.h"



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

    //A.创建发布对象
    ros::Publisher pub=nh.advertise("/turtle2/cmd_vel",100);



    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //1.计算son1和son2的关系
            
            geometry_msgs::TransformStamped son1ToSon2=buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            // ROS_INFO("turtle1相对于turtle2的信息：父级：%s,子级:%s,偏移量(%.2f,%.2f,%.2f)",
            //             son1ToSon2.header.frame_id.c_str(),//turtle2
            //             son1ToSon2.child_frame_id.c_str(),//turtle1
            //             son1ToSon2.transform.translation.x,
            //             son1ToSon2.transform.translation.y,
            //             son1ToSon2.transform.translation.z    
            // );
            //B.根据相对计算并组织速度消息
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2));
            twist.angular.z = 4 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);

            //C.发布
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << 'n';
        }
        

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

4.2 运行所有节点的launch文件

ROS 学习笔记4.TF变换

C/C++/C#相关栏目本月热门文章