jsk_rviz_plugins: bounding_box_sample.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 
00004 import rospy
00005 
00006 from jsk_recognition_msgs.msg import BoundingBoxArray, BoundingBox
00007 from tf.transformations import *
00008 rospy.init_node("bbox_sample")
00009 pub = rospy.Publisher("bbox", BoundingBoxArray)
00010 r = rospy.Rate(24)
00011 counter = 0
00012 while not rospy.is_shutdown():
00013   box_a = BoundingBox()
00014   box_b = BoundingBox()
00015   box_a.label = 2
00016   box_b.label = 5
00017   box_arr = BoundingBoxArray()
00018   now = rospy.Time.now()
00019   box_a.header.stamp = now
00020   box_b.header.stamp = now
00021   box_arr.header.stamp = now
00022   box_a.header.frame_id = "map"
00023   box_b.header.frame_id = "map"
00024   box_arr.header.frame_id = "map"
00025   q = quaternion_about_axis((counter % 100) * math.pi * 2 / 100.0, [0, 0, 1])
00026   box_a.pose.orientation.x = q[0]
00027   box_a.pose.orientation.y = q[1]
00028   box_a.pose.orientation.z = q[2]
00029   box_a.pose.orientation.w = q[3]
00030   box_b.pose.orientation.w = 1
00031   box_b.pose.position.y = 2
00032   box_b.dimensions.x = (counter % 10 + 1) * 0.1
00033   box_b.dimensions.y = ((counter + 1) % 10 + 1) * 0.1
00034   box_b.dimensions.z = ((counter + 2) % 10 + 1) * 0.1
00035   box_a.dimensions.x = 1
00036   box_a.dimensions.y = 1
00037   box_a.dimensions.z = 1
00038   box_a.value = (counter % 100) / 100.0
00039   box_b.value = 1 - (counter % 100) / 100.0
00040   box_arr.boxes.append(box_a)
00041   box_arr.boxes.append(box_b)
00042   pub.publish(box_arr)
00043   r.sleep()
00044   counter = counter + 1
00045