这片博客 教你 如何使用TF2 实现 监听 获得 一个 坐标变换
在上一篇博客 创建了一个TF2 的 广播者 ，来发布一个 乌龟的位置 到 TF2， 这个博客 将 创建 一个 TF2 的 监听者 来开始使用TF2.
1、如何创建一个TF2的监听者
1.1 代码
在learning_tf2 的功能包的src 文件夹下 创建一个新的cpp文件 命名为 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;
ros::service::waitForService("spawn");
ros::ServiceClient spawner =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn turtle;
turtle.request.x = 4;
turtle.request.y = 2;
turtle.request.theta = 0;
turtle.request.name = "turtle2";
spawner.call(turtle);
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0);
while (node.ok()){
geometry_msgs::TransformStamped transformStamped;
try{
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;
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));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
AI 代码解读
1.2 代码解释
=====================================================================
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
AI 代码解读
以上是需要包含的文件
tf2软件包提供了TransformListener的实现，以帮助简化接收转换的任务。 要使用TransformListener，需要包含tf2 / transform_listener.h头文件。
=====================================================================
ros::service::waitForService("spawn");
ros::ServiceClient spawner =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn turtle;
turtle.request.x = 4;
turtle.request.y = 2;
turtle.request.theta = 0;
turtle.request.name = "turtle2";
spawner.call(turtle);
AI 代码解读
turtle 相关 如此 则 创建 了 一个 乌龟 命名为：turtle2 初始位置 在 x=4，y=2 角度 为0
=====================================================================
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
AI 代码解读
声明 速度 topic 发布 句柄 下面 讲 发布 速度控制的 消息 来控制 turtle2 运动
=====================================================================
重点
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
AI 代码解读
在这里，创建一个TransformListener对象。 创建侦听器后，它将开始通过网络接收tf2转换，并将其最多缓冲10秒钟。 TransformListener对象的作用域应为持久性，否则其缓存将无法填充，并且几乎每个查询都会失败。 一种常见的方法是使TransformListener对象成为类的成员变量。
tf2_ros::TransformListener tfListener(tfBuffer)
这样 tfListener 相当于 TransformListener 类的实例 ，tfBuffer 相当于其成员变量
=====================================================================
geometry_msgs::TransformStamped transformStamped;AI 代码解读
声明 TF2 的标准 消息类型 变量 ，用于存放 监听的TF2
=====================================================================
重点
try{
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
AI 代码解读
这里是真正工作的地方，查询监听者一个指定的坐标变换。
通过 lookupTransform （） 函数 实现
这个函数有四个参数
lookupTransform 的 使用 说明
- 希望 变换到的 坐标系
- 从这个坐标系 （参数2），转换到目标坐标系（参数1）
- 转化希望时间 ， ros::Time(0) 即为 得到最新的 可用坐标变换、
- 超时前的时间 ， 可选 默认为 default=ros::Duration(0.0)
lookupTransform("turtle2", "turtle1",ros::Time(0)
这样即获得了 有 turtle1（坐标系） 到 turtle2 （坐标系）的 坐标变换
=====================================================================
geometry_msgs::Twist vel_msg;
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));
turtle_vel.publish(vel_msg);
AI 代码解读
乌龟1 的位置 在 turtle1（坐标系） 的 原点
所以 transformStamped.transform.translation.x / y 就是 以 乌龟2 为 原点在 turtle2 （坐标系）下的 位置。
通过这个位置转化为 速度 控制量 来控制 乌龟2 的 运动 实现 乌龟2 跟着 乌龟 1 的效果
=====================================================================
2、运行这个监听者
在 CMakeLists.txt 中 加入 如下 代码：
add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp) target_link_libraries(turtle_tf2_listener ${catkin_LIBRARIES} )AI 代码解读
编译成功的话
会 在工作空间 的这个路径下面 生成 turtle_tf2_listener 的 bin 文件
下面添加 launch 文件 给这个demo
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<node pkg="learning_tf2" type="turtle_tf2_broadcaster"
args="/turtle1" name="turtle1_tf2_broadcaster" />
<node pkg="learning_tf2" type="turtle_tf2_broadcaster"
args="/turtle2" name="turtle2_tf2_broadcaster" />
<node pkg="learning_tf2" type="turtle_tf2_listener"
name="listener" />
</launch>
AI 代码解读
运行
$ roslaunch learning_tf2 start_demo.launchAI 代码解读
会看到 有2 只 乌龟 在屏幕上 ， 乌龟1 不动 ，乌龟2 和乌龟1 初始位置不一样 ，一会 乌龟2 就到了 乌龟1 的地方
移动 乌龟 1
rosrun turtlesim turtle_teleop_key
AI 代码解读
乌龟2 会 跟着 乌龟 1 移动 最后 两者 位置 重合
