torus_array_sample.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 from jsk_recognition_msgs.msg import TorusArray, Torus
5 from geometry_msgs.msg import Pose
6 import math
7 
8 rospy.init_node("test_torus")
9 p = rospy.Publisher("test_torus", TorusArray)
10 r = rospy.Rate(5)
11 counter = 0
12 while not rospy.is_shutdown():
13  torus_array = TorusArray()
14  torus1 = Torus()
15  torus1.header.frame_id = "base_link"
16  torus1.large_radius = 4
17  torus1.small_radius = 1
18  p1 = Pose()
19  p1.position.x = 2
20  p1.position.z = 4
21  p1.orientation.x = math.sqrt(0.5)
22  p1.orientation.y = math.sqrt(0.5)
23  torus1.pose = p1
24 
25  torus2 = Torus()
26  torus2.header.frame_id = "base_link"
27  torus2.large_radius = 5
28  torus2.small_radius = 1
29  p2 = Pose()
30  p2.position.x = 3
31  p2.position.y = -4
32  p2.orientation.z = math.sqrt(0.5)
33  p2.orientation.y = math.sqrt(0.5)
34  torus2.pose = p2
35 
36  torus_array.header.frame_id = "base_link"
37 
38  torus_array.toruses.append(torus1)
39  torus_array.toruses.append(torus2)
40 
41  p.publish(torus_array)
42  r.sleep()


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Sat Mar 20 2021 03:03:18