【ROS】续：官方tf教程turtle_tf2源码原理解读

最新推荐文章于 2024-04-20 19:59:02 发布

Wzt_Robot_Dream

最新推荐文章于 2024-04-20 19:59:02 发布

阅读量611

点赞数 3

文章标签： stm32 c++

本文链接：https://blog.csdn.net/weixin_43171162/article/details/125565975

版权

接上一章节的内容，本文讲的是 turtle_tf2_listener.cpp里面的源码。

直接上源码吧：

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_listener");

ros::NodeHandle node;

// 调用服务产生第二只乌龟turtle2

ros::service::waitForService("spawn");
ros::ServiceClient spawner =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn turtle;
turtle.request.x = 4; //turtle2 生成时在world坐标系下的x轴为4，这里可以自己改变数值好好体会一下
turtle.request.y = 2; //turtle2 生成时在world坐标系下的y轴为2，这里可以自己改变数值好好体会一下
turtle.request.theta = 0; //turtle2 生成时在world坐标系下的z轴为0，0度表明龟头就是向右。
turtle.request.name = "turtle2";
spawner.call(turtle);

// 声明控制turtle2速度的Publisher

ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

//实例化一个接收tf数据buff的缓冲区

tf2_ros::Buffer tfBuffer;

//创建一个TransformListener对象。创建侦听器后，它将开始通过网络接收所有的tf2转换.
//TransformListener对象的作用域应为持久性，否则其缓存将无法填充，并且几乎每个查询都会失败。
//一种常见的方法是使TransformListener对象成为类的成员变量。
//这样 tfListener 相当于 TransformListener 类的实例，tfBuffer 相当于其成员变量
tf2_ros::TransformListener tfListener(tfBuffer);

//发布信息的速度为10Hz， ros::Rate loop_rate( )写在循环外部

ros::Rate rate(10.0);
while (node.ok()){
geometry_msgs::TransformStamped transformStamped;

// try-catch结构，可以获取抛出的异常
try{

// 1. 定义监听器 tf::TransformListener listener
// 2. 定义存放变换关系的变量 tf::StampedTransform transform
// 3. lookupTransform()是tf库的核心方法用于监听两个坐标系之间的变换，实现/turtle2坐标系到turtle1坐标系的变换
// 4. ros::Time(0) 表示最新一次的坐标变换。我们只能订阅最新的tf变换，而不能订阅当下的tf变换，也就是说只能监听ros::Time(0)时刻最新的，
// 而不能监听ros::Time::now()，因为建立tf是需要时间的
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}

geometry_msgs::Twist vel_msg;

// pow(x,2)求x的平方
// sqrt(x)求x的平方根
//为什么只要x轴的线速度和z轴的角速度，好好想想就知道原因了
vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
transformStamped.transform.translation.x);
vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
pow(transformStamped.transform.translation.y, 2));

//Publisher海龟2的速度
turtle_vel.publish(vel_msg);

rate.sleep();
}
return 0;
};

最后：如果看过上一章节 turtle_tf2_broadcaster.cpp里面的解析，我们应该知道 turtle1 开始是在世界坐标系的原点的，因为：poseCallback回调函数里面的：

transformStamped.transform.translation.x = msg->x;
transformStamped.transform.translation.y = msg->y;

源码里面直接时msg->x;msg->y;就代表是msg->x+0;msg->y+0;与世界坐标系（world）原点重合！！这里至关重要，明白这一点就好办了

最后的最后上一下，海龟2追海龟1，海龟3追海龟2，3个海龟的源码。turtle_tf2_listener.cpp文件如下修改：