Go to the documentation of this file.00001
00002
00003 import rospy
00004 import tf
00005 from geometry_msgs.msg import PoseStamped
00006
00007 if __name__ == '__main__':
00008 rospy.init_node('publish_pose', anonymous=True)
00009 worldFrame = rospy.get_param("~worldFrame", "/world")
00010 name = rospy.get_param("~name")
00011 r = rospy.get_param("~rate")
00012 x = rospy.get_param("~x")
00013 y = rospy.get_param("~y")
00014 z = rospy.get_param("~z")
00015
00016 rate = rospy.Rate(r)
00017
00018 msg = PoseStamped()
00019 msg.header.seq = 0
00020 msg.header.stamp = rospy.Time.now()
00021 msg.header.frame_id = worldFrame
00022 msg.pose.position.x = x
00023 msg.pose.position.y = y
00024 msg.pose.position.z = z
00025 quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
00026 msg.pose.orientation.x = quaternion[0]
00027 msg.pose.orientation.y = quaternion[1]
00028 msg.pose.orientation.z = quaternion[2]
00029 msg.pose.orientation.w = quaternion[3]
00030
00031 pub = rospy.Publisher(name, PoseStamped, queue_size=1)
00032
00033 while not rospy.is_shutdown():
00034 msg.header.seq += 1
00035 msg.header.stamp = rospy.Time.now()
00036 pub.publish(msg)
00037 rate.sleep()