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:
