tf_set.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('dynamic_tf_publisher')
00003 
00004 import sys
00005 import rospy
00006 import tf
00007 from dynamic_tf_publisher.srv import SetDynamicTF
00008 from geometry_msgs.msg import *
00009 
00010 def set_tf(pos, q, parent, child, freq):
00011     rospy.wait_for_service('/set_dynamic_tf')
00012     try:
00013         client = rospy.ServiceProxy('/set_dynamic_tf', SetDynamicTF)
00014         pose = TransformStamped()
00015         pose.header.frame_id = pa
00016         pose.child_frame_id = ch
00017         pose.transform.translation = Vector3(*pos)
00018         pose.transform.rotation = Quaternion(*q)
00019         print pose
00020         res = client(freq, pose)
00021         return
00022     except rospy.ServiceException, e:
00023         print "Service call failed: %s"%e
00024 
00025 def isfloat(str):
00026     try:
00027         float(str)
00028         return True
00029     except Exception, e:
00030         return False
00031 
00032 if __name__ == "__main__":
00033     try:
00034         pos = map(float,sys.argv[1:4])
00035         if isfloat(sys.argv[7]): # quaternion
00036             q = map(float,sys.argv[4:8])
00037             rest = sys.argv[8:]
00038         else: # rpy
00039             rpy = map(float,sys.argv[4:7])
00040             # rpy angle is in "zyx" axis
00041             q = tf.transformations.quaternion_from_euler(*rpy, axes="rzyx")
00042             print q
00043             rest = sys.argv[7:]
00044         pa = rest[0]
00045         ch = rest[1]
00046         hz = 1000.0 / float(rest[2])
00047         set_tf(pos, q, pa, ch, hz)
00048     except Exception, e:
00049         print sys.argv
00050         print "args: x y z (r p y)|(x y z w) parent child msec"
00051     sys.exit(0)


dynamic_tf_publisher
Author(s): Manabu Saito
autogenerated on Sun Jan 25 2015 00:28:48