update_ear_transforms.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib
00004 roslib.load_manifest('rfid_hardware')
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         # Right pan
00026         rospy.Subscriber( right_pan, Float64, 
00027                           functools.partial( self.rpan_cb, bc = tf.TransformBroadcaster() ))
00028 
00029         # Right tilt
00030         rospy.Subscriber( right_tilt, Float64, 
00031                           functools.partial( self.rtilt_cb, bc = tf.TransformBroadcaster() ))
00032 
00033         # Left pan
00034         rospy.Subscriber( left_pan, Float64, 
00035                           functools.partial( self.lpan_cb, bc = tf.TransformBroadcaster() ))
00036 
00037         # Left tilt
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         # bc is a specific TransformBroadcaster
00046         bc.sendTransform( (-0.0655, -0.0510, 0.0675),
00047                           tf.transformations.quaternion_from_euler( 0.0, 0.0, -1.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         # bc is a specific TransformBroadcaster
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         # bc is a specific TransformBroadcaster
00062         bc.sendTransform( (-0.0655, +0.0510, 0.0675),
00063                           tf.transformations.quaternion_from_euler( 0.0, 0.0, -1.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         # bc is a specific TransformBroadcaster
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()


rfid_hardware
Author(s): Travis Deyle
autogenerated on Wed Nov 27 2013 11:48:46