def add_cb(self, data):
if self.args['test'] in ('angle', 'distance'):
value = self.conv_func(get_attr(data, self.field))
self.args['value'] = value
if self.cur is None and (within(value, self.first, self.pm) or \
self.args['test'] == 'angle'):
self.cur = self.first
self.args['label'] = str(self.cur)
self.take_picture(data)
elif self.cur is not None:
if within(value, self.cur + self.dx, self.pm) and \
value > self.last:
self.args['label'] = str(self.last)
self.take_picture(data)
self._remove_subscriptions()
rospy.signal_shutdown('')
elif within(value, self.cur + self.dx, self.pm):
self.cur = self.cur + self.dx
self.args['label'] = str(self.cur)
self.take_picture(data)
else:
self.take_picture(data)
self._remove_subscriptions()
cv.WaitKey(0)
rospy.signal_shutdown('')
def _convert_user_ang(self, user_ang):
"""Convert fields of UserAng message into dictionary."""
if not self._angle_cache:
self._angle_cache = get_joint_angles_from_msg(user_ang)
d = {}
for angle in self._angle_cache:
d[angle] = get_attr(user_ang, angle).angle
return d
