tf_publish.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib; roslib.load_manifest('pr2_plugs_actions')
00004 import rospy
00005 import sys
00006 import actionlib
00007 import tf
00008 from pr2_common_action_msgs.msg import *
00009 from pr2_plugs_msgs.msg import *
00010 from pr2_controllers_msgs.msg import *
00011 from actionlib_msgs.msg import *
00012 from geometry_msgs.msg import *
00013 from trajectory_msgs.msg import JointTrajectoryPoint
00014 
00015 
00016 class tfPublisher:
00017     def __init__(self, my_topic, my_type, my_field, my_tf_name):
00018         self.initialized = False
00019         self.pose = PoseStamped()
00020         rospy.Subscriber(my_topic, eval(my_type), self.callback)
00021         self.tf_broadcaster = tf.TransformBroadcaster()
00022         self.my_tf_name = my_tf_name
00023         self.my_field = my_field
00024 
00025     def callback(self, data):
00026         self.pose = eval("data."+self.my_field)
00027         if(len(self.pose.header.frame_id)>0):
00028             if not self.initialized:
00029                 rospy.loginfo('Initializing tf publisher')
00030             self.initialized = True
00031 
00032     def publish_tf(self): 
00033         if self.initialized:
00034             position = self.pose.pose.position
00035             orientation = self.pose.pose.orientation
00036             self.tf_broadcaster.sendTransform((position.x, position.y, position.z), 
00037                                               (orientation.x, orientation.y, orientation.z, orientation.w),
00038                                               rospy.Time.now(), self.my_tf_name, self.pose.header.frame_id)
00039         
00040 
00041 
00042 if __name__ == '__main__':
00043     rospy.init_node('listener', anonymous=True)
00044     my_argv = rospy.myargv()
00045 
00046     if len(my_argv) != 5:
00047         rospy.logerr('tf publisher requires 4 arguments: topic type field tf_name')
00048         
00049     else:
00050         my_topic = my_argv[1]
00051         rospy.loginfo('Topic: %s' % my_topic)
00052         
00053         my_type = my_argv[2]
00054         rospy.loginfo('Type: %s' % my_type)
00055         
00056         my_field = my_argv[3]
00057         rospy.loginfo('Field: %s' % my_field)
00058 
00059         my_tf_name = my_argv[4]
00060         rospy.loginfo('Tf name: %s' % my_tf_name)
00061 
00062         tf_pub = tfPublisher(my_topic, my_type, my_field, my_tf_name)
00063         while not rospy.is_shutdown():
00064             tf_pub.publish_tf()
00065             rospy.sleep(0.1)
00066 
00067 
00068 
00069         


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13