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