ROS小车研究笔记：ROS TF坐标系管理

TF用于管理和查询机器人坐标系变换。通过TF，我们可以得到10秒之内任何机器人两个坐标系间的位置关系

TF使用广播/监听模型。各个节点的坐标构成TF树用以保存节点间坐标变换。如果一个节点要得到某一坐标系变换可以通过TF树进行查询

tf包中可视化tf树工具：

rosrun tf view_frames

监听tf树5秒并保存得到的结果，会生成一个pdf文件

rosrun tf tf_echo 坐标系1 坐标系2

获取到两个坐标系间的tf变换，可实时检测

tf广播和监听程序实现（基于官方海龟跟随案例）

tf广播器

#!/usr/bin/env python

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
	br = tf.TransformBroadcaster()
	br.snedTransform((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()

br = tf.TransformBroadcaster()

创建tf坐标广播器以广播tf坐标

br.snedTransform((msg.x, msg.y, 0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world")

创建tf坐标广播内容。由于该行在Pose回调函数里，我们使用Pose话题中得到的(x, y, 0) （由于海龟程序为平面不存在z轴）作为广播的位置坐标。Pose中theta作为角度坐标，时间戳（当前时间rospy.Time.now()），和要广播的两个坐标系

tf监听器

#!/usr/bin/env python

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()

listener = tf.TransformListener()

创建tf监听者

(trans, rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))

使用listener监听trutle2和turtle1在当前时间（rospy.Time(0)）的tf变换。得到位置trans和旋转rot

		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)

根据得到的平移和旋转坐标计算速度指令的线速度和角速度。发布速度指令Twist控制海龟跟随

ROS小车研究笔记：ROS TF坐标系管理

猜你喜欢