ROS中使用TF广播器发布海龟机器人的位姿信息

这段代码通过订阅 /turtle1/pose 话题，获取到海龟机器人的位姿信息，然后将其转化为 TF 坐标系相对信息，通过 TF 广播器发布出去。具体实现步骤如下：

引入需要的头文件。

#include 'ros/ros.h'
#include 'turtlesim/Pose.h'
#include 'tf2_ros/transform_broadcaster.h'
#include 'geometry_msgs/TransformStamped.h'
#include 'tf2/LinearMath/Quaternion.h'

初始化 ROS 节点。

int main(int argc, char *argv[]) {
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "dynamic_tf_pub");
    ...
}

创建 ROS 句柄。

ros::NodeHandle nh;

创建订阅对象，订阅 /turtle1/pose 话题，设置回调函数为 doPose。

ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 1000, doPose);

在回调函数 doPose 中实现 TF 广播器的功能。

5-1. 创建 TF 广播器。

void doPose(const turtlesim::Pose::ConstPtr& pose) {
    static tf2_ros::TransformBroadcaster broadcaster;
    ...
}

5-2. 创建广播的数据，通过 pose 设置。

5-2-1. 头设置。

geometry_msgs::TransformStamped tfs;
...
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

5-2-2. 坐标系 ID。

tfs.child_frame_id = "turtle1";

5-2-3. 坐标系相对信息设置。

tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0

5-2-4. 四元数设置。

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);

spin()，进入 ROS 循环，等待回调函数被触发。

ros::spin();

通过上述步骤，就实现了将海龟机器人的位姿信息转化为 TF 坐标系相对信息的功能。