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_pcl_ros.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_arr = BoundingBoxArray()
00016   now = rospy.Time.now()
00017   box_a.header.stamp = now
00018   box_b.header.stamp = now
00019   box_arr.header.stamp = now
00020   box_a.header.frame_id = "map"
00021   box_b.header.frame_id = "map"
00022   box_arr.header.frame_id = "map"
00023   q = quaternion_about_axis((counter % 100) * math.pi * 2 / 100.0, [0, 0, 1])
00024   box_a.pose.orientation.x = q[0]
00025   box_a.pose.orientation.y = q[1]
00026   box_a.pose.orientation.z = q[2]
00027   box_a.pose.orientation.w = q[3]
00028   box_b.pose.orientation.w = 1
00029   box_b.pose.position.y = 2
00030   box_b.dimensions.x = (counter % 10 + 1) * 0.1
00031   box_b.dimensions.y = ((counter + 1) % 10 + 1) * 0.1
00032   box_b.dimensions.z = ((counter + 2) % 10 + 1) * 0.1
00033   box_a.dimensions.x = 1
00034   box_a.dimensions.y = 1
00035   box_a.dimensions.z = 1
00036   box_arr.boxes.append(box_a)
00037   box_arr.boxes.append(box_b)
00038   pub.publish(box_arr)
00039   r.sleep()
00040   counter = counter + 1