上次我们学习了 TF 的基本概念和如何发布静态的 TF 坐标:
这次来总结下如何发布一个自定义的 TF 坐标转换,并监听这个变换。
一、编写 TF 广播者
进入上次创建的 learning_tf2
包中:
roscd learning_tf2
在 src
下新建一个 turtle_tf2_broadcaster.cpp
文件,代码如下:
#include <ros/ros.h>
// 存储要发布的坐标变换
#include <geometry_msgs/TransformStamped.h>
// 四元数
#include <tf2/LinearMath/Quaternion.h>
// 变换广播者
#include <tf2_ros/transform_broadcaster.h>
// 乌龟的位姿定义
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建 tf 广播对象
static tf2_ros::TransformBroadcaster br;
// 存储要发布的坐标变换消息
geometry_msgs::TransformStamped transformStamped;
// 变换的时间戳
transformStamped.header.stamp = ros::Time::now();
// 父坐标系名称
transformStamped.header.frame_id = "world";
// 当前要发布的坐标系名称 - 乌龟的名字
transformStamped.child_frame_id = turtle_name;
// 乌龟在二维平面运动,所以 z 坐标高度为 0
transformStamped.transform.translation.x = msg->x;
transformStamped.transform.translation.y = msg->y;
transformStamped.transform.translation.z = 0.0;
// 用四元数存储乌龟的旋转角
tf2::Quaternion q;
// 因为乌龟在二维平面运动,只能绕 z 轴旋转,所以 x,y 轴的旋转量为 0
q.setRPY(0, 0, msg->theta);
// 把四元数拷贝到要发布的坐标变换中
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
// 用 tf 广播者把订阅的乌龟位姿发布到 tf 中
br.sendTransform(transformStamped);
}
int main(int argc, char** argv)
{
// 当前节点的名称
ros::init(argc, argv, "my_tf2_broadcaster");
ros::NodeHandle private_node("~");
// 判断当前要广播的乌龟节点名字
if (!private_node.hasParam