jsk_topic_tools: pose_stamped_publisher.py Source File

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 PoseStamped
00006 from tf.transformations import quaternion_from_euler
00007 
00008 def usage():
00009     "print usage"
00010     print "Usage: pose_stamped_publisher.py x y z roll pitch yaw from_frame rate"
00011 
00012 def eulerToQuaternion(roll, pitch, yaw):
00013     "convert roll-pitch-yaw to quaternion"
00014     return quaternion_from_euler(roll, pitch, yaw)
00015     
00016 if __name__ == "__main__":
00017     rospy.init_node("pose_stamped_publisher")
00018     argv = rospy.myargv()
00019     if len(argv) != 9:
00020         usage()
00021         sys.exit(1)
00022     x = float(argv[1])
00023     y = float(argv[2])
00024     z = float(argv[3])
00025     roll = float(argv[4])
00026     pitch = float(argv[5])
00027     yaw = float(argv[6])
00028     frame = argv[7]
00029     rate = float(argv[8])
00030     pose = PoseStamped()
00031     
00032     pose.header.frame_id = frame
00033     pose.pose.position.x = x
00034     pose.pose.position.y = y
00035     pose.pose.position.z = z
00036     # roll, pitch, yaw to quaternion
00037     q = eulerToQuaternion(roll, pitch, yaw)
00038     pose.pose.orientation.x = q[0]
00039     pose.pose.orientation.y = q[1]
00040     pose.pose.orientation.x = q[2]
00041     pose.pose.orientation.w = q[3]
00042     r = rospy.Rate(rate)
00043     pub = rospy.Publisher("~output", PoseStamped)
00044     while not rospy.is_shutdown():
00045         pose.header.stamp = rospy.Time.now()
00046         pub.publish(pose)
00047         r.sleep()