def callback(self, img_msg, rects_msg, class_msg):
bridge = self.bridge
tracker = self.tracker
frame = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
scores = []
rects = []
for i, r in enumerate(rects_msg.rects):
if self.target_labels is not None and \
class_msg.label_names[i] not in self.target_labels:
continue
rects.append((int(r.x), int(r.y), int(r.width), int(r.height)))
scores.append(class_msg.label_proba[i])
rects = np.array(rects, 'f')
scores = np.array(scores, 'f')
if len(rects) > 0:
tracker.track(frame, rects, scores)
if self.pub_labels.get_num_connections() > 0:
msg_labels = LabelArray(header=img_msg.header)
for object_id, target_object in tracker.tracking_objects.items():
if target_object['out_of_frame']:
continue
msg_labels.labels.append(Label(id=object_id))
self.pub_labels.publish(msg_labels)
if self.image_pub.get_num_connections() > 0:
frame = tracker.visualize(frame, rects)
msg = bridge.cv2_to_imgmsg(frame, "bgr8")
msg.header = img_msg.header
self.image_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 _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):
bridge = cv_bridge.CvBridge()
img = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='rgb8')
img_chw = img.transpose(2, 0, 1) # C, H, W
bboxes, masks, labels, scores = self.model.predict([img_chw])
bboxes = bboxes[0]
masks = masks[0]
labels = labels[0]
scores = scores[0]
msg_indices = ClusterPointIndices(header=imgmsg.header)
msg_labels = LabelArray(header=imgmsg.header)
# -1: label for background
lbl_cls = -np.ones(img.shape[:2], dtype=np.int32)
lbl_ins = -np.ones(img.shape[:2], dtype=np.int32)
for ins_id, (mask, label) in enumerate(zip(masks, labels)):
indices = np.where(mask.flatten())[0]
indices_msg = PointIndices(header=imgmsg.header, indices=indices)
msg_indices.cluster_indices.append(indices_msg)
class_name = self.fg_class_names[label]
msg_labels.labels.append(Label(id=label, name=class_name))
lbl_cls[mask] = label
lbl_ins[mask] = ins_id # instance_id
self.pub_indices.publish(msg_indices)
self.pub_labels.publish(msg_labels)
msg_lbl_cls = bridge.cv2_to_imgmsg(lbl_cls)
msg_lbl_ins = bridge.cv2_to_imgmsg(lbl_ins)
msg_lbl_cls.header = msg_lbl_ins.header = imgmsg.header
self.pub_lbl_cls.publish(msg_lbl_cls)
self.pub_lbl_ins.publish(msg_lbl_ins)
if self.pub_viz.get_num_connections() > 0:
n_fg_class = len(self.fg_class_names)
captions = [
'{:d}: {:s}'.format(l, self.fg_class_names[l]) for l in labels
]
viz = chainer_mask_rcnn.utils.draw_instance_bboxes(
img,
bboxes,
labels + 1,
n_class=n_fg_class + 1,
masks=masks,
captions=captions)
msg_viz = bridge.cv2_to_imgmsg(viz, encoding='rgb8')
msg_viz.header = imgmsg.header
self.pub_viz.publish(msg_viz)
def image_cb(self, msg):
if self.profiling:
rospy.loginfo(
"callback start: incomming msg is %s msec behind" %
((rospy.Time.now() - msg.header.stamp).to_sec() * 1000.0))
tprev = time.time()
try:
# transform image to RGB, float, CHW
img = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8")
img = np.asarray(img, dtype=np.float32)
img = img.transpose(2, 0, 1) # (H, W, C) -> (C, H, W)
except Exception as e:
rospy.logerr("Failed to convert image: %s" % str(e))
return
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("convert",
(tcur - tprev) * 1000))
tprev = tcur
if self.gpu >= 0:
chainer.cuda.get_device_from_id(self.gpu).use()
bboxes, labels, scores = self.model.predict([img])
bboxes, labels, scores = bboxes[0], labels[0], scores[0]
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("predict",
(tcur - tprev) * 1000))
tprev = tcur
labels_msg = LabelArray(header=msg.header)
for l in labels:
l_name = self.label_names[l]
labels_msg.labels.append(Label(id=l, name=l_name))
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make labels msg",
(tcur - tprev) * 1000))
cluster_indices_msg = ClusterPointIndices(header=msg.header)
H = img.shape[1]
W = img.shape[2]
for bbox in bboxes:
ymin = max(0, int(np.floor(bbox[0])))
xmin = max(0, int(np.floor(bbox[1])))
ymax = min(H, int(np.ceil(bbox[2])))
xmax = min(W, int(np.ceil(bbox[3])))
indices = [
range(W * y + xmin, W * y + xmax) for y in range(ymin, ymax)
]
indices = np.array(indices, dtype=np.int32).flatten()
indices_msg = PointIndices(header=msg.header, indices=indices)
cluster_indices_msg.cluster_indices.append(indices_msg)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make cluster_indices msg",
(tcur - tprev) * 1000))
tprev = tcur
rect_msg = RectArray(header=msg.header)
for bbox in bboxes:
rect = Rect(x=bbox[1],
y=bbox[0],
width=bbox[3] - bbox[1],
height=bbox[2] - bbox[0])
rect_msg.rects.append(rect)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make rect msg",
(tcur - tprev) * 1000))
tprev = tcur
cls_msg = ClassificationResult(
header=msg.header,
classifier=self.classifier_name,
target_names=self.label_names,
labels=labels,
label_names=[self.label_names[l] for l in labels],
label_proba=scores,
)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("make cls msg",
(tcur - tprev) * 1000))
tprev = tcur
self.pub_labels.publish(labels_msg)
self.pub_indices.publish(cluster_indices_msg)
self.pub_rects.publish(rect_msg)
self.pub_class.publish(cls_msg)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("publish msg",
(tcur - tprev) * 1000))
tprev = tcur
if self.visualize:
self.publish_bbox_image(img, bboxes, labels, scores, msg.header)
if self.profiling:
tcur = time.time()
rospy.loginfo("%s: elapsed %f msec" % ("callback end",
(tcur - tprev) * 1000))
tprev = tcur
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 callback(self, imgmsg):
bridge = cv_bridge.CvBridge()
img = bridge.imgmsg_to_cv2(imgmsg, desired_encoding='rgb8')
img_chw = img.transpose(2, 0, 1) # C, H, W
if self.gpu >= 0:
chainer.cuda.get_device_from_id(self.gpu).use()
bboxes, masks, labels, scores = self.model.predict([img_chw])
bboxes = bboxes[0]
masks = masks[0]
labels = labels[0]
scores = scores[0]
msg_indices = ClusterPointIndices(header=imgmsg.header)
msg_labels = LabelArray(header=imgmsg.header)
# -1: label for background
lbl_cls = -np.ones(img.shape[:2], dtype=np.int32)
lbl_ins = -np.ones(img.shape[:2], dtype=np.int32)
for ins_id, (mask, label) in enumerate(zip(masks, labels)):
indices = np.where(mask.flatten())[0]
indices_msg = PointIndices(header=imgmsg.header, indices=indices)
msg_indices.cluster_indices.append(indices_msg)
class_name = self.fg_class_names[label]
msg_labels.labels.append(Label(id=label, name=class_name))
lbl_cls[mask] = label
lbl_ins[mask] = ins_id # instance_id
self.pub_indices.publish(msg_indices)
self.pub_labels.publish(msg_labels)
msg_lbl_cls = bridge.cv2_to_imgmsg(lbl_cls)
msg_lbl_ins = bridge.cv2_to_imgmsg(lbl_ins)
msg_lbl_cls.header = msg_lbl_ins.header = imgmsg.header
self.pub_lbl_cls.publish(msg_lbl_cls)
self.pub_lbl_ins.publish(msg_lbl_ins)
cls_msg = ClassificationResult(
header=imgmsg.header,
classifier=self.classifier_name,
target_names=self.fg_class_names,
labels=labels,
label_names=[self.fg_class_names[l] for l in labels],
label_proba=scores,
)
rects_msg = RectArray(header=imgmsg.header)
for bbox in bboxes:
rect = Rect(x=bbox[1],
y=bbox[0],
width=bbox[3] - bbox[1],
height=bbox[2] - bbox[0])
rects_msg.rects.append(rect)
self.pub_rects.publish(rects_msg)
self.pub_class.publish(cls_msg)
if self.pub_viz.get_num_connections() > 0:
n_fg_class = len(self.fg_class_names)
captions = [
'{:d}: {:s}'.format(l, self.fg_class_names[l]) for l in labels
]
viz = chainer_mask_rcnn.utils.draw_instance_bboxes(
img,
bboxes,
labels + 1,
n_class=n_fg_class + 1,
masks=masks,
captions=captions)
msg_viz = bridge.cv2_to_imgmsg(viz, encoding='rgb8')
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)