KontrolSubscriber.py
Go to the documentation of this file.
00001 import math
00002 
00003 from actions.ActionSet import ActionSet
00004 from actions.DefaultAction import DefaultAction
00005 from actions.Pr2GripperAction import Pr2GripperAction
00006 from actions.Pr2JointTrajectoryAction import Pr2JointTrajectoryAction
00007 from actions.Pr2MoveHeadAction import Pr2MoveHeadAction
00008 from actions.Pr2MoveLeftArmAction import Pr2MoveLeftArmAction
00009 from actions.Pr2MoveLeftGripperAction import Pr2MoveLeftGripperAction
00010 from actions.Pr2MoveRightArmAction import Pr2MoveRightArmAction
00011 from actions.Pr2MoveRightGripperAction import Pr2MoveRightGripperAction
00012 from actions.Pr2MoveTorsoAction import Pr2MoveTorsoAction
00013 import rospy
00014 from sensor_msgs.msg import Joy
00015 from Signal import Signal
00016 
00017 class KontrolSubscriber(object):
00018 
00019     previous_button = 'previous'
00020     play_button = 'play'
00021     next_button = 'next'
00022     repeat_button = 'repeat'
00023     stop_button = 'stop'
00024     record_button = 'record'
00025 
00026     top1_button = 'top1'
00027     bottom1_button = 'bottom1'
00028     top2_button = 'top2'
00029     bottom2_button = 'bottom2'
00030 
00031     top6_button = 'top6'
00032     bottom6_button = 'bottom6'
00033     top7_button = 'top7'
00034     bottom7_button = 'bottom7'
00035 
00036     top8_button = 'top8'
00037     bottom8_button = 'bottom8'
00038     top9_button = 'top9'
00039     bottom9_button = 'bottom9'
00040 
00041     def __init__(self):
00042         self.axes_changed = Signal()
00043         self.buttons_changed = Signal()
00044         self._axes = None
00045         self._buttons = None
00046         self._pressed_buttons = set()
00047         self._last_larm = None
00048         self._last_rarm = None
00049         self._last_lgrip = None
00050         self._last_rgrip = None
00051         rospy.Subscriber('korg_joy', Joy, self._joy_callback)
00052 
00053     def _joy_callback(self, joy_msg):
00054         if self._axes != joy_msg.axes:
00055             self._axes = joy_msg.axes
00056             self.axes_changed.emit()
00057         if self._buttons != joy_msg.buttons:
00058             self._buttons = joy_msg.buttons
00059             self.buttons_changed.emit()
00060 
00061     def is_valid_action_set(self):
00062         if self._buttons is None:
00063             return False
00064         if len(self._buttons) < 25:
00065             return False
00066         mode = self._buttons[24]
00067         if mode == 3:
00068             return False
00069         return True
00070 
00071     def get_action_set(self):
00072         set = ActionSet()
00073         if self._axes is None or self._buttons is None:
00074             # default action to test without hardware slider
00075             return DefaultAction()
00076 
00077         if not self.is_valid_action_set():
00078             return None
00079 
00080         mode = self._buttons[24]
00081 
00082         head = Pr2MoveHeadAction()
00083         head_data = [-self._axes[9], -self._axes[0]]
00084         self._set_transformed_data(head, head_data)
00085         set.add_action(head)
00086 
00087         torso = Pr2MoveTorsoAction()
00088         torso_data = [self._axes[8]]
00089         self._set_transformed_data(torso, torso_data)
00090         set.add_action(torso)
00091 
00092         if mode != 3:
00093             larm_data = []
00094             larm_data.extend(self._axes[1:7])
00095             larm_data.append(self._axes[16])
00096             larm_data[0] = -larm_data[0]
00097             larm_data[1] = -larm_data[1]
00098             larm_data[2] = -larm_data[2]
00099             larm_data[4] = -larm_data[4]
00100             larm_data[6] = -larm_data[6]
00101 
00102             rarm_data = []
00103             rarm_data.extend(self._axes[1:7])
00104             rarm_data.append(self._axes[16])
00105             rarm_data[1] = -rarm_data[1]
00106 
00107             lgrip_data = [self._axes[7]]
00108             rgrip_data = [lgrip_data[0]]
00109 
00110         if mode == 0 or mode == 1:
00111             larm = Pr2MoveLeftArmAction()
00112             self._set_transformed_data(larm, larm_data)
00113             set.add_action(larm)
00114             self._last_larm = larm
00115 
00116             lgrip = Pr2MoveLeftGripperAction()
00117             self._set_transformed_data(lgrip, lgrip_data)
00118             set.add_action(lgrip)
00119             self._last_lgrip = lgrip
00120         elif mode == 2:
00121             if self._last_larm is not None:
00122                 set.add_action(self._last_larm.deepcopy())
00123             if self._last_lgrip is not None:
00124                 set.add_action(self._last_lgrip.deepcopy())
00125 
00126         if mode == 0 or mode == 2:
00127             rarm = Pr2MoveRightArmAction()
00128             self._set_transformed_data(rarm, rarm_data)
00129             set.add_action(rarm)
00130             self._last_rarm = rarm
00131 
00132             rgrip = Pr2MoveRightGripperAction()
00133             self._set_transformed_data(rgrip, rgrip_data)
00134             set.add_action(rgrip)
00135             self._last_rgrip = rgrip
00136         elif mode == 1:
00137             if self._last_rarm is not None:
00138                 set.add_action(self._last_rarm.deepcopy())
00139             if self._last_rgrip is not None:
00140                 set.add_action(self._last_rgrip.deepcopy())
00141 
00142         duration = self._transform_value(self._axes[17], 0.5, 5.0)
00143         set.set_duration(duration)
00144 
00145         return set
00146 
00147     def get_joint_values(self):
00148         values = {}
00149         set = self.get_action_set()
00150         for action in set._actions:
00151             is_degree = isinstance(action, Pr2JointTrajectoryAction) and not isinstance(action, Pr2MoveTorsoAction)
00152             for index, data in enumerate(action._joints):
00153                 name = data['label']
00154                 value = action._values[index]
00155                 if is_degree:
00156                     value = value * math.pi / 180.0
00157                 values[name] = value
00158         return values
00159 
00160     def get_triggered_buttons(self):
00161         buttons = {
00162             18: self.previous_button,
00163             19: self.play_button,
00164             20: self.next_button,
00165             21: self.repeat_button,
00166             22: self.stop_button,
00167             23: self.record_button,
00168             0: self.top1_button,
00169             1: self.bottom1_button,
00170             2: self.top2_button,
00171             3: self.bottom2_button,
00172             10: self.top6_button,
00173             11: self.bottom6_button,
00174             12: self.top7_button,
00175             13: self.bottom7_button,
00176             14: self.top8_button,
00177             15: self.bottom8_button,
00178             16: self.top9_button,
00179             17: self.bottom9_button,
00180         }
00181         triggered = set()
00182         for index, value in enumerate(self._buttons):
00183             if index in buttons.keys():
00184                 index = buttons[index]
00185             if value == 1:
00186                 if index not in self._pressed_buttons:
00187                     self._pressed_buttons.add(index)
00188                     triggered.add(index)
00189             else:
00190                 if index in self._pressed_buttons:
00191                     self._pressed_buttons.remove(index)
00192         return triggered
00193 
00194     def _set_transformed_data(self, action, data):
00195         assert(len(action._joints) == len(data))
00196         transformed = []
00197         for index, joint in enumerate(action._joints):
00198             min_value = joint['min']
00199             max_value = joint['max']
00200             value = self._transform_value(data[index], min_value, max_value)
00201             transformed.append(value)
00202         action.set_values(transformed)
00203         #print 'joints', action._joints
00204         #print 'data', data
00205 
00206     def _transform_value(self, value, min_value, max_value):
00207         assert(value >= -1 and value <= 1)
00208         value = (value + 1.0) / 2.0
00209         value = min_value + value * (max_value - min_value)
00210         assert(value >= min_value)
00211         assert(value <= max_value)
00212         return value


slider_gui
Author(s): Dirk Thomas
autogenerated on Wed Aug 26 2015 15:37:50