ROS中tf2实操之坐标追踪详解（示例+代码）

目录

项目中文件结构如下所示：

1. 启动turtle1节点和turtle1的键盘操作节点、订阅turtle1的位姿信息、发布turtle1的坐标信息（getTurtle1Pose.cpp）：

2. 启动turtle2节点、订阅turtle2的位姿信息、发布turtle2的坐标信息(getTurtle2Pose.cpp)：

3. 订阅/tf_static话题并获取坐标系相对关系、发布turtle2的运动信息(frameTransform.cpp)：

4. launch文件的编写：

5. 运行结果如下所示：

---

项目中文件结构如下所示：

要实现坐标追踪首先要理清代码编写的逻辑，其次我们这个实操中用的是“tf2动态坐标变换而非静态坐标变换”!

动态坐标变换详解：

ROS中动态坐标变换（动态参数调节+动态坐标变换）_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122912545静态坐标变换详解：

ROS中的静态坐标转换（解析+示例）_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122908060

1. 启动turtle1节点和turtle1的键盘操作节点、订阅turtle1的位姿信息、发布turtle1的坐标信息（getTurtle1Pose.cpp）：

#include "geometry_msgs/Twist.h"  
#include "turtlesim/Pose.h"  
#include "ros/ros.h"  
#include "tf2_ros/static_transform_broadcaster.h"  
#include "geometry_msgs/TransformStamped.h"  
#include "tf2/LinearMath/Quaternion.h"  
  
void SubCallbackFunc(const turtlesim::Pose::ConstPtr& TurtlePoseInfo)  
{  
    // “static属性”保证发布者对象保持不变  
    static tf2_ros::StaticTransformBroadcaster pub;  
    geometry_msgs::TransformStamped TransformInfo;  
    tf2::Quaternion qtn;  
  
    TransformInfo.child_frame_id = "Turtle1Frame";  
    TransformInfo.header.frame_id = "world";  
    TransformInfo.header.stamp = ros::Time::now();  
    TransformInfo.transform.translation.x = TurtlePoseInfo->x;  
    TransformInfo.transform.translation.y = TurtlePoseInfo->y;  
    qtn.setRPY(0,0,TurtlePoseInfo->theta);  
    TransformInfo.transform.rotation.w = qtn.getW();  
    TransformInfo.transform.rotation.x = qtn.getX();  
    TransformInfo.transform.rotation.y = qtn.getY();  
    TransformInfo.transform.rotation.z = qtn.getZ();  
  
    pub.sendTransform(TransformInfo);  
}  
  
int main(int argc,char* argv[])  
{  
    setlocale(LC_ALL,"");  
    ros::init(argc,argv,"getTurtle1Pose");  
    ros::NodeHandle nh("turtle1");  
    // 订阅turtle1的位姿(通过私有话题订阅消息)  
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("pose",10,boost::bind(SubCallbackFunc,_1));  
    ros::spin();  
    return 0;  
} 

注意：

1）为了保证发布turtle1坐标信息的发布者对象恒定不变，我们在函数中将turtle1坐标信息的发布对象声明为static类型的，表示该发布对象仅被创建一次，防止了“订阅tf_static话题的发布端对象频繁被创建的弊端”；

2）该代码的结构如下所示：

3）代码实现逻辑如下所示：

2. 启动turtle2节点、订阅turtle2的位姿信息、发布turtle2的坐标信息(getTurtle2Pose.cpp)：

#include "geometry_msgs/Twist.h"  
#include "turtlesim/Pose.h"  
#include "ros/ros.h"  
#include "tf2_ros/static_transform_broadcaster.h"  
#include "geometry_msgs/TransformStamped.h"  
#include "tf2/LinearMath/Quaternion.h"  
  
void SubCallbackFunc(const turtlesim::Pose::ConstPtr& TurtlePoseInfo)  
{  
    // “static属性”保证发布者对象保持不变  
    static tf2_ros::StaticTransformBroadcaster pub;  
    geometry_msgs::TransformStamped TransformInfo;  
    tf2::Quaternion qtn;  
  
    TransformInfo.child_frame_id = "Turtle1Frame";  
    TransformInfo.header.frame_id = "world";  
    TransformInfo.header.stamp = ros::Time::now();  
    TransformInfo.transform.translation.x = TurtlePoseInfo->x;  
    TransformInfo.transform.translation.y = TurtlePoseInfo->y;  
    qtn.setRPY(0,0,TurtlePoseInfo->theta);  
    TransformInfo.transform.rotation.w = qtn.getW();  
    TransformInfo.transform.rotation.x = qtn.getX();  
    TransformInfo.transform.rotation.y = qtn.getY();  
    TransformInfo.transform.rotation.z = qtn.getZ();  
  
    pub.sendTransform(TransformInfo);  
}  
  
int main(int argc,char* argv[])  
{  
    setlocale(LC_ALL,"");  
    ros::init(argc,argv,"getTurtle1Pose");  
    ros::NodeHandle nh("turtle1");  
    // 订阅turtle1的位姿(通过私有话题订阅消息)  
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("pose",10,boost::bind(SubCallbackFunc,_1));  
    ros::spin();  
    return 0;  
} 

注意：

1）代码的结构如下所示：

2）代码的实现逻辑如下所示：

3. 订阅/tf_static话题并获取坐标系相对关系、发布turtle2的运动信息(frameTransform.cpp)：

#include "tf2_ros/buffer.h"  
#include "tf2_ros/transform_listener.h"  
#include "geometry_msgs/TransformStamped.h"  
#include "geometry_msgs/Twist.h"  
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"  
#include "math.h"  
  
int main(int argc,char* argv[])  
{  
    setlocale(LC_ALL,"");  
    ros::init(argc,argv,"frameTrasnform");  
    // 创建专属于listener的buffer  
    tf2_ros::Buffer buffer;  
    tf2_ros::TransformListener listener(buffer);  
    // 创建cmd_vel话题的发布者  
    geometry_msgs::Twist ExpectedRunState;  
    ros::NodeHandle nh("turtle2");  
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);  
  
    ros::Rate rate(1);  
    while(ros::ok())  
    {  
        try  
        {  
            geometry_msgs::TransformStamped Turtle1ToTurtle2 = buffer.lookupTransform("Turtle2Frame","Turtle1Frame",ros::Time());  
            ExpectedRunState.angular.z = std::atan2(Turtle1ToTurtle2.transform.rotation.y,Turtle1ToTurtle2.transform.rotation.x);  
            ExpectedRunState.linear.x = Turtle1ToTurtle2.transform.translation.x;  
            ExpectedRunState.linear.y = Turtle1ToTurtle2.transform.translation.y;  
        }  
        catch(const std::exception& e)  
        {  
            std::cerr << e.what() << '\n';  
        }  
        pub.publish(ExpectedRunState);  
        rate.sleep();  
    }  
}

注意：

1）代码中lookupTransform函数的第三个参数表示“要进行转换的两个坐标系位置信息发布的时间间隔”，我们一般设置为ros::Time(0)/ros::Time()，即采用无效时间戳，该参数表示“要求在所有源坐标系和目标坐标系位置信息列表中挑选发布时间间隔在允许范围内最短的源坐标系和目标坐标系进行转换”：

因此，我们采用“3s时发布的源坐标系位置信息和4s时发布的目标坐标系位置信息”进行坐标系相对位置关系转换。

2）代码结构如下所示：

3）代码设计逻辑如下所示：

4. launch文件的编写：

<launch>  
    <!-- 启动第一只乌龟节点与键盘控制节点 -->  
    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node" output="screen" />  
    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>  
    <!-- 发布第一只乌龟的坐标信息 -->  
    <node pkg="tf2_turtle" type="getTurtle1Pose" name="getTurtle1Pose" output="screen"/>  
    <!-- 创建第二只乌龟并且订阅发布坐标信息 -->  
    <node pkg="tf2_turtle" type="getTurtle2Pose" name="getTurtle2Pose" output="screen"/>  
    <!-- 坐标变换 -->  
    <node pkg="tf2_turtle" type="frameTransform" name="frameTransform" output="screen"/>  
</launch> 

一般整个项目运行的逻辑都可以从launch文件中得出，上述launch文件告诉我们：

1） 启动turtlesim功能包下的turtlesim_node节点：

启动该节点可以在GUI中生成turtle1节点、实例化乌龟运动位姿信息的发布者对象；

2）启动turtlesim功能包下的turtle_teleop_key节点：

启动该节点可以实例化乌龟运动控制信息的订阅者对象；

3）启动自定义功能包tf2_turtle下的getTurtle2Pose节点：

启动该节点可以在GUI中生成turtle2节点并且订阅到turtle2乌龟运动节点的位姿信息；

4）启动自定义功能包tf2_turtle下的getTurtle1Pose节点：

启动该节点可以订阅到turtle1乌龟运动节点的位姿信息；

5）启动自定义功能包tf2_turtle下的frameTransform节点：

启动该节点可以监听到坐标系相对位置关系、从坐标系位置关系中计算得到turtle2的速度信息、发布turtle2的速度信息。launch文件的编写语法详解：

ROS中文件组织形式：launch文件+元功能包（示例+代码+参数解析）_超级霸霸强的博客-CSDN博客https://blog.csdn.net/weixin_45590473/article/details/122647788

5. 运行结果如下所示：