最近通过学习读取编码器值从而计算出角度,不可避免的了解到了一些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("出错,退出中...")
接下来你会如下看到效果