snapit_sample_pose_publisher.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import numpy
00005 from math import pi
00006 from geometry_msgs.msg import PoseStamped, Pose
00007 import tf
00008 def main():
00009     convex_pose_pub = rospy.Publisher("/snapit/input/convex_align", PoseStamped)
00010     plane_pose_pub = rospy.Publisher("/snapit/input/plane_align", PoseStamped)
00011     r = rospy.Rate(1)
00012     while not rospy.is_shutdown():
00013         x = (numpy.random.rand(1.0)[0] - 0.5) * 2.0
00014         y = (numpy.random.rand(1.0)[0] - 0.5) * 2.0
00015         z = (numpy.random.rand(1.0)[0] - 0.5) * 2.0
00016         # roll = (numpy.random.rand(1.0)[0] - 0.5) * pi
00017         # pitch = (numpy.random.rand(1.0)[0] - 0.5) * pi
00018         # yaw = (numpy.random.rand(1.0)[0] - 0.5) * pi
00019         roll = 0
00020         pitch = 0
00021         yaw = 0
00022         msg = PoseStamped()
00023         msg.pose.position.x = x
00024         msg.pose.position.y = y
00025         msg.pose.position.z = z
00026         q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
00027         msg.pose.orientation.x = q[0]
00028         msg.pose.orientation.y = q[1]
00029         msg.pose.orientation.z = q[2]
00030         msg.pose.orientation.w = q[3]
00031         msg.header.stamp = rospy.Time.now()
00032         if numpy.random.rand(1.0) < 0.5:
00033             msg.header.frame_id = "odom"
00034         else:
00035             msg.header.frame_id = "odom"
00036         convex_pose_pub.publish(msg)
00037         plane_pose_pub.publish(msg)
00038         r.sleep()
00039         
00040 if __name__ == "__main__":
00041     rospy.init_node("snapit_sample_pose_publisher")
00042     main()


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 04:36:48