torus_array_sample.py

Go to the documentation of this file.

#!/usr/bin/env python

import rospy
from jsk_recognition_msgs.msg import TorusArray, Torus
from geometry_msgs.msg import Pose
import math

rospy.init_node("test_torus")
p = rospy.Publisher("test_torus", TorusArray)
r = rospy.Rate(5)
counter = 0
while not rospy.is_shutdown():
  torus_array = TorusArray()
  torus1 = Torus()
  torus1.header.frame_id = "base_link"
  torus1.large_radius = 4
  torus1.small_radius = 1
  p1 = Pose()
  p1.position.x = 2
  p1.position.z = 4
  p1.orientation.x = math.sqrt(0.5)
  p1.orientation.y = math.sqrt(0.5)
  torus1.pose = p1

  torus2 = Torus()
  torus2.header.frame_id = "base_link"
  torus2.large_radius = 5
  torus2.small_radius = 1
  p2 = Pose()
  p2.position.x = 3
  p2.position.y = -4
  p2.orientation.z = math.sqrt(0.5)
  p2.orientation.y = math.sqrt(0.5)
  torus2.pose = p2

  torus_array.header.frame_id = "base_link"

  torus_array.toruses.append(torus1)
  torus_array.toruses.append(torus2)

  p.publish(torus_array)
  r.sleep()