jsk_rviz_plugins: twist_stamped_add

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import sys
00005 from geometry_msgs.msg import Twist, TwistStamped
00006 
00007 rospy.init_node("twist_stamped_add_header")
00008 pub = rospy.Publisher("cmd_vel_stamped", TwistStamped)
00009 
00010 def callback(msg):
00011     global pub
00012     output = TwistStamped()
00013     output.header.stamp = rospy.Time.now()
00014     output.header.frame_id = sys.argv[1]
00015     output.twist = msg
00016     pub.publish(output)
00017 
00018 if len(sys.argv) != 3:
00019     print "Usage: twist_stamped_add_header frame_id topic"
00020 
00021 sub = rospy.Subscriber(sys.argv[2], Twist, callback)
00022 rospy.spin()
00023