bounding_box_array_publisher.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import sys
4 
5 from geometry_msgs.msg import Point
6 from geometry_msgs.msg import Quaternion
7 from geometry_msgs.msg import Vector3
8 from jsk_recognition_msgs.msg import BoundingBox
9 from jsk_recognition_msgs.msg import BoundingBoxArray
10 import rospy
11 from tf.transformations import quaternion_from_euler
12 
13 
15 
16  def __init__(self):
17  self.seq = 0
18 
19  self.frame_id = rospy.get_param('~frame_id')
20  if (rospy.has_param('~positions') or
21  rospy.has_param('~rotations') or
22  rospy.has_param('~dimensions')):
23  # Deprecated bounding box pose/dimension specification
24  rospy.logwarn("DEPRECATION WARNING: Rosparam '~positions', "
25  "'~rotations' and '~dimensions' are being "
26  "deprecated. Please use '~boxes' instead.")
27  positions = rospy.get_param('~positions')
28  rotations = rospy.get_param('~rotations')
29  dimensions = rospy.get_param('~dimensions')
30  if len(rotations) != len(positions):
31  rospy.logfatal('Number of ~rotations is expected as {}, but {}'
32  .format(len(positions), len(rotations)))
33  sys.exit(1)
34  if len(dimensions) != len(positions):
35  rospy.logfatal('Number of ~dimensions is expected as {}, but {}'
36  .format(len(positions), len(dimensions)))
37  sys.exit(1)
38  self.boxes = []
39  for pos, rot, dim in zip(positions, rotations, dimensions):
40  self.boxes.append({
41  'position': pos,
42  'rotation': rot,
43  'dimension': dim,
44  })
45  else:
46  self.boxes = rospy.get_param('~boxes')
47  self.pub = rospy.Publisher('~output', BoundingBoxArray, queue_size=1)
48 
49  rate = rospy.get_param('~rate', 1)
50  self.timer = rospy.Timer(rospy.Duration(1. / rate), self.publish)
51 
52  def publish(self, event):
53  bbox_array_msg = BoundingBoxArray()
54  bbox_array_msg.header.seq = self.seq
55  bbox_array_msg.header.frame_id = self.frame_id
56  bbox_array_msg.header.stamp = event.current_real
57  for box in self.boxes:
58  pos = box['position']
59  rot = box.get('rotation', [0, 0, 0])
60  qua = quaternion_from_euler(*rot)
61  dim = box['dimension']
62  label = box.get('label', 0)
63 
64  bbox_msg = BoundingBox()
65  bbox_msg.header.seq = self.seq
66  bbox_msg.header.frame_id = self.frame_id
67  bbox_msg.header.stamp = event.current_real
68  bbox_msg.pose.position = Point(*pos)
69  bbox_msg.pose.orientation = Quaternion(*qua)
70  bbox_msg.dimensions = Vector3(*dim)
71  bbox_msg.label = label
72 
73  bbox_array_msg.boxes.append(bbox_msg)
74 
75  self.pub.publish(bbox_array_msg)
76 
77 
78 if __name__ == '__main__':
79  rospy.init_node('bounding_box_array_publisher')
81  rospy.spin()


jsk_recognition_utils
Author(s):
autogenerated on Wed May 27 2020 03:59:48