tf_link_flipper.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 import roslib
00003 roslib.load_manifest('hrl_generic_arms')
00004 import rospy
00005 import tf
00006 import tf.transformations as tf_trans
00007 from hrl_generic_arms.pose_converter import PoseConverter
00008 
00009 def main():
00010     rospy.init_node("tf_link_flipper")
00011 
00012     child_frame = rospy.get_param("~child_frame")
00013     parent_frame = rospy.get_param("~parent_frame")
00014     link_frame = rospy.get_param("~link_frame")
00015     rate = rospy.get_param("~rate", 100)
00016     link_trans = rospy.get_param("~link_transform")
00017 
00018     l_B_c = PoseConverter.to_homo_mat(link_trans['pos'], link_trans['quat'])
00019 
00020     tf_broad = tf.TransformBroadcaster()
00021     tf_listener = tf.TransformListener()
00022     rospy.sleep(1)
00023     r = rospy.Rate(rate)
00024     while not rospy.is_shutdown():
00025         time = rospy.Time.now()
00026         tf_listener.waitForTransform(child_frame, parent_frame, rospy.Time(0), rospy.Duration(1))
00027         pos, quat = tf_listener.lookupTransform(child_frame, parent_frame, rospy.Time(0))
00028         c_B_p = PoseConverter.to_homo_mat(pos, quat)
00029         l_B_p = l_B_c * c_B_p
00030         tf_pos, tf_quat = PoseConverter.to_pos_quat(l_B_p)
00031         tf_broad.sendTransform(tf_pos, tf_quat, time, parent_frame, link_frame)
00032         r.sleep()
00033 
00034         
00035 
00036 if __name__ == "__main__":
00037     main()


hrl_phri_2011
Author(s): Kelsey Hawkins
autogenerated on Wed Nov 27 2013 12:22:40