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 _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)