提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档
一、TF简介
在机器人中坐标的变换非常的重要,而且涉及多个坐标的变换,TF就是机器人坐标变换的功能包。简单来说,只要给定一个坐标系下的一个点的坐标,就能获得这个点在其他坐标系的坐标.。
提示:以下是本篇文章正文内容,下面案例可供参考
二、TF坐标变换演示
安装实例功能包(我用的是kintic版本的)
sudo apt-get install ros-kinetic-turtle-tf
运行launch文件
roslaunch turtle_tf turtle_tf_demo.launch
结果如图
运行键盘控制节点
rosrun turtlesim turtle_teleop_key
结果如图
这个示例中坐标变换是怎么样子的呢?我们就需要借助一款小工具了:view_frames
用命令在终端运行rosrun tf view_frames
,生成如图的文件
另外一款小工具tf_echo,可以查询任意两个坐标系之间的关系。查看turtle1和turtle2两个坐标系之间的关系。
rosrun tf tf_echo turtle1 turtle2
结果如图
这些信息表明的意思是 :turtle2怎么变换到turtle1
其中,Translation表示平移变换[x,y,z];Rotaion表示旋转变化;Quaternion(四元数,表示[x,y,z,w])、RPY(radian)–弧度和RPY(degree)–角度,都表示相同的意思。
第3个可视化工具rviz,指令
rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
结果如图
查看节点可用命令,可查看系统运行的全部节点以及相关的信息
rqt_graph
三、TF编程讲解
使用TF时,我们需要编写两个程序分别为TF广播器和TF监听器,功能分别是广播TF变换和监听TF变换。
- TF广播器:广播TF变换,向系统中广播参考系之间的坐标变换关系。系统中可能会存在多个不同部分的tf变换广播,但每个广播都可以直接将参考系变换关系直接插入tf树中,不需要再进行同步。
- TF监听器:监听TF变换,接收并缓存系统中发布的所有参考系变换,并从中查询所需要的参考系变换。
1.1、创建工作空间
在home目录下打开终端
mkdir my_test1 #创建文件夹my_test1
cd my_test1/ #进入到文件夹my_test1
mkdir src #创建文件夹src
cd src #进入到文件夹src
catkin_init_workspace #初始化工作空间
得到如下的工作空间
1.2、编译工作空间
cd ~/my_test1
catkin_make
catkin_make install #创建安装文件夹
此时,工作空间文件夹如下
1.3、创建功能包
cd ~/my_test1/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
在目录:/home/xzs/my_test1/src/learning_tf/src下添加C++文件,这个文件实现了一个tf的广播器
程序思路
- 初始化ROS节点,并订阅 turtle 的位置消息;
- 循环等待话题消息,接收到之后进入回调函数,该回调函数用以处理并发布坐标变换;
- 在该回调函数内部定义一个广播器;
- 根据接收到的小海龟的位置消息,创建坐标变换值
- 通过定义的广播器发布坐标变换
/**
* 该例程产生tf数据,并计算、发布turtle2的速度指令
*/
#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数据
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);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster");
// 输入参数作为海龟的名字
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
// 订阅海龟的位姿话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
// 循环等待回调函数
ros::spin();
return 0;
};
tf::Transform transform函数用法介绍:传送门
在相同的位置创建C++文件
此文件是实现一个TF监听器,内容如下
思路
- 初始化ROS节点,并向MASTER注册节点信息;
- 通过服务调用产生第二只海龟;
- 创建turtle2的速度控制发布器;
- 创建tf监听器并监听turtle2相对于turtle1的坐标变换;
- 根据坐标变换发布速度控制指令;
/**
* 该例程监听tf数据,并计算、发布turtle2的速度指令
*/
#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节点
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())
{
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;
try
{
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的速度控制指令
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;
};
listener函数用法介绍:传送门
- tf :: TransformListener :: waitForTransform函数,测试是否可以在时间time将source_frame转换为target_frame,并且返回一个bool类型值表明是否可以进行变换。
- tf :: TransformListener :: lookupTransform函数,返回两个坐标系之间的变换,返回的转换方向为从target_frame到source_frame。
然后配置CMakeLists.txt中的编译规则
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
返回工作空间的根目录编译
cd ~/my_test1/
catkin_make
1.4、设置环境变量并检查
cd ~/my_test1/
source devel/setup.bash #设置环境变量
echo $ROS_PACKAGE_PATH #检查环境变量
1.5、编译并运行
cd ~/my_test1
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key
(注意:不想每次打开一个终端就要source一下设置环境变量,可以在.bashrc文件添加环环境变量:source ~/my_test1/devel/setup.bash。关闭所有终端再重新打开才能生效,实在不行重启电脑)
运行到rosrun learning_tf turtle_tf_listener时,出现两个海龟,如图
运行键盘控制节点后rosrun turtlesim turtle_teleop_key,用键盘控制,可以看到海龟2跟随海龟1运动。
1.6、python版的实现
1.5、编译并运行
广播器的实现
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务,服务数据类型learning_service::Person
import roslib
roslib.load_manifest('learning_tf')
import rospy
import tf
import turtlesim.msg
def handle_turtle_pose(msg, turtlename):
br = tf.TransformBroadcaster()
br.sendTransform((msg.x, msg.y, 0),
tf.transformations.quaternion_from_euler(0, 0, msg.theta),
rospy.Time.now(),
turtlename,
"world")
if __name__ == '__main__':
rospy.init_node('turtle_tf_broadcaster')
turtlename = rospy.get_param('~turtle')
rospy.Subscriber('/%s/pose' % turtlename,
turtlesim.msg.Pose,
handle_turtle_pose,
turtlename)
rospy.spin()
监听器的实现
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务,服务数据类型learning_service::Person
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if __name__ == '__main__':
rospy.init_node('turtle_tf_listener')
listener = tf.TransformListener()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
rate = rospy.Rate(10.0)
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
angular = 4 * math.atan2(trans[1], trans[0])
linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
turtle_vel.publish(cmd)
rate.sleep()
存放的路径,scripts文件夹要自己创建。