point_test.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 
00004 import roslib
00005 roslib.load_manifest('tf')
00006 import rospy, tf
00007 from geometry_msgs.msg import PointStamped
00008 from math import *
00009 
00010 pub = rospy.Publisher('point_test', PointStamped)
00011 rospy.init_node('point_test')
00012 r = rospy.Rate(10)
00013 br = tf.TransformBroadcaster()
00014 
00015 count = 0;
00016 while not rospy.is_shutdown():
00017     t = count/10.0
00018     br.sendTransform((3*sin(t/10),3*cos(t/10),sin(t/20)),
00019                      tf.transformations.quaternion_from_euler(0,0,0),
00020                      rospy.Time.now(),
00021                      "/map", "/point")
00022     msg = PointStamped();
00023     msg.header.frame_id = "/point"
00024     msg.header.stamp = rospy.Time.now();
00025     msg.point.x = 2*sin(t);
00026     msg.point.y = 2*cos(t);
00027     msg.point.z = 0;
00028     print msg
00029     pub.publish(msg);
00030     r.sleep()
00031     count += 1;
00032 


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sun Sep 13 2015 22:29:03