def _timer_cb(self, event):
if self.json_dir is None:
rospy.logwarn_throttle(10, 'Input json_dir is not set.')
return
if not osp.isdir(self.json_dir):
rospy.logfatal_throttle(
10, 'Input json_dir is not directory: %s' % self.json_dir)
return
filename = osp.join(self.json_dir, 'item_location_file.json')
if osp.exists(filename):
with open(filename) as location_f:
data = json.load(location_f)
bin_contents = {}
for bin_ in data['bins']:
bin_contents[bin_['bin_id']] = bin_['contents']
tote_contents = data['tote']['contents']
if self.target_location[:3] == 'bin':
contents = bin_contents[self.target_location[4]]
elif self.target_location == 'tote':
contents = tote_contents
else:
return
candidates_fixed = [
l for l in self.label_names if l.startswith('__')
]
candidates = candidates_fixed + contents
label_list = [self.label_names.index(x) for x in candidates]
label_list = sorted(label_list)
labels = []
for label in label_list:
label_msg = Label()
label_msg.id = label
label_msg.name = self.label_names[label]
labels.append(label_msg)
msg = LabelArray()
msg.labels = labels
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
def _timer_cb(self, event):
if self.json_dir is None:
rospy.logwarn_throttle(10, 'Input json_dir is not set.')
return
if not osp.isdir(self.json_dir):
rospy.logfatal_throttle(
10, 'Input json_dir is not directory: %s' % self.json_dir)
return
filename = osp.join(self.json_dir, 'item_location_file.json')
if osp.exists(filename):
with open(filename) as location_f:
data = json.load(location_f)
bin_contents = {}
for bin_ in data['bins']:
bin_contents[bin_['bin_id']] = bin_['contents']
tote_contents = data['tote']['contents']
if self.target_location[:3] == 'bin':
contents = bin_contents[self.target_location[4]]
elif self.target_location == 'tote':
contents = tote_contents
else:
return
candidates_fixed = [l for l in self.label_names
if l.startswith('__')]
candidates = candidates_fixed + contents
label_list = [self.label_names.index(x) for x in candidates]
label_list = sorted(label_list)
labels = []
for label in label_list:
label_msg = Label()
label_msg.id = label
label_msg.name = self.label_names[label]
labels.append(label_msg)
msg = LabelArray()
msg.labels = labels
msg.header.stamp = rospy.Time.now()
self.pub.publish(msg)
def callback(self, imgmsg):
raw_image = None
self.image_width = imgmsg.width
self.image_height = imgmsg.height
try:
raw_image = self.bridge.imgmsg_to_cv2(imgmsg,
desired_encoding='bgr8')
except:
rospy.logerr('failed transform image')
return
dataset = ImageDataset(pkg_dir,
raw_image,
self.templates_dir,
thresh=self.thresh,
image_name=str(imgmsg.header.stamp))
scores, w_array, h_array, label_list = calculate_scores(
self.model, dataset)
boxes, indices = nms(scores, w_array, h_array, dataset.thresh,
label_list)
output_image = plot_results(dataset.image_raw,
boxes,
label_list,
indices,
show=False,
save_name=None)
labels_msg = LabelArray()
rects_msg = RectArray()
original_size_rects_msg = RectArray()
for i in range(len(indices)):
rect_msg = Rect()
label_msg = Label()
original_size_rect_msg = Rect()
box = boxes[i][None, :, :][0]
x, y, width, height = self.check_out_of_image(box)
rect_msg.x = x
rect_msg.y = y
rect_msg.width = width
rect_msg.height = height
original_size_rect_msg.x = x * 1 / self.resize_scale
original_size_rect_msg.y = y * 1 / self.resize_scale
original_size_rect_msg.width = width * 1 / self.resize_scale
original_size_rect_msg.height = height * 1 / self.resize_scale
# rect_msg.x = box[0][0]
# rect_msg.y = box[0][1]
# rect_msg.width = box[1][0] - box[0][0]
# rect_msg.height = box[1][1] - box[0][1]
label_msg.name = label_list[indices[i]]
rects_msg.rects.append(rect_msg)
labels_msg.labels.append(label_msg)
original_size_rects_msg.rects.append(original_size_rect_msg)
rects_msg.header = imgmsg.header
labels_msg.header = imgmsg.header
original_size_rects_msg.header = imgmsg.header
self.labels_pub.publish(labels_msg)
self.rects_pub.publish(rects_msg)
self.original_size_rects_pub.publish(original_size_rects_msg)
msg_viz = cv_bridge.CvBridge().cv2_to_imgmsg(output_image,
encoding='bgr8')
msg_viz.header = imgmsg.header
self.pub_viz.publish(msg_viz)
def _scale_cb(self, *weight_msgs):
assert len(weight_msgs) == len(self.prev_weight_values)
# Publish debug info
weight_values = [w.weight.value for w in weight_msgs]
weight_sum = sum(weight_values)
sum_msg = WeightStamped()
sum_msg.header = weight_msgs[0].header
sum_msg.weight.value = weight_sum
sum_msg.weight.stable = all(msg.weight.stable for msg in weight_msgs)
sum_at_reset_msg = WeightStamped()
sum_at_reset_msg.header = weight_msgs[0].header
sum_at_reset_msg.weight.value = self.weight_sum_at_reset
sum_at_reset_msg.weight.stable = True
self.weight_sum_at_reset_pub.publish(sum_at_reset_msg)
self.weight_sum_pub.publish(sum_msg)
if not sum_msg.weight.stable:
return # unstable
# Store stable weight and enable resetting
self.prev_weight_values = weight_values
if not self.can_reset:
self.reset_srv = rospy.Service('~reset', Trigger, self._reset)
self.can_reset = True
if not self.candidates:
rospy.logwarn_throttle(10, 'No candidates, so skip refining')
return
candidates = self.candidates
# Judge if scale value is changed
weight_diff = weight_sum - self.weight_sum_at_reset
diff_lower = weight_diff - self.error
diff_upper = weight_diff + self.error
weight_min = min(
self.object_weights.get(x, float('inf'))
for x in candidates.keys())
changed_msg = BoolStamped()
changed_msg.header = weight_msgs[0].header
if -weight_min < diff_lower and diff_upper < weight_min \
and diff_lower < 0 and 0 < diff_upper:
changed_msg.data = False
else:
changed_msg.data = True
self.changed_pub.publish(changed_msg)
# Output candidates
pick_msg = LabelArray()
place_msg = LabelArray()
pick_msg.header = weight_msgs[0].header
place_msg.header = weight_msgs[0].header
for obj_name, w in self.object_weights.items():
if obj_name not in candidates:
continue
obj_id = candidates[obj_name]
if diff_upper > w and w > diff_lower:
label = Label()
label.id = obj_id
label.name = obj_name
place_msg.labels.append(label)
elif diff_upper > -w and -w > diff_lower:
label = Label()
label.id = obj_id
label.name = obj_name
pick_msg.labels.append(label)
self.picked_pub.publish(pick_msg)
self.placed_pub.publish(place_msg)
def _scale_cb(self, *weight_msgs):
assert len(weight_msgs) == len(self.prev_weight_values)
# Publish debug info
weight_values = [w.weight.value for w in weight_msgs]
weight_sum = sum(weight_values)
sum_msg = WeightStamped()
sum_msg.header = weight_msgs[0].header
sum_msg.weight.value = weight_sum
sum_msg.weight.stable = all(msg.weight.stable for msg in weight_msgs)
sum_at_reset_msg = WeightStamped()
sum_at_reset_msg.header = weight_msgs[0].header
sum_at_reset_msg.weight.value = self.weight_sum_at_reset
sum_at_reset_msg.weight.stable = True
self.weight_sum_at_reset_pub.publish(sum_at_reset_msg)
self.weight_sum_pub.publish(sum_msg)
if not sum_msg.weight.stable:
return # unstable
# Store stable weight and enable resetting
self.prev_weight_values = weight_values
if not self.can_reset:
self.reset_srv = rospy.Service('~reset', Trigger, self._reset)
self.can_reset = True
if not self.candidates:
rospy.logwarn_throttle(10, 'No candidates, so skip refining')
return
candidates = self.candidates
# Judge if scale value is changed
weight_diff = weight_sum - self.weight_sum_at_reset
diff_lower = weight_diff - self.error
diff_upper = weight_diff + self.error
weight_min = min(self.object_weights.get(x, float('inf'))
for x in candidates.keys())
changed_msg = BoolStamped()
changed_msg.header = weight_msgs[0].header
if -weight_min < diff_lower and diff_upper < weight_min \
and diff_lower < 0 and 0 < diff_upper:
changed_msg.data = False
else:
changed_msg.data = True
self.changed_pub.publish(changed_msg)
# Output candidates
pick_msg = LabelArray()
place_msg = LabelArray()
pick_msg.header = weight_msgs[0].header
place_msg.header = weight_msgs[0].header
for obj_name, w in self.object_weights.items():
if obj_name not in candidates:
continue
obj_id = candidates[obj_name]
if diff_upper > w and w > diff_lower:
label = Label()
label.id = obj_id
label.name = obj_name
place_msg.labels.append(label)
elif diff_upper > -w and -w > diff_lower:
label = Label()
label.id = obj_id
label.name = obj_name
pick_msg.labels.append(label)
self.picked_pub.publish(pick_msg)
self.placed_pub.publish(place_msg)