class Action(object): def __init__(self): super(Action, self).__init__() self._duration = 3.0 self.execute_finished_signal = Signal() def get_duration(self): return self._duration def set_duration(self, duration): self._duration = duration #@abstractmethod def to_string(self): pass def deepcopy(self): action = self.__class__() action._duration = self._duration return action def serialize(self, stream): stream.serialize_data(self._duration) def deserialize(self, stream): self._duration = stream.deserialize_data() #@abstractmethod def execute(self): pass def _execute_finished(self): self.execute_finished_signal.emit()
class Ps3Subscriber(object): top_button = 'top' right_button = 'right' bottom_button = 'bottom' left_button = 'left' select_button = 'select' start_button = 'start' square_button = 'square' triangle_button = 'triangle' cross_button = 'cross' circle_button = 'circle' def __init__(self): self.buttons_changed = Signal() self._buttons = None self._pressed_buttons = set() rospy.Subscriber('ps3_joy', Joy, self._joy_callback) def _joy_callback(self, joy_msg): if self._buttons != joy_msg.buttons: self._buttons = joy_msg.buttons self.buttons_changed.emit() def get_triggered_buttons(self): buttons = { 4: self.top_button, 5: self.right_button, 6: self.bottom_button, 7: self.left_button, 0: self.select_button, 3: self.start_button, 15: self.square_button, 12: self.triangle_button, 14: self.cross_button, 13: self.circle_button, } triggered = set() for index, value in enumerate(self._buttons): if index in buttons.keys(): index = buttons[index] if value != 0: if index not in self._pressed_buttons: self._pressed_buttons.add(index) triggered.add(index) else: if index in self._pressed_buttons: self._pressed_buttons.remove(index) return triggered
class KontrolSubscriber(object): previous_button = 'previous' play_button = 'play' next_button = 'next' repeat_button = 'repeat' stop_button = 'stop' record_button = 'record' top1_button = 'top1' bottom1_button = 'bottom1' top2_button = 'top2' bottom2_button = 'bottom2' top6_button = 'top6' bottom6_button = 'bottom6' top7_button = 'top7' bottom7_button = 'bottom7' top8_button = 'top8' bottom8_button = 'bottom8' top9_button = 'top9' bottom9_button = 'bottom9' def __init__(self): self.axes_changed = Signal() self.buttons_changed = Signal() self._axes = None self._buttons = None self._pressed_buttons = set() self._last_larm = None self._last_rarm = None self._last_lgrip = None self._last_rgrip = None #************ rospy.Subscriber('korg_joy', Joy, self._joy_callback) #rospy.Subscriber('/joy', Joy, self._joy_callback) #************ def _joy_callback(self, joy_msg): if self._axes != joy_msg.axes: self._axes = joy_msg.axes self.axes_changed.emit() if self._buttons != joy_msg.buttons: self._buttons = joy_msg.buttons self.buttons_changed.emit() def is_valid_action_set(self): if self._buttons is None: return False if len(self._buttons) < 25: return False mode = self._buttons[24] if mode == 3: return False return True def get_action_set(self): set = ActionSet() if self._axes is None or self._buttons is None: # default action to test without hardware slider return DefaultAction() if not self.is_valid_action_set(): return None mode = self._buttons[24] head = Pr2MoveHeadAction() head_data = [-self._axes[9], -self._axes[0]] self._set_transformed_data(head, head_data) set.add_action(head) torso = Pr2MoveTorsoAction() torso_data = [self._axes[8]] self._set_transformed_data(torso, torso_data) set.add_action(torso) if mode != 3: larm_data = [] larm_data.extend(self._axes[1:7]) larm_data.append(self._axes[16]) larm_data[0] = -larm_data[0] larm_data[1] = -larm_data[1] larm_data[2] = -larm_data[2] larm_data[4] = -larm_data[4] larm_data[6] = -larm_data[6] rarm_data = [] rarm_data.extend(self._axes[1:7]) rarm_data.append(self._axes[16]) rarm_data[1] = -rarm_data[1] lgrip_data = [self._axes[7]] rgrip_data = [lgrip_data[0]] if mode == 0 or mode == 1: larm = Pr2MoveLeftArmAction() self._set_transformed_data(larm, larm_data) set.add_action(larm) self._last_larm = larm lgrip = Pr2MoveLeftGripperAction() self._set_transformed_data(lgrip, lgrip_data) set.add_action(lgrip) self._last_lgrip = lgrip elif mode == 2: if self._last_larm is not None: set.add_action(self._last_larm.deepcopy()) if self._last_lgrip is not None: set.add_action(self._last_lgrip.deepcopy()) if mode == 0 or mode == 2: rarm = Pr2MoveRightArmAction() self._set_transformed_data(rarm, rarm_data) set.add_action(rarm) self._last_rarm = rarm rgrip = Pr2MoveRightGripperAction() self._set_transformed_data(rgrip, rgrip_data) set.add_action(rgrip) self._last_rgrip = rgrip elif mode == 1: if self._last_rarm is not None: set.add_action(self._last_rarm.deepcopy()) if self._last_rgrip is not None: set.add_action(self._last_rgrip.deepcopy()) duration = self._transform_value(self._axes[17], 0.5, 5.0) set.set_duration(duration) return set def get_joint_values(self): values = {} set = self.get_action_set() for action in set._actions: is_degree = isinstance(action, Pr2JointTrajectoryAction) and not isinstance(action, Pr2MoveTorsoAction) for index, data in enumerate(action._joints): name = data['label'] value = action._values[index] if is_degree: value = value * math.pi / 180.0 values[name] = value return values def get_triggered_buttons(self): buttons = { 18: self.previous_button, 19: self.play_button, 20: self.next_button, 21: self.repeat_button, 22: self.stop_button, 23: self.record_button, 0: self.top1_button, 1: self.bottom1_button, 2: self.top2_button, 3: self.bottom2_button, 10: self.top6_button, 11: self.bottom6_button, 12: self.top7_button, 13: self.bottom7_button, 14: self.top8_button, 15: self.bottom8_button, 16: self.top9_button, 17: self.bottom9_button, } triggered = set() for index, value in enumerate(self._buttons): if index in buttons.keys(): index = buttons[index] if value == 1: if index not in self._pressed_buttons: self._pressed_buttons.add(index) triggered.add(index) else: if index in self._pressed_buttons: self._pressed_buttons.remove(index) return triggered def _set_transformed_data(self, action, data): assert(len(action._joints) == len(data)) transformed = [] for index, joint in enumerate(action._joints): min_value = joint['min'] max_value = joint['max'] value = self._transform_value(data[index], min_value, max_value) transformed.append(value) action.set_values(transformed) #print 'joints', action._joints #print 'data', data def _transform_value(self, value, min_value, max_value): assert(value >= -1 and value <= 1) value = (value + 1.0) / 2.0 value = min_value + value * (max_value - min_value) assert(value >= min_value) assert(value <= max_value) return value
class KontrolSubscriber(object): previous_button = 'previous' play_button = 'play' next_button = 'next' repeat_button = 'repeat' stop_button = 'stop' record_button = 'record' top1_button = 'top1' bottom1_button = 'bottom1' top2_button = 'top2' bottom2_button = 'bottom2' top6_button = 'top6' bottom6_button = 'bottom6' top7_button = 'top7' bottom7_button = 'bottom7' top8_button = 'top8' bottom8_button = 'bottom8' top9_button = 'top9' bottom9_button = 'bottom9' def __init__(self): self.axes_changed = Signal() self.buttons_changed = Signal() self._axes = None self._buttons = None self._pressed_buttons = set() self._last_larm = None self._last_rarm = None self._last_lgrip = None self._last_rgrip = None rospy.Subscriber('korg_joy', Joy, self._joy_callback) def _joy_callback(self, joy_msg): if self._axes != joy_msg.axes: self._axes = joy_msg.axes self.axes_changed.emit() if self._buttons != joy_msg.buttons: self._buttons = joy_msg.buttons self.buttons_changed.emit() def is_valid_action_set(self): if self._buttons is None: return False if len(self._buttons) < 25: return False mode = self._buttons[24] if mode == 3: return False return True def get_action_set(self): set = ActionSet() if self._axes is None or self._buttons is None: # default action to test without hardware slider return DefaultAction() if not self.is_valid_action_set(): return None mode = self._buttons[24] head = Pr2MoveHeadAction() head_data = [-self._axes[9], -self._axes[0]] self._set_transformed_data(head, head_data) set.add_action(head) torso = Pr2MoveTorsoAction() torso_data = [self._axes[8]] self._set_transformed_data(torso, torso_data) set.add_action(torso) if mode != 3: larm_data = [] larm_data.extend(self._axes[1:7]) larm_data.append(self._axes[16]) larm_data[0] = -larm_data[0] larm_data[1] = -larm_data[1] larm_data[2] = -larm_data[2] larm_data[4] = -larm_data[4] larm_data[6] = -larm_data[6] rarm_data = [] rarm_data.extend(self._axes[1:7]) rarm_data.append(self._axes[16]) rarm_data[1] = -rarm_data[1] lgrip_data = [self._axes[7]] rgrip_data = [lgrip_data[0]] if mode == 0 or mode == 1: larm = Pr2MoveLeftArmAction() self._set_transformed_data(larm, larm_data) set.add_action(larm) self._last_larm = larm lgrip = Pr2MoveLeftGripperAction() self._set_transformed_data(lgrip, lgrip_data) set.add_action(lgrip) self._last_lgrip = lgrip elif mode == 2: if self._last_larm is not None: set.add_action(self._last_larm.deepcopy()) if self._last_lgrip is not None: set.add_action(self._last_lgrip.deepcopy()) if mode == 0 or mode == 2: rarm = Pr2MoveRightArmAction() self._set_transformed_data(rarm, rarm_data) set.add_action(rarm) self._last_rarm = rarm rgrip = Pr2MoveRightGripperAction() self._set_transformed_data(rgrip, rgrip_data) set.add_action(rgrip) self._last_rgrip = rgrip elif mode == 1: if self._last_rarm is not None: set.add_action(self._last_rarm.deepcopy()) if self._last_rgrip is not None: set.add_action(self._last_rgrip.deepcopy()) duration = self._transform_value(self._axes[17], 0.5, 5.0) set.set_duration(duration) return set def get_joint_values(self): values = {} set = self.get_action_set() for action in set._actions: is_degree = isinstance(action, Pr2JointTrajectoryAction) and not isinstance(action, Pr2MoveTorsoAction) for index, data in enumerate(action._joints): name = data['label'] value = action._values[index] if is_degree: value = value * math.pi / 180.0 values[name] = value return values def get_triggered_buttons(self): buttons = { 18: self.previous_button, 19: self.play_button, 20: self.next_button, 21: self.repeat_button, 22: self.stop_button, 23: self.record_button, 0: self.top1_button, 1: self.bottom1_button, 2: self.top2_button, 3: self.bottom2_button, 10: self.top6_button, 11: self.bottom6_button, 12: self.top7_button, 13: self.bottom7_button, 14: self.top8_button, 15: self.bottom8_button, 16: self.top9_button, 17: self.bottom9_button, } triggered = set() for index, value in enumerate(self._buttons): if index in buttons.keys(): index = buttons[index] if value == 1: if index not in self._pressed_buttons: self._pressed_buttons.add(index) triggered.add(index) else: if index in self._pressed_buttons: self._pressed_buttons.remove(index) return triggered def _set_transformed_data(self, action, data): assert(len(action._joints) == len(data)) transformed = [] for index, joint in enumerate(action._joints): min_value = joint['min'] max_value = joint['max'] value = self._transform_value(data[index], min_value, max_value) transformed.append(value) action.set_values(transformed) #print 'joints', action._joints #print 'data', data def _transform_value(self, value, min_value, max_value): assert(value >= -1 and value <= 1) value = (value + 1.0) / 2.0 value = min_value + value * (max_value - min_value) assert(value >= min_value) assert(value <= max_value) return value