[python]通过读取编码器值来学习TF转换

最近通过学习读取编码器值从而计算出角度，不可避免的了解到了一些TF转换的知识。

接下来将有一个小脚本来看现象（这个脚本是根据校正底盘角度写的）：

#!/usr/bin/env python
#coding=utf-8


import rospy
from geometry_msgs.msg import Quaternion
from nav_msgs.msg import Odometry
import tf
from math import radians
import PyKDL

def quat_to_angle(quat):
	rot = PyKDL.Rotation.Quaternion(quat.x,quat.y,quat.z,quat.w)
	return rot.GetRPY()[2]
	
class MyOdomRead():
	def __init__(self):
		rospy.init_node('calibrate_angular',anonymous=False)
		rospy.on_shutdown(self.shutdown)

		self.testangle = rospy.get_param('~test_angle',360.0)
		self.test_angle = radians(self.testangle)#这里其实并没有用，但是这里有一个由角度转换为弧度的函数很有意义
		self.start_test = rospy.get_param('~start_test',True)
		self.base_frame = rospy.get_param('~base_frame','/base_footprint')
		self.odom_frame = rospy.get_param('~odom_frame','/odom')

		self.tf_listener = tf.TransformListener()
		rospy.sleep(2)
		self.tf_listener.waitForTransform(self.odom_frame,self.base_frame,rospy.Time(),rospy.Duration(60.0))
                #官方解释：tf provides a nice tool that will wait until a transform becomes available.
                #翻译：TF提供了一个很好的工具，可以等到转换变得可用为止。
		while not rospy.is_shutdown():
			self.odom_angle = self.get_odom_angle()#获取角度值--弧度
			print self.odom_angle
			 
	def get_odom_angle(self):
		try:
			(trans,rot)=self.tf_listener.lookupTransform(self.odom_frame,self.base_frame,rospy.Time(0))
                        #官方解释： Until now we used the lookupTransform() function to get access to the latest available transforms in that tf tree, without knowing at what time that transform was recorded. 
                        #翻译：我们可以利用lookupTransform()访问该TF树中的最新可用转换，无需知道时间
		except (tf.Exception,tf.ConnectivityException,tf.LookupExceptiom):
			rospy.loginfo("TF转换有问题")
			return
		return quat_to_angle(Quaternion(*rot))

	def shutdown(self):
		rospy.loginfo("读取停止中...")
		rospy.sleep(1)
if __name__=='__main__':
	try:
		MyOdomRead()
	except:
		rospy.loginfo("Calibration terminated")

首先运行上面的脚本，然后再运行一个小脚本来使底盘原地沿着z轴转：

#! /usr/bin/env python
#coding=utf-8
import rospy
from geometry_msgs.msg import Twist 

class SpeedPub():
	def __init__(self):
		rospy.init_node('speedpub',anonymous=False)
		self.pub=rospy.Publisher('/cmd_vel',Twist,queue_size=10)
		rospy.on_shutdown(self.shutdown)
		self.twister=Twist()
		while not rospy.is_shutdown():
			self.twister.linear.x=0
			self.twister.linear.y=0
			self.twister.linear.z=0
			self.twister.angular.x=0
			self.twister.angular.y=0
			self.twister.angular.z=0.2
			self.pub.publish(self.twister)
	def shutdown(self):
		self.pub.publish(Twist())
		rospy.loginfo("机器人正在停止，请稍候...")
		rospy.sleep(4)
if __name__=='__main__':
	try:	
		SpeedPub()
	except:
		rospy.loginfo("出错，退出中...")

接下来你会如下看到效果

学习更多嵌入式Linux、Qt以及嵌入式单片机关注公众号“爱玩嵌入式”：