binpack_rect_array.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import numpy as np
4 
5 import cv_bridge
6 from jsk_recognition_utils.depth import split_fore_background
7 from jsk_topic_tools import ConnectionBasedTransport
8 from jsk_recognition_msgs.msg import RectArray, Rect
9 import message_filters
10 import rospy
11 from sensor_msgs.msg import Image
12 
13 class Block():
14  def __init__(self, rect):
15  self.rect = rect
16  self.used = False
17  def max_cmp(self, another):
18  return max(self.rect.width, self.rect.height) - max(another.rect.width, another.rect.height)
19  def min_cmp(self, another):
20  return min(self.rect.width, self.rect.height) - min(another.rect.width, another.rect.height)
21  def height_cmp(self, another):
22  return self.rect.height - another.rect.height
23  def width_cmp(self, another):
24  return self.rect.width - another.rect.width
25  def __cmp__(self, another): # python2
26  return self.max_cmp(another) or self.min_cmp(another) or self.height_cmp(another) or self.width_cmp(another) or 0
27 
28 class Packer():
29  def __init__(self):
30  pass
31  def fit(self, blocks):
32  if len(blocks) == 0:
33  return
34  self.root = Block(Rect(x=0, y=0, width=blocks[0].rect.width, height=blocks[0].rect.height))
35  for block in blocks:
36  node = self.find_node(self.root, block.rect.width, block.rect.height)
37  if node:
38  block.fit_location = self.split(node, block.rect.width, block.rect.height)
39  else:
40  block.fit_location = self.grow_node(block.rect.width, block.rect.height)
41  def find_node(self, root, w, h):
42  if root.used:
43  return self.find_node(root.right, w, h) or self.find_node(root.down, w, h)
44  elif w <= root.rect.width and h <= root.rect.height:
45  return root
46  else:
47  return False
48  def split(self, node, w, h):
49  node.used = True
50  node.down = Block(Rect(x=node.rect.x, y=node.rect.y + h, width=w, height=node.rect.height - h))
51  node.right = Block(Rect(x=node.rect.x + w, y=node.rect.y, width=node.rect.width - w, height=h))
52  return node
53  def grow_node(self, w, h):
54  can_grow_down = w <= self.root.rect.width
55  can_grow_right = h <= self.root.rect.height
56  should_grow_right = can_grow_right and self.root.rect.height >= self.root.rect.width + w
57  should_grow_down = can_grow_down and self.root.rect.width >= self.root.rect.height + h
58  if should_grow_right:
59  return self.grow_right(w, h)
60  elif should_grow_down:
61  return self.grow_down(w, h)
62  elif can_grow_right:
63  return self.grow_right(w, h)
64  elif can_grow_down:
65  return self.grow_down(w, h)
66  else:
67  return None
68  def grow_right(self, w, h):
69  prev_root = self.root
70  self.root = Block(Rect(x=0, y=0, width=self.root.rect.width + w, height=self.root.rect.height))
71  self.root.down = prev_root
72  self.root.right = Block(Rect(x=prev_root.rect.width, y=0, width=w, height=prev_root.rect.height))
73  self.root.used = True
74  node = self.find_node(self.root, w, h)
75  if node:
76  return self.split(node, w, h)
77  else:
78  return None
79  def grow_down(self, w, h):
80  prev_root = self.root
81  self.root = Block(Rect(x=0, y=0, width=self.root.rect.width, height=self.root.rect.height + h))
82  self.root.right = prev_root
83  self.root.down = Block(Rect(x=0, y=prev_root.rect.height, width=prev_root.rect.width, height=h))
84  self.root.used = True
85  node = self.find_node(self.root, w, h)
86  if node:
87  return self.split(node, w, h)
88  else:
89  return None
90 
91 # def hoge(msg):
92 # print "~input", msg.header.stamp.to_sec()
93 # def fuga(msg):
94 # print "~input/rect_array", msg.header.stamp.to_sec()
95 
96 class BinPack(ConnectionBasedTransport):
97  def __init__(self):
98  super(BinPack, self).__init__()
99  self.pub_ = self.advertise('~output', Image, queue_size=10)
100  def subscribe(self):
101  self.sub_ = message_filters.Subscriber('~input', Image, queue_size=1)
102  self.sub_rects_ = message_filters.Subscriber('~input/rect_array', RectArray, queue_size=1)
103  # self.sub_.registerCallback(hoge)
104  # self.sub_rects_.registerCallback(fuga)
105  if rospy.get_param('~approximate_sync', False):
106  self.sync = message_filters.ApproximateTimeSynchronizer(
107  [self.sub_, self.sub_rects_], queue_size=100, slop=.1)
108  else:
110  [self.sub_, self.sub_rects_], queue_size=100)
111  self.sync.registerCallback(self._apply)
112  def unsubscribe(self):
113  self.sub_.sub.unregister()
114  self.sub_rects_.sub.unregister()
115  def _apply(self, img_msg, rects):
116  bridge = cv_bridge.CvBridge()
117  img = bridge.imgmsg_to_cv2(img_msg)
118  root_rect, blocks = self.bin_pack(rects)
119  output_img = np.zeros((root_rect.height, root_rect.width, img.shape[2]), dtype=img.dtype)
120  for block in blocks:
121  location = block.fit_location
122  output_img[location.rect.y:location.rect.y + block.rect.height,
123  location.rect.x:location.rect.x + block.rect.width] = img[block.rect.y:block.rect.y+block.rect.height, block.rect.x:block.rect.x + block.rect.width]
124  output_img_msg = bridge.cv2_to_imgmsg(output_img, encoding=img_msg.encoding)
125  output_img_msg.header = img_msg.header
126  self.pub_.publish(output_img_msg)
127  def bin_pack(self, rects):
128  # Solve 2D BinPacking problem
129  packer = Packer()
130  blocks = [Block(rect) for rect in rects.rects]
131  # sort by some algorithms
132  #blocks.sort(key=lambda x: x.rect.width*x.rect.height, reverse=True)
133  blocks.sort(reverse=True)
134  packer.fit(blocks)
135  print((packer.root.rect.width, packer.root.rect.height))
136  return (packer.root.rect, blocks)
137  # for block in blocks:
138  # if block.fit_location:
139  # print (block.fit_location.rect.x, block.fit_location.rect.y, block.fit_location.rect.width, block.fit_location.rect.height)
140  # print " --", (block.rect.x, block.rect.y, block.rect.width, block.rect.height)
141 
142 
143 if __name__ == '__main__':
144  rospy.init_node('binpack')
145  bin_pack = BinPack()
146  rospy.spin()


jsk_perception
Author(s): Manabu Saito, Ryohei Ueda
autogenerated on Mon May 3 2021 03:03:27