torus_array_sample.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 from jsk_recognition_msgs.msg import TorusArray, Torus
00005 from geometry_msgs.msg import Pose
00006 import math
00007 
00008 rospy.init_node("test_torus")
00009 p = rospy.Publisher("test_torus", TorusArray)
00010 r = rospy.Rate(5)
00011 counter = 0
00012 while not rospy.is_shutdown():
00013   torus_array = TorusArray()
00014   torus1 = Torus()
00015   torus1.header.frame_id = "base_link"
00016   torus1.large_radius = 4
00017   torus1.small_radius = 1
00018   p1 = Pose()
00019   p1.position.x = 2
00020   p1.position.z = 4
00021   p1.orientation.x = math.sqrt(0.5)
00022   p1.orientation.y = math.sqrt(0.5)
00023   torus1.pose = p1
00024 
00025   torus2 = Torus()
00026   torus2.header.frame_id = "base_link"
00027   torus2.large_radius = 5
00028   torus2.small_radius = 1
00029   p2 = Pose()
00030   p2.position.x = 3
00031   p2.position.y = -4
00032   p2.orientation.z = math.sqrt(0.5)
00033   p2.orientation.y = math.sqrt(0.5)
00034   torus2.pose = p2
00035 
00036   torus_array.header.frame_id = "base_link"
00037 
00038   torus_array.toruses.append(torus1)
00039   torus_array.toruses.append(torus2)
00040 
00041   p.publish(torus_array)
00042   r.sleep()


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