jsk_recognition_utils: bounding_box_array

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import sys
00004 
00005 from geometry_msgs.msg import Point
00006 from geometry_msgs.msg import Quaternion
00007 from geometry_msgs.msg import Vector3
00008 from jsk_recognition_msgs.msg import BoundingBox
00009 from jsk_recognition_msgs.msg import BoundingBoxArray
00010 import rospy
00011 from tf.transformations import quaternion_from_euler
00012 
00013 
00014 class BoundingBoxArrayPublisher(object):
00015 
00016     def __init__(self):
00017         self.seq = 0
00018 
00019         self.frame_id = rospy.get_param('~frame_id')
00020         if (rospy.has_param('~positions') or
00021                 rospy.has_param('~rotations') or
00022                 rospy.has_param('~dimensions')):
00023             # Deprecated bounding box pose/dimension specification
00024             rospy.logwarn("DEPRECATION WARNING: Rosparam '~positions', "
00025                           "'~rotations' and '~dimensions' are being "
00026                           "deprecated. Please use '~boxes' instead.")
00027             positions = rospy.get_param('~positions')
00028             rotations = rospy.get_param('~rotations')
00029             dimensions = rospy.get_param('~dimensions')
00030             if len(rotations) != len(positions):
00031                 rospy.logfatal('Number of ~rotations is expected as {}, but {}'
00032                             .format(len(positions), len(rotations)))
00033                 sys.exit(1)
00034             if len(dimensions) != len(positions):
00035                 rospy.logfatal('Number of ~dimensions is expected as {}, but {}'
00036                             .format(len(positions), len(dimensions)))
00037                 sys.exit(1)
00038             self.boxes = []
00039             for pos, rot, dim in zip(positions, rotations, dimensions):
00040                 self.boxes.append({
00041                     'position': pos,
00042                     'rotation': rot,
00043                     'dimension': dim,
00044                 })
00045         else:
00046             self.boxes = rospy.get_param('~boxes')
00047         self.pub = rospy.Publisher('~output', BoundingBoxArray, queue_size=1)
00048 
00049         rate = rospy.get_param('~rate', 1)
00050         self.timer = rospy.Timer(rospy.Duration(1. / rate), self.publish)
00051 
00052     def publish(self, event):
00053         bbox_array_msg = BoundingBoxArray()
00054         bbox_array_msg.header.seq = self.seq
00055         bbox_array_msg.header.frame_id = self.frame_id
00056         bbox_array_msg.header.stamp = event.current_real
00057         for box in self.boxes:
00058             pos = box['position']
00059             rot = box.get('rotation', [0, 0, 0])
00060             qua = quaternion_from_euler(*rot)
00061             dim = box['dimension']
00062             label = box.get('label', 0)
00063 
00064             bbox_msg = BoundingBox()
00065             bbox_msg.header.seq = self.seq
00066             bbox_msg.header.frame_id = self.frame_id
00067             bbox_msg.header.stamp = event.current_real
00068             bbox_msg.pose.position = Point(*pos)
00069             bbox_msg.pose.orientation = Quaternion(*qua)
00070             bbox_msg.dimensions = Vector3(*dim)
00071             bbox_msg.label = label
00072 
00073             bbox_array_msg.boxes.append(bbox_msg)
00074 
00075         self.pub.publish(bbox_array_msg)
00076 
00077 
00078 if __name__ == '__main__':
00079     rospy.init_node('bounding_box_array_publisher')
00080     BoundingBoxArrayPublisher()
00081     rospy.spin()