00001
00002
00003 import roslib
00004 roslib.load_manifest('rfid_people_following')
00005 import rospy
00006
00007 from std_msgs.msg import Float64
00008 import tf
00009
00010 import time
00011 import numpy as np, math
00012 import functools
00013
00014 class tf_updater():
00015 def __init__(self, name,
00016 right_pan = '/robotis/servo_right_pan',
00017 right_tilt = '/robotis/servo_right_tilt',
00018 left_pan = '/robotis/servo_left_pan',
00019 left_tilt = '/robotis/servo_left_tilt'):
00020 try:
00021 rospy.init_node( name )
00022 except:
00023 pass
00024
00025
00026 rospy.Subscriber( right_pan, Float64,
00027 functools.partial( self.rpan_cb, bc = tf.TransformBroadcaster() ))
00028
00029
00030 rospy.Subscriber( right_tilt, Float64,
00031 functools.partial( self.rtilt_cb, bc = tf.TransformBroadcaster() ))
00032
00033
00034 rospy.Subscriber( left_pan, Float64,
00035 functools.partial( self.lpan_cb, bc = tf.TransformBroadcaster() ))
00036
00037
00038 rospy.Subscriber( left_tilt, Float64,
00039 functools.partial( self.ltilt_cb, bc = tf.TransformBroadcaster() ))
00040
00041
00042
00043
00044 def rpan_cb( self, ang_msg, bc ):
00045
00046 bc.sendTransform( (-0.0655, -0.0510, 0.0675),
00047 tf.transformations.quaternion_from_euler( 0.0, 0.0, ang_msg.data - math.radians( 60.0 ) ),
00048 rospy.Time.now(),
00049 'ear_pan_right',
00050 'plate_right_base' )
00051
00052 def rtilt_cb( self, ang_msg, bc ):
00053
00054 bc.sendTransform( (0.0673, 0.0, 0.0),
00055 tf.transformations.quaternion_from_euler( 0.0, ang_msg.data, 0.0 ),
00056 rospy.Time.now(),
00057 'ear_tilt_right',
00058 'ear_pan_right' )
00059
00060 def lpan_cb( self, ang_msg, bc ):
00061
00062 bc.sendTransform( (-0.0655, +0.0510, 0.0675),
00063 tf.transformations.quaternion_from_euler( 0.0, 0.0, ang_msg.data + math.radians( 60.0 ) ),
00064 rospy.Time.now(),
00065 'ear_pan_left',
00066 'plate_left_base' )
00067
00068 def ltilt_cb( self, ang_msg, bc ):
00069
00070 bc.sendTransform( (0.0673, 0.0, 0.0),
00071 tf.transformations.quaternion_from_euler( 0.0, -1.0 * ang_msg.data, 0.0 ),
00072 rospy.Time.now(),
00073 'ear_tilt_left',
00074 'ear_pan_left' )
00075
00076
00077
00078
00079
00080 if __name__ == '__main__':
00081 tfs = tf_updater('servo_tf_updater')
00082 rospy.spin()