![ed0e01b6e203f944ab73606076583801.png](https://i-blog.csdnimg.cn/blog_migrate/1eddd8046ba176ec07781d8ec6caad3c.jpeg)
~欢迎关注~
微信公众号:古月居
新浪微博:古月春旭
知乎专栏:古月居
原文链接:ROS技术点滴 —— 海龟例程中的tf
先学知识: ROS技术点滴 —— tf坐标变换库
代码例程: https:// github.com/huchunxu/ros _exploring
本篇我们在海龟仿真器中,通过一个例程(turtle_tf)来理解TF的作用,并且熟悉之前学到的TF工具。该例程的功能包turtle_tf可以使用如下命令进行安装:
$ sudo apt-get install ros-kinetic-turtle-tf
安装完成后,就可以使用如下命令运行例程了:
$ roslaunch turtle_tf turtle_tf_demo.launch
海龟仿真器打开后会出现两只小海龟,并且下方的小海龟自动向中心位置的小海龟移动。
![db6eed18590a10a3303d34ea38664bca.png](https://i-blog.csdnimg.cn/blog_migrate/8ca9062d4d659fb126d829b4922c9d7c.jpeg)
打开键盘控制节点,控制中心位置的小海龟运行:
$ rosrun turtlesim turtle_teleop_key
![e2d21081131bfdde24d65a6c425e0be8.png](https://i-blog.csdnimg.cn/blog_migrate/9216c2838a1bfd1da404624ba75bfe6b.jpeg)
另外一只海龟总是会跟随我们控制的那只海龟运行。在这个例程中,TF是如何运用的呢?我们首先使用TF工具来看一下这个例程中的TF树是什么样的:
$ rosrun tf view_frames
![16aa8dbf1688b34c2a41f2faed694f14.png](https://i-blog.csdnimg.cn/blog_migrate/ace3965ba9abaa04fee2820484a0b9b6.jpeg)
在当前系统中存在三个坐标系:world、turtle1、turtle2。world是世界坐标系,作为系统的基础坐标系,其他坐标系都相对该坐标系建立,所以world是TF树的根节点。相对于world坐标系,又分别针对两只海龟创建了两个海龟坐标系,这两个坐标系的原点就是海龟在世界坐标系下的坐标位置。
现在要让turtle2跟随turtle1运动,相当于turtle2坐标系向turtle1坐标系移动,这就需要知道turtle2与turtle1之间的坐标变换。三个坐标系之间的变换关系可以使用如下公式描述:
![dcd14e3750c6fcd6403e57524e55df60.png](https://i-blog.csdnimg.cn/blog_migrate/162d7cca828a9b9073f022fc7828f5eb.png)
使用tf_echo工具在TF树中查找海龟坐标系之间的变换关系:
$ rosrun tf tf_echoturtle1 turtle2
![aef2e499fa9aa9fbec0eff96a07b77ae.png](https://i-blog.csdnimg.cn/blog_migrate/dd987bac1bff45a752abe7e29be08cb8.jpeg)
也可以通过rviz的图形界面更加形象的看到这三者之间的坐标关系:
$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
得到turtle2与turtle1之间的坐标变换后,就可以计算两只海龟间的距离和角度,即可控制turtle2向turtle1移动了。
接下来,我们以这个例程为目标,学习如何实现TF的广播和监听功能。
一、创建TF广播器
首先,我们需要创建一个发布海龟坐标系与世界坐标系之间TF变换的节点,实现源码turtle_tf_broadcaster.cpp的具体内容如下:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// tf广播器
static tf::TransformBroadcaster br;
// 根据海龟当前的位姿,设置相对于世界坐标系的坐标变换
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 发布坐标变换
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化节点
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
};
turtle_name = argv[1];
// 订阅海龟的pose信息
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
以上代码的关键部分是处理海龟pose消息的回调函数poseCallback。在广播TF消息之前需要定义tf::TransformBroadcaster广播器,然后根据海龟当前的位姿,设置tf::Transform类型的坐标变换,包含setOrigin设置的平移变换以及setRotation设置的旋转变换。
然后使用广播器将坐标变换插入TF树并且发布,这里发布的TF消息类型是tf::StampedTransform,不仅包含tf::Transform类型的坐标变换、时间戳,而且还需要指定坐标变换的源坐标系(parent)和目标坐标系(child)。
二、创建TF监听器
TF消息广播之后,其他节点就可以监听该TF消息,从而获取需要的坐标变换了。
目前我们已经将海龟相对于world坐标系的TF变换广播,接下来需要监听TF消息,并从中获取turtle2相对于turtle1坐标系的变换,从而控制turtle2移动。实现源码turtle_tf_listener.cpp的详细内容如下:
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
// 初始化节点
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
// 通过服务调用,产生第二只乌龟turtle2
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 定义turtle2的速度控制发布器
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
// tf监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
tf::StampedTransform transform;
try
{
// 查找turtle2与turtle1的坐标变换
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1和turtle2之间的坐标变换,计算turtle2需要运动的线速度和角速度
// 并发布速度控制指令,使turtle2向turtle1移动
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
该节点首先通过服务调用产生海龟turtle2,然后声明控制turtle2速度的Publisher。在监听TF消息之前,需要创建一个tf::TransformListener类型的监听器,创建成功后监听器会自动接收TF树的消息,并且缓存10秒。
然后在循环中就可以实时查找TF树中的坐标变换了,这里需要调用的是tf::TransformListener中的两个接口:
waitForTransform(const std::string &target_frame, const std::string&source_frame, const ros::Time&time, const ros::Duration &timeout)
给定的源坐标系(source_frame)和目标坐标系(target_frame),等待两个坐标系之间指定时间(time)的变换关系,该函数会阻塞程序运行,所以需要设置超时时间(timeout)。
lookupTransform(const std::string & target_frame,const std::string &source_frame, const ros::Time & time,StampedTransform & transform)
给定的源坐标系(source_frame)和目标坐标系(target_frame),得到两个坐标系之间指定时间(time)的坐标变换(transform),ros::Time(0) 表示我们想要的是最新一次的坐标变换。
通过以上两个接口的调用,就可以获取turtle2相对于turtle1的坐标变换了。然后根据坐标系之间的位置关系,计算得到turtle2需要运动的线速度和角速度,并发布速度控制指令,使turtle2向turtle1移动。
三、实现海龟跟随运动
现在小海龟跟随例程的所有代码都已经完成,我们来编写一个launch文件,将所有节点运行起来,start_demo_with_listener.launch:
<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 两只海龟的tf广播 -->
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
<!-- 监听tf广播,并且控制turtle2移动 -->
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>
运行该launch文件,就可以看到与之前例程类似的两只海龟的界面了,在终端中通过键盘控制turtle1移动,turtle2也跟随移动。
通过这个例程的实现,我们学习了TF广播与监听的实现方法,在实际应用中会产生更多坐标系,TF树的结构也会更加复杂,但是基本的使用方法依然相同。
更多内容欢迎关注:
微信公众号:古月居 (guyue_home)
新浪微博:古月春旭
知乎专栏:古月居
或访问古月居网站: