class BrrUi(object): def __init__(self, share_path, conf_path, commands): self.share_path = share_path self.conf_path = conf_path self.windows = dict() self._btn_context = dict() self._functions = demo_functions self._load_config() self.img = Image.new('RGB', (1024, 600), 'white') self.active_window = self.windows['demo_1'] self.xdisp = rospy.Publisher('/robot/xdisplay', ImageMsg, latch=True, queue_size=1) self._status = RobotEnable() self._commands = commands self._font = ImageFont.truetype( '%s/HelveticaLight.ttf' % share_path, 30 ) self._textHeight = self._font.getsize('W')[1] self._active_example = False self._navigators = {'left': Navigator('left'), 'torso_left': Navigator('torso_left'), 'right': Navigator('right'), 'torso_right': Navigator('torso_right')} self._listeners_connected = False self._connect_listeners() self._estop_state = False self._estop_sub = rospy.Subscriber('robot/state', AssemblyState, self._estop_callback) self._wheel_ok = True self.cameras = dict() camera_list = ['left_hand', 'right_hand', 'head'] for idx, cam in enumerate(camera_list): try: self.cameras[cam] = CameraController('%s_camera' % cam) except AttributeError: try: # This camera might not be powered # Turn off the power to the last camera # this will turn power on to the current camera CameraController('%s_camera' % camera_list[idx-1]).close() # And try again to locate the camera service self.cameras[cam] = CameraController('%s_camera' % cam) except AttributeError: # This camera is unavailable (might be broken) # Disable camera button in the UI self.windows['cam_submenu'].set_btn_selectable('cam_%s' % cam, False) sel = self.windows['cam_submenu'].selected_btn() bad_cam = self.windows['cam_submenu'].get_btn('cam_%s' % cam) if (sel == bad_cam and not self.windows['cam_submenu'].scroll(1)): self.windows['cam_submenu'].selected_btn_index = 0 self.cam_sub = None self._l_grip = {'interface': Gripper('left'), 'type': 'custom'} self._r_grip = {'interface': Gripper('right'), 'type': 'custom'} rospy.Timer(rospy.Duration(.5), self._check_enable) self.error_state = False self._enable() self.calib_stage = 0 self.draw() mk_process('rosrun baxter_tools tuck_arms.py -u') def _estop_callback(self, msg): if self._estop_state != msg.stopped: self._estop_state = msg.stopped if msg.stopped and self._listeners_connected: self._disconnect_listeners() elif not msg.stopped and not self._listeners_connected: self._connect_listeners() def _connect_listeners(self): # Navigator OK Button self._navigators['left'].button0_changed.connect(self._left_ok_pressed) self._navigators['torso_left'].button0_changed.connect( self._left_ok_pressed) self._navigators['right'].button0_changed.connect( self._right_ok_pressed) self._navigators['torso_right'].button0_changed.connect( self._right_ok_pressed) # Navigator Wheel self._navigators['left'].wheel_changed.connect(self._left_wheel_moved) self._navigators['torso_left'].wheel_changed.connect( self._left_wheel_moved) self._navigators['right'].wheel_changed.connect( self._right_wheel_moved) self._navigators['torso_right'].wheel_changed.connect( self._right_wheel_moved) # Navigator Baxter Button self._navigators['left'].button2_changed.connect(self._enable) self._navigators['torso_left'].button2_changed.connect(self._enable) self._navigators['right'].button2_changed.connect(self._enable) self._navigators['torso_right'].button2_changed.connect(self._enable) # Navigator Back Button self._navigators['left'].button1_changed.connect(self.back) self._navigators['torso_left'].button1_changed.connect(self.back) self._navigators['right'].button1_changed.connect(self.back) self._navigators['torso_right'].button1_changed.connect(self.back) self._listeners_connected = True def _disconnect_listeners(self): # Navigator OK Button self._navigators['left'].button0_changed.disconnect( self._left_ok_pressed) self._navigators['torso_left'].button0_changed.disconnect( self._left_ok_pressed) self._navigators['right'].button0_changed.disconnect( self._right_ok_pressed) self._navigators['torso_right'].button0_changed.disconnect( self._right_ok_pressed) # Navigator Wheel self._navigators['left'].wheel_changed.disconnect( self._left_wheel_moved) self._navigators['torso_left'].wheel_changed.disconnect( self._left_wheel_moved) self._navigators['right'].wheel_changed.disconnect( self._right_wheel_moved) self._navigators['torso_right'].wheel_changed.disconnect( self._right_wheel_moved) # Navigator Baxter Button self._navigators['left'].button2_changed.disconnect(self._enable) self._navigators['torso_left'].button2_changed.disconnect(self._enable) self._navigators['right'].button2_changed.disconnect(self._enable) self._navigators['torso_right'].button2_changed.disconnect(self._enable) # Navigator Back Button self._navigators['left'].button1_changed.disconnect(self.back) self._navigators['torso_left'].button1_changed.disconnect(self.back) self._navigators['right'].button1_changed.disconnect(self.back) self._navigators['torso_right'].button1_changed.disconnect(self.back) self._listeners_connected = False def _load_config(self): f = open(self.conf_path).read() conf_data = json.loads(f) for window in conf_data['Windows']: buttons = dict() if window['back']: name = '%s_back' % window['name'] size = window['back']['size'] offset = window['back']['offset'] icon_prefix = 'Inner_Back' icon_offset = window['back']['icon_offset'] buttons[name] = BrrButton(name, size, offset, 0, icon_prefix, 'TopSmall', icon_offset, '', 0, True, self.share_path) self._btn_context[name] = {'nextWindow': window['parent'], 'function': 'Back'} if 'Buttons' in window.keys(): for btn in window['Buttons']: buttons[btn['name']] = BrrButton( btn['name'], btn['size'], btn['offset'], btn['index'], btn['icon_prefix'], btn['button'], btn['icon_offset'], btn['label'], btn['label_y'], btn['selectable'], self.share_path ) self._btn_context[btn['name']] = { 'nextWindow': btn['nextWindow'], 'function': btn['function'] } self.windows[window['name']] = BrrWindow(window, buttons, self.share_path) errors = conf_data['Error'] for error in errors['errors']: name = error['name'] buttons = dict() buttons['OK'] = BrrButton( '%s_OK' % name, # name [200, 60], # size errors['OK']['offset'], # button offset 0, # index None, # icon prefix "Wide", # button type [0, 0], # icon offset "OK", # label 16, # label y-offset True, # selectable? self.share_path ) self._btn_context["%s_OK" % name] = { 'nextWindow': None, 'function': 'Back' } window = { 'name': '%s_error' % name, 'bg': errors['bg'], 'back': False, 'offset': errors['offset'], 'parent': False, 'default': '%s_OK' % name, 'no_scroll': False, 'text': [{'text': error['text'], 'text_y': error['text_y']}], } self.windows['%s_error' % name] = BrrWindow(window, buttons, self.share_path) for win in conf_data['Confirmation']['Windows']: conf = conf_data['Confirmation'] name = win['name'] labels = list() for text in win['messages']: labels.append({'text': text['text'], 'text_y': text['text_y']}) buttons = dict() buttons['OK'] = BrrButton( '%s_OK' % name, # name [200, 60], # size conf['OK']['offset'], # button offset 1, # index None, # icon prefix "Wide", # button type [0, 0], # icon offset win['conf_text'], # label 16, # label y-offset True, # selectable? self.share_path) self._btn_context['%s_OK' % name] = { 'nextWindow': win['nextWindow'], 'function': win['function']} buttons['Cancel'] = BrrButton( '%s_Back' % name, [200, 60], conf['Cancel']['offset'], 0, None, "Wide", [0, 0], "Cancel", 16, True, self.share_path ) self._btn_context['%s_Back' % name] = { 'nextWindow': win['parent'], 'function': 'Back'} window = { 'name': '%s_conf' % win['name'], 'bg': conf['bg'], 'back': False, 'offset': conf['offset'], 'parent': win['parent'], 'default': '%s_OK' % name, 'no_scroll': False, 'text': labels } self.windows['%s_conf' % name] = BrrWindow(window, buttons, self.share_path) def selected(self): return self.active_window.selected_btn() '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Main Draw function. # Converts the appropriate frame to a ros message and sends # it to the screen. # Also sets the current_frame parameter, in expectation of # future hooks to merge images into the current view ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def draw(self): img = Image.new('RGB', (1024, 600), 'white') img = gen_cv(self._draw_window(img, self.active_window.name)) self.img = img msg = cv_to_msg(img) self.xdisp.publish(msg) rospy.sleep(.1) '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Simple method that sets the active window based on the window's name # and re-draws the UI. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def set_active_window(self, name): self.active_window = self.windows[name] self.draw() def _draw_window(self, img, window, selected=True): if self.windows[window].parent: img = self._draw_window(img, window=self.windows[window].parent, selected=False) return self.windows[window].draw(img, selected) '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Functions linking wheel turns with scrolling in the UI ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def _left_wheel_moved(self, v): self._wheel_moved(v, 'left') def _right_wheel_moved(self, v): self._wheel_moved(v, 'right') def _wheel_moved(self, v, side): if not self._active_example and self._wheel_ok: if v > 0: self.scroll(1) else: self.scroll(-1) self._wheel_ok = False rospy.Timer(rospy.Duration(.01), self._set_wheel_ok, oneshot=True) def scroll(self, direction): self.active_window.scroll(direction) self.draw() def _set_wheel_ok(self, event): self._wheel_ok = True '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Functions linking pressing the OK button on either arm with # the currently selected example ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def _left_ok_pressed(self, v): self.ok_pressed(v, 'left') def _right_ok_pressed(self, v): self.ok_pressed(v, 'right') def ok_pressed(self, v, side): if v == True: context = self._btn_context[self.selected().name] func = self._btn_context[self.selected().name]['function'] if func == 'Back': self.error_state = False self.kill_examples() mk_process('rm -rf /var/tmp/hlr/demo_calib.txt') self.set_active_window(context['nextWindow']) self.draw() if func and func != 'Back': getattr(self._functions, func)(self, side) def back(self, v): if v == True: self.error_state = False if self.active_window.parent: self.kill_examples() self.set_active_window(self.active_window.parent) self.draw() '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Commands to enable the robot (if it is disabled when the demo # starts) and to kill all currently running examples. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def kill_examples(self, v=1): rospy.loginfo('--@kill_examples') self._active_example = False self.selected()._status = 'selected' for cmd in self._commands: kill_python_procs(cmd) if self.cam_sub != None: self.cam_sub.unregister() self.draw() if not self.error_state: self._enable() def _check_enable(self, event): self._enable() def _enable(self, v=1): if v == 1 and not self._status.state().enabled: try: self._status.enable() except: self.error_screen('stopped') return False if not self._status.state().enabled: self.error_screen('no_enable') return False self._enable_cuff() return True def error_screen(self, error): if self.error_state == False: self.error_state = error self.kill_examples() error_screen = '%s_error' % error if self.active_window.name.startswith('run'): new_parent = self.active_window.parent elif not self.active_window.name.endswith('_error'): new_parent = self.active_window.name self._btn_context['%s_OK' % error]['nextWindow'] = new_parent self.windows[error_screen].parent = new_parent self.set_active_window(error_screen) self.draw() def _enable_cuff(self): if len(python_proc_ids('gripper_cuff_control')) == 0: RosProcess('rosrun baxter_examples gripper_cuff_control.py')
class BrrUi(object): def __init__(self, share_path, conf_path, commands): self.share_path = share_path self.conf_path = conf_path self.windows = dict() self._btn_context = dict() self._functions = demo_functions self._load_config() self.img = Image.new('RGB', (1024, 600), 'white') self.active_window = self.windows['demo_1'] self.xdisp = rospy.Publisher('/robot/xdisplay', ImageMsg, latch=True, queue_size=1) self._status = RobotEnable() self._commands = commands self._font = ImageFont.truetype('%s/HelveticaLight.ttf' % share_path, 30) self._textHeight = self._font.getsize('W')[1] self._active_example = False self._navigators = { 'left': Navigator('left'), 'torso_left': Navigator('torso_left'), 'right': Navigator('right'), 'torso_right': Navigator('torso_right') } self._listeners_connected = False self._connect_listeners() self._estop_state = False self._estop_sub = rospy.Subscriber('robot/state', AssemblyState, self._estop_callback) self._wheel_ok = True self.cameras = dict() camera_list = ['left_hand', 'right_hand', 'head'] for idx, cam in enumerate(camera_list): try: self.cameras[cam] = CameraController('%s_camera' % cam) except AttributeError: try: # This camera might not be powered # Turn off the power to the last camera # this will turn power on to the current camera CameraController('%s_camera' % camera_list[idx - 1]).close() # And try again to locate the camera service self.cameras[cam] = CameraController('%s_camera' % cam) except AttributeError: # This camera is unavailable (might be broken) # Disable camera button in the UI self.windows['cam_submenu'].set_btn_selectable( 'cam_%s' % cam, False) sel = self.windows['cam_submenu'].selected_btn() bad_cam = self.windows['cam_submenu'].get_btn('cam_%s' % cam) if (sel == bad_cam and not self.windows['cam_submenu'].scroll(1)): self.windows['cam_submenu'].selected_btn_index = 0 self.cam_sub = None self._l_grip = {'interface': Gripper('left'), 'type': 'custom'} self._r_grip = {'interface': Gripper('right'), 'type': 'custom'} rospy.Timer(rospy.Duration(.5), self._check_enable) self.error_state = False self._enable() self.calib_stage = 0 self.draw() mk_process('rosrun baxter_tools tuck_arms.py -u') def _estop_callback(self, msg): if self._estop_state != msg.stopped: self._estop_state = msg.stopped if msg.stopped and self._listeners_connected: self._disconnect_listeners() elif not msg.stopped and not self._listeners_connected: self._connect_listeners() def _connect_listeners(self): # Navigator OK Button self._navigators['left'].button0_changed.connect(self._left_ok_pressed) self._navigators['torso_left'].button0_changed.connect( self._left_ok_pressed) self._navigators['right'].button0_changed.connect( self._right_ok_pressed) self._navigators['torso_right'].button0_changed.connect( self._right_ok_pressed) # Navigator Wheel self._navigators['left'].wheel_changed.connect(self._left_wheel_moved) self._navigators['torso_left'].wheel_changed.connect( self._left_wheel_moved) self._navigators['right'].wheel_changed.connect( self._right_wheel_moved) self._navigators['torso_right'].wheel_changed.connect( self._right_wheel_moved) # Navigator Baxter Button self._navigators['left'].button2_changed.connect(self._enable) self._navigators['torso_left'].button2_changed.connect(self._enable) self._navigators['right'].button2_changed.connect(self._enable) self._navigators['torso_right'].button2_changed.connect(self._enable) # Navigator Back Button self._navigators['left'].button1_changed.connect(self.back) self._navigators['torso_left'].button1_changed.connect(self.back) self._navigators['right'].button1_changed.connect(self.back) self._navigators['torso_right'].button1_changed.connect(self.back) self._listeners_connected = True def _disconnect_listeners(self): # Navigator OK Button self._navigators['left'].button0_changed.disconnect( self._left_ok_pressed) self._navigators['torso_left'].button0_changed.disconnect( self._left_ok_pressed) self._navigators['right'].button0_changed.disconnect( self._right_ok_pressed) self._navigators['torso_right'].button0_changed.disconnect( self._right_ok_pressed) # Navigator Wheel self._navigators['left'].wheel_changed.disconnect( self._left_wheel_moved) self._navigators['torso_left'].wheel_changed.disconnect( self._left_wheel_moved) self._navigators['right'].wheel_changed.disconnect( self._right_wheel_moved) self._navigators['torso_right'].wheel_changed.disconnect( self._right_wheel_moved) # Navigator Baxter Button self._navigators['left'].button2_changed.disconnect(self._enable) self._navigators['torso_left'].button2_changed.disconnect(self._enable) self._navigators['right'].button2_changed.disconnect(self._enable) self._navigators['torso_right'].button2_changed.disconnect( self._enable) # Navigator Back Button self._navigators['left'].button1_changed.disconnect(self.back) self._navigators['torso_left'].button1_changed.disconnect(self.back) self._navigators['right'].button1_changed.disconnect(self.back) self._navigators['torso_right'].button1_changed.disconnect(self.back) self._listeners_connected = False def _load_config(self): f = open(self.conf_path).read() conf_data = json.loads(f) for window in conf_data['Windows']: buttons = dict() if window['back']: name = '%s_back' % window['name'] size = window['back']['size'] offset = window['back']['offset'] icon_prefix = 'Inner_Back' icon_offset = window['back']['icon_offset'] buttons[name] = BrrButton(name, size, offset, 0, icon_prefix, 'TopSmall', icon_offset, '', 0, True, self.share_path) self._btn_context[name] = { 'nextWindow': window['parent'], 'function': 'Back' } if 'Buttons' in window.keys(): for btn in window['Buttons']: buttons[btn['name']] = BrrButton( btn['name'], btn['size'], btn['offset'], btn['index'], btn['icon_prefix'], btn['button'], btn['icon_offset'], btn['label'], btn['label_y'], btn['selectable'], self.share_path) self._btn_context[btn['name']] = { 'nextWindow': btn['nextWindow'], 'function': btn['function'] } self.windows[window['name']] = BrrWindow(window, buttons, self.share_path) errors = conf_data['Error'] for error in errors['errors']: name = error['name'] buttons = dict() buttons['OK'] = BrrButton( '%s_OK' % name, # name [200, 60], # size errors['OK']['offset'], # button offset 0, # index None, # icon prefix "Wide", # button type [0, 0], # icon offset "OK", # label 16, # label y-offset True, # selectable? self.share_path) self._btn_context["%s_OK" % name] = { 'nextWindow': None, 'function': 'Back' } window = { 'name': '%s_error' % name, 'bg': errors['bg'], 'back': False, 'offset': errors['offset'], 'parent': False, 'default': '%s_OK' % name, 'no_scroll': False, 'text': [{ 'text': error['text'], 'text_y': error['text_y'] }], } self.windows['%s_error' % name] = BrrWindow( window, buttons, self.share_path) for win in conf_data['Confirmation']['Windows']: conf = conf_data['Confirmation'] name = win['name'] labels = list() for text in win['messages']: labels.append({'text': text['text'], 'text_y': text['text_y']}) buttons = dict() buttons['OK'] = BrrButton( '%s_OK' % name, # name [200, 60], # size conf['OK']['offset'], # button offset 1, # index None, # icon prefix "Wide", # button type [0, 0], # icon offset win['conf_text'], # label 16, # label y-offset True, # selectable? self.share_path) self._btn_context['%s_OK' % name] = { 'nextWindow': win['nextWindow'], 'function': win['function'] } buttons['Cancel'] = BrrButton('%s_Back' % name, [200, 60], conf['Cancel']['offset'], 0, None, "Wide", [0, 0], "Cancel", 16, True, self.share_path) self._btn_context['%s_Back' % name] = { 'nextWindow': win['parent'], 'function': 'Back' } window = { 'name': '%s_conf' % win['name'], 'bg': conf['bg'], 'back': False, 'offset': conf['offset'], 'parent': win['parent'], 'default': '%s_OK' % name, 'no_scroll': False, 'text': labels } self.windows['%s_conf' % name] = BrrWindow(window, buttons, self.share_path) def selected(self): return self.active_window.selected_btn() '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Main Draw function. # Converts the appropriate frame to a ros message and sends # it to the screen. # Also sets the current_frame parameter, in expectation of # future hooks to merge images into the current view ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def draw(self): img = Image.new('RGB', (1024, 600), 'white') img = gen_cv(self._draw_window(img, self.active_window.name)) self.img = img msg = cv_to_msg(img) self.xdisp.publish(msg) rospy.sleep(.1) '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Simple method that sets the active window based on the window's name # and re-draws the UI. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def set_active_window(self, name): self.active_window = self.windows[name] self.draw() def _draw_window(self, img, window, selected=True): if self.windows[window].parent: img = self._draw_window(img, window=self.windows[window].parent, selected=False) return self.windows[window].draw(img, selected) '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Functions linking wheel turns with scrolling in the UI ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def _left_wheel_moved(self, v): self._wheel_moved(v, 'left') def _right_wheel_moved(self, v): self._wheel_moved(v, 'right') def _wheel_moved(self, v, side): if not self._active_example and self._wheel_ok: if v > 0: self.scroll(1) else: self.scroll(-1) self._wheel_ok = False rospy.Timer(rospy.Duration(.01), self._set_wheel_ok, oneshot=True) def scroll(self, direction): self.active_window.scroll(direction) self.draw() def _set_wheel_ok(self, event): self._wheel_ok = True '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Functions linking pressing the OK button on either arm with # the currently selected example ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def _left_ok_pressed(self, v): self.ok_pressed(v, 'left') def _right_ok_pressed(self, v): self.ok_pressed(v, 'right') def ok_pressed(self, v, side): if v == True: context = self._btn_context[self.selected().name] func = self._btn_context[self.selected().name]['function'] if func == 'Back': self.error_state = False self.kill_examples() mk_process('rm -rf /var/tmp/hlr/demo_calib.txt') self.set_active_window(context['nextWindow']) self.draw() if func and func != 'Back': getattr(self._functions, func)(self, side) def back(self, v): if v == True: self.error_state = False if self.active_window.parent: self.kill_examples() self.set_active_window(self.active_window.parent) self.draw() '''~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ # Commands to enable the robot (if it is disabled when the demo # starts) and to kill all currently running examples. ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~''' def kill_examples(self, v=1): rospy.loginfo('--@kill_examples') self._active_example = False self.selected()._status = 'selected' for cmd in self._commands: kill_python_procs(cmd) if self.cam_sub != None: self.cam_sub.unregister() self.draw() if not self.error_state: self._enable() def _check_enable(self, event): self._enable() def _enable(self, v=1): if v == 1 and not self._status.state().enabled: try: self._status.enable() except: self.error_screen('stopped') return False if not self._status.state().enabled: self.error_screen('no_enable') return False self._enable_cuff() return True def error_screen(self, error): if self.error_state == False: self.error_state = error self.kill_examples() error_screen = '%s_error' % error if self.active_window.name.startswith('run'): new_parent = self.active_window.parent elif not self.active_window.name.endswith('_error'): new_parent = self.active_window.name self._btn_context['%s_OK' % error]['nextWindow'] = new_parent self.windows[error_screen].parent = new_parent self.set_active_window(error_screen) self.draw() def _enable_cuff(self): if len(python_proc_ids('gripper_cuff_control')) == 0: RosProcess('rosrun baxter_examples gripper_cuff_control.py')
class Baxter(object): def __init__(self, calibrate_grippers=True): self._baxter_state = RobotEnable() self._left = Limb(LEFT) self._right = Limb(RIGHT) self._limbs = {LEFT: self._left, RIGHT: self._right} self._head = Head() self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT) if calibrate_grippers: self.calibrate() self._left_ikservice = IKService(LEFT) self._right_ikservice = IKService(RIGHT) def calibrate(self): self._left_gripper.calibrate() self._left_gripper_max = self._left_gripper.position() self._right_gripper.calibrate() self._right_gripper_max = self._right_gripper.position() @property def left_gripper_max(self): return self._left_gripper_max @property def right_gripper_max(self): return self._right_gripper_max @property def left_gripper(self): return self._left_gripper.position() @left_gripper.setter def left_gripper(self, position): self._left_gripper.command_position(position) @property def right_gripper(self): return self._right_gripper.position() @right_gripper.setter def right_gripper(self, position): self._right_gripper.command_position(position) def set_left_joints(self, angles): joints = self._left.joint_angles() for joint, angle in angles.iteritems(): if angle: joints[joint] = angle self.enable_check() self._left.set_joint_positions(joints) def set_right_joints(self, angles): joints = self._right.joint_angles() for joint, angle in angles.iteritems(): if angle: joints[joint] = angle self.enable_check() self._right.set_joint_positions(joints) def reset_limb(self, side): angles = {joint: 0.0 for joint in self._limbs[side].joint_angles()} self.enable_check() self._limbs[side].move_to_joint_positions(angles) def enable_check(self): # Sometimes robot is disabled due to another program resetting state if not self._baxter_state.state().enabled: self._baxter_state.enable() @property def joints(self): joints = { limb: joint.joint_angles() for limb, joint in self._limbs.iteritems() } return joints @property def enabled(self): return self._baxter_state.state().enabled @property def left_s0(self): return self._left.joint_angle('left_s0') @left_s0.setter def left_s0(self, angle): self.set_left_joints({'left_s0': angle}) @property def left_s1(self): return self._left.joint_angle('left_s1') @left_s1.setter def left_s1(self, angle): self.set_left_joints({'left_s1': angle}) @property def left_e0(self): return self._left.joint_angle('left_e0') @left_e0.setter def left_e0(self, angle): self.set_left_joints({'left_e0': angle}) @property def left_e1(self): return self._left.joint_angle('left_e1') @left_e1.setter def left_e1(self, angle): self.set_left_joints({'left_e1': angle}) @property def left_w0(self): return self._left.joint_angle('left_w0') @left_w0.setter def left_w0(self, angle): self.set_left_joints({'left_w0': angle}) @property def left_w1(self): return self._left.joint_angle('left_w1') @left_w1.setter def left_w1(self, angle): self.set_left_joints({'left_w1': angle}) @property def left_w2(self): return self._left.joint_angle('left_w2') @left_w2.setter def left_w2(self, angle): self.set_left_joints({'left_w2': angle}) @property def right_s0(self): return self._right.joint_angle('right_s0') @right_s0.setter def right_s0(self, angle): self.set_right_joints({'right_s0': angle}) @property def right_s1(self): return self._right.joint_angle('right_s1') @right_s1.setter def right_s1(self, angle): self.set_right_joints({'right_s1': angle}) @property def right_e0(self): return self._right.joint_angle('right_e0') @right_e0.setter def right_e0(self, angle): self.set_right_joints({'right_e0': angle}) @property def right_e1(self): return self._right.joint_angle('right_e1') @right_e1.setter def right_e1(self, angle): self.set_right_joints({'right_e1': angle}) @property def right_w0(self): return self._right.joint_angle('right_w0') @right_w0.setter def right_w0(self, angle): self.set_right_joints({'right_w0': angle}) @property def right_w1(self): return self._right.joint_angle('right_w1') @right_w1.setter def right_w1(self, angle): self.set_right_joints({'right_w1': angle}) @property def right_w2(self): return self._right.joint_angle('right_w2') @right_w2.setter def right_w2(self, angle): self.set_right_joints({'right_w2': angle}) @property def left_position(self): return self._left.endpoint_pose()['position'] @property def left_position_x(self): return self.left_position.x @left_position_x.setter def left_position_x(self, point): self.set_left_pose(position={'x': point}) @property def left_position_y(self): return self.left_position.y @left_position_y.setter def left_position_y(self, point): self.set_left_pose(position={'y': point}) @property def left_position_z(self): return self.left_position.z @left_position_z.setter def left_position_z(self, point): self.set_left_pose(position={'z': point}) @property def left_orientation(self): return self._left.endpoint_pose()['orientation'] @property def left_orientation_x(self): return self.left_orientation.x @left_orientation_x.setter def left_orientation_x(self, point): self.set_left_pose(orientation={'x': point}) @property def left_orientation_y(self): return self.left_orientation.y @left_orientation_y.setter def left_orientation_y(self, point): self.set_left_pose(orientation={'y': point}) @property def left_orientation_z(self): return self.left_orientation.z @left_orientation_z.setter def left_orientation_z(self, point): self.set_left_pose(orientation={'z': point}) @property def left_orientation_w(self): return self.left_orientation.w @left_orientation_w.setter def left_orientation_w(self, point): self.set_left_pose(orientation={'w': point}) @property def right_position(self): return self._right.endpoint_pose()['position'] @property def right_orientation(self): return self._right.endpoint_pose()['orientation'] def set_left_pose(self, position={}, orientation={}): pos = { 'x': self.left_position_x, 'y': self.left_position_y, 'z': self.left_position_z, } for key, value in position.iteritems(): pos[key] = value orient = { 'x': self.left_orientation_x, 'y': self.left_orientation_y, 'z': self.left_orientation_z, 'w': self.left_orientation_w, } for key, value in orientation.iteritems(): orient[key] = value pos = self._left_ikservice.solve_position( Pose(position=Point(**pos), orientation=Quaternion(**orient))) if pos: self.set_left_joints(pos) else: print 'nothing' #print self.joints @property def right_position(self): return self._right.endpoint_pose()['position'] @property def right_position_x(self): return self.right_position.x @right_position_x.setter def right_position_x(self, point): self.set_right_pose(position={'x': point}) @property def right_position_y(self): return self.right_position.y @right_position_y.setter def right_position_y(self, point): self.set_right_pose(position={'y': point}) @property def right_position_z(self): return self.right_position.z @right_position_z.setter def right_position_z(self, point): self.set_right_pose(position={'z': point}) @property def right_orientation(self): return self._right.endpoint_pose()['orientation'] @property def right_orientation_x(self): return self.right_orientation.x @right_orientation_x.setter def right_orientation_x(self, point): self.set_right_pose(orientation={'x': point}) @property def right_orientation_y(self): return self.right_orientation.y @right_orientation_y.setter def right_orientation_y(self, point): self.set_right_pose(orientation={'y': point}) @property def right_orientation_z(self): return self.right_orientation.z @right_orientation_z.setter def right_orientation_z(self, point): self.set_right_pose(orientation={'z': point}) @property def right_orientation_w(self): return self.right_orientation.w @right_orientation_w.setter def right_orientation_w(self, point): self.set_right_pose(orientation={'w': point}) @property def right_position(self): return self._right.endpoint_pose()['position'] @property def right_orientation(self): return self._right.endpoint_pose()['orientation'] def set_right_pose(self, position={}, orientation={}): pos = { 'x': self.right_position_x, 'y': self.right_position_y, 'z': self.right_position_z, } for key, value in position.iteritems(): pos[key] = value orient = { 'x': self.right_orientation_x, 'y': self.right_orientation_y, 'z': self.right_orientation_z, 'w': self.right_orientation_w, } for key, value in orientation.iteritems(): orient[key] = value pos = self._right_ikservice.solve_position( Pose(position=Point(**pos), orientation=Quaternion(**orient))) if pos: self.set_right_joints(pos) @property def head_position(self): return self._head.pan() @head_position.setter def head_position(self, position): self._head.set_pan(position)
class Baxter: def __init__(self, usegripper = True): rospy.init_node("Baxter") threading.Thread(target=self.__jointtraj_server).start() self.trajrgt = Trajectory("right") self.trajlft = Trajectory("left") if usegripper: self.__right_hand = Gripper("right") self.__left_hand = Gripper("left") self.__right_hand.calibrate() self.__left_hand.calibrate() self.__right_limb = Limb("right") self.__left_limb = Limb("left") self.baxter = RobotEnable() self.__enabled = self.baxter.state().enabled self.enable_baxter() self.img = None for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']: try: cam = CameraController(camname) # cam.resolution = (1280, 800) cam.resolution = (320, 200) except: pass print("baxter opened") @property def enabled(self): return self.__enabled def __jointtraj_server(self): limb = "both" rate = 100.0 mode = "position" if mode == 'velocity': dyn_cfg_srv = Server(VelocityJointTrajectoryActionServerConfig, lambda config, level: config) elif mode == 'position': dyn_cfg_srv = Server(PositionJointTrajectoryActionServerConfig, lambda config, level: config) else: dyn_cfg_srv = Server(PositionFFJointTrajectoryActionServerConfig, lambda config, level: config) jtas = [] if limb == 'both': jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv, rate, mode)) jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv, rate, mode)) else: jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode)) def cleanup(): for j in jtas: j.clean_shutdown() rospy.on_shutdown(cleanup) print("Running. Ctrl-c to quit") rospy.spin() def enable_baxter(self): print("Opening the baxter") if not self.__enabled: self.baxter.enable() self.__enabled = self.baxter.state().enabled def disable_baxter(self): if self.__enabled: self.baxter.disable() self.__enabled = self.baxter.state().enabled def opengripper(self, armname="rgt"): if not self.__enabled: return arm = self.__right_hand if armname == "rgt" else self.__left_hand if self._is_grippable(arm): arm.open() else: rospy.logwarn(armname+ " have not been calibrated") def closegripper(self, armname="rgt"): if not self.__enabled: return arm = self.__right_hand if armname == "rgt" else self.__left_hand if self._is_grippable(arm): arm.close() else: rospy.logwarn(armname + " have not been calibrated") def commandgripper(self,pos , armname="rgt"): if not self.__enabled: return arm = self.__right_hand if armname == "rgt" else self.__left_hand if self._is_grippable(arm): arm.command_position(position = pos) else: rospy.logwarn(armname + " have not been calibrated") def _is_grippable(self,gripper): return (gripper.calibrated() and gripper.ready()) def currentposgripper(self,armname ="rgt"): arm = self.__right_hand if armname == "rgt" else self.__left_hand return arm.position() def getjnts(self,armname="rgt"): if not self.__enabled: return limb = self.__right_limb if armname == "rgt" else self.__left_limb angles = limb.joint_angles() return angles def movejnts(self,jnts_dict, speed =.6 , armname="rgt"): if not self.__enabled: return limb = self.__right_limb if armname == "rgt" else self.__left_limb traj = self.trajrgt if armname is "rgt" else self.trajlft # limb.set_joint_position_speed(speed) currentjnts = [limb.joint_angle(joint) for joint in limb.joint_names()] traj.add_point(currentjnts, 0) if isinstance(jnts_dict, dict): path_angle = [jnts_dict[joint_name] for joint_name in limb.joint_names()] else: path_angle = jnts_dict # print(path_angle) traj.add_point(path_angle, speed) # limb.move_to_joint_positions(jnts_dict,threshold= baxter_interface.settings.JOINT_ANGLE_TOLERANCE) limb.set_joint_position_speed(.3) def movejnts_cont(self, jnts_dict_list, speed=.1, armname="rgt"): traj = self.trajrgt if armname is "rgt" else self.trajlft if not self.__enabled: return limb = self.__right_limb if armname == "rgt" else self.__left_limb # limb.set_joint_position_speed(.6) currentjnts = [limb.joint_angle(joint) for joint in limb.joint_names()] traj.add_point(currentjnts, 0) rospy.on_shutdown(traj.stop) t = speed for jnts_dict in jnts_dict_list: # limb.move_to_joint_positions(jnts_dict, threshold= baxter_interface.settings.JOINT_ANGLE_TOLERANCE) if isinstance(jnts_dict,dict): path_angle = [jnts_dict[joint_name] for joint_name in limb.joint_names()] else: path_angle = jnts_dict # print(path_angle) traj.add_point(path_angle,t) t += speed traj.start() traj.wait(-1) limb.set_joint_position_speed(.3) traj.clear() def getforce(self,armname="rgt"): if not self.__enabled: return limb = self.__right_limb if armname == "rgt" else self.__left_limb ft = limb.endpoint_effort() force = ft['force'] torque = ft['torque'] print ft return list(force+torque) def setscreenimage(self,path): # it seems not working """ Send the image located at the specified path to the head display on Baxter. @param path: path to the image file to load and send """ # rospy.init_node('rsdk_xdisplay_image', anonymous=True) print("Changing the photo") img = cv.imread(path) msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8") pub = rospy.Publisher('/robot_s/xdisplay', Image, latch=True, queue_size=1) pub.publish(msg) # Sleep to allow for image to be published. rospy.sleep(1) def getimage(self,cameraname="left_hand_camera"): # ['head_camera', 'left_hand_camera', 'right_hand_camera'] # if cameraname != self.camera_on: # cam = CameraController(self.camera_on) # cam.close() # cam = CameraController(cameraname) # cam.resolution = (1280, 800) # cam.open() # self.camera_on = cameraname print("Getting imag...") "rosrun image_view image_view image:=/cameras/left_hand_camera/image" "rosrun image_view image_view image:=/cameras/head_camera/image" # Instantiate CvBridge bridge = cv_bridge.CvBridge() def image_callback(msg): print("Received an image!") try: # Convert your ROS Image message to OpenCV2 cv_img = bridge.imgmsg_to_cv2(msg, "bgr8") except cv_bridge.CvBridgeError, e: print(e) else: